add bio follower function (finish)
This commit is contained in:
Vendored
+1
-1
@@ -9,7 +9,7 @@
|
||||
"name": "ROS: Launch",
|
||||
"type": "ros",
|
||||
"request": "launch",
|
||||
"target": "/home/dodo/ardupilot_ws/src/class_model/launch/test.launch",
|
||||
"target": "/home/dodo/ardupilot_ws/src/class_model/launch/testLib.launch",
|
||||
"launch": [
|
||||
"rviz",
|
||||
"gz",
|
||||
|
||||
@@ -20,7 +20,7 @@ result check_leader(global_location data[], size_t index){
|
||||
// std::cout << i<<"," <<d[data[i].ID] <<std::endl;
|
||||
}
|
||||
|
||||
std::vector<float> _d(d+1,d+index-1);
|
||||
std::vector<float> _d(d+1,d+index);
|
||||
|
||||
check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
|
||||
_result.leader_ID = check_ID + 1;
|
||||
|
||||
+24
@@ -0,0 +1,24 @@
|
||||
{
|
||||
"configurations": [
|
||||
{
|
||||
"browse": {
|
||||
"databaseFilename": "${default}",
|
||||
"limitSymbolsToIncludedHeaders": false
|
||||
},
|
||||
"includePath": [
|
||||
"/home/dodo/ardupilot_ws/devel/include/**",
|
||||
"/opt/ros/noetic/include/**",
|
||||
"/home/dodo/ardupilot_ws/src/ROS_controll/include/**",
|
||||
"/home/dodo/ardupilot_ws/src/class_model/include/**",
|
||||
"/home/dodo/ardupilot_ws/src/iq_gnc/include/**",
|
||||
"/usr/include/**"
|
||||
],
|
||||
"name": "ROS",
|
||||
"intelliSenseMode": "gcc-x64",
|
||||
"compilerPath": "/usr/bin/gcc",
|
||||
"cStandard": "gnu11",
|
||||
"cppStandard": "c++14"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
+10
@@ -0,0 +1,10 @@
|
||||
{
|
||||
"python.autoComplete.extraPaths": [
|
||||
"/home/dodo/ardupilot_ws/devel/lib/python3/dist-packages",
|
||||
"/opt/ros/noetic/lib/python3/dist-packages"
|
||||
],
|
||||
"python.analysis.extraPaths": [
|
||||
"/home/dodo/ardupilot_ws/devel/lib/python3/dist-packages",
|
||||
"/opt/ros/noetic/lib/python3/dist-packages"
|
||||
]
|
||||
}
|
||||
@@ -22,21 +22,24 @@ void FollowerClass::plane(global_location target, global_location leader, global
|
||||
c = target.lat - slope*target.lon;
|
||||
y = slope * follower.lon + c;
|
||||
|
||||
if(follower.lat > y){
|
||||
if(follower.lat >= y){
|
||||
switch (follower.ID){
|
||||
case 1:
|
||||
drone1.init_location.lat = follower.lat;
|
||||
drone1.init_location.lon = follower.lon;
|
||||
drone1.ID = follower.ID;
|
||||
drone1.plane = -1; //right plane
|
||||
break;
|
||||
case 2:
|
||||
drone2.init_location.lat = follower.lat;
|
||||
drone2.init_location.lon = follower.lon;
|
||||
drone2.ID = follower.ID;
|
||||
drone2.plane = -1;
|
||||
break;
|
||||
case 3:
|
||||
drone3.init_location.lat = follower.lat;
|
||||
drone3.init_location.lon = follower.lon;
|
||||
drone3.ID = follower.ID;
|
||||
drone3.plane = -1;
|
||||
break;
|
||||
}
|
||||
@@ -51,11 +54,13 @@ void FollowerClass::plane(global_location target, global_location leader, global
|
||||
case 2:
|
||||
drone2.init_location.lat = follower.lat;
|
||||
drone2.init_location.lon = follower.lon;
|
||||
drone2.ID = follower.ID;
|
||||
drone2.plane = 1;
|
||||
break;
|
||||
case 3:
|
||||
drone3.init_location.lat = follower.lat;
|
||||
drone3.init_location.lon = follower.lon;
|
||||
drone3.ID = follower.ID;
|
||||
drone3.plane = 1;
|
||||
break;
|
||||
}
|
||||
@@ -68,10 +73,10 @@ int direct(global_location target, global_location leader, DroneStatus self, Dro
|
||||
|
||||
global_location vector_LT,vector_SM;
|
||||
float m,n,k,cosTheta;
|
||||
vector_LT.lon = leader.lon - target.lon; //vector_LT (leader->target)
|
||||
vector_LT.lat = leader.lat - target.lat;
|
||||
vector_SM.lon = leader.lon - target.lon; //vector_SM (self->member)
|
||||
vector_SM.lat = leader.lat - target.lat;
|
||||
vector_LT.lon = target.lon - leader.lon; //vector_LT (leader->target)
|
||||
vector_LT.lat = target.lat - leader.lat;
|
||||
vector_SM.lon = member.init_location.lon - self.init_location.lon; //vector_SM (self->member)
|
||||
vector_SM.lat = member.init_location.lat - self.init_location.lat;
|
||||
k = (vector_LT.lon*vector_SM.lon)+(vector_LT.lat*vector_SM.lat); //k = vector_LT dot vector_SM
|
||||
m = sqrt(pow(vector_LT.lon,2)+pow(vector_LT.lat,2)); //m = |vector_LT|
|
||||
n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2)); //n = |vector_SM|
|
||||
@@ -80,7 +85,7 @@ int direct(global_location target, global_location leader, DroneStatus self, Dro
|
||||
if(cosTheta > 0){
|
||||
return 1;
|
||||
}else{
|
||||
return INFINITY;
|
||||
return 10000;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -88,7 +93,7 @@ int direct(global_location target, global_location leader, DroneStatus self, Dro
|
||||
void FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){
|
||||
|
||||
for(int i=1 ; i<index ; i++){ //check leader drone
|
||||
if(leaderID = data[i].ID){
|
||||
if(leaderID == data[i].ID){
|
||||
leader_drone = data[i];
|
||||
switch(leaderID){
|
||||
case 1:
|
||||
@@ -111,6 +116,7 @@ void FollowerClass::find_ref_drone(global_location data[], size_t index, int lea
|
||||
break;
|
||||
|
||||
}
|
||||
// std::cout << data[i].ID << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -120,52 +126,59 @@ void FollowerClass::find_ref_drone(global_location data[], size_t index, int lea
|
||||
}
|
||||
}
|
||||
|
||||
int j=0,k=0;
|
||||
int j=0,s_index=0;
|
||||
if(drone1.ID == data[1].ID){ //seperate self and other
|
||||
self = drone1;
|
||||
self = drone1; //problem
|
||||
}else{
|
||||
memberDrone[j] = drone1;
|
||||
j+=1;
|
||||
std::cout << "drone1" <<std::endl;
|
||||
}
|
||||
if(drone2.ID == data[1].ID){
|
||||
self = drone2;
|
||||
}else{
|
||||
memberDrone[j] = drone2;
|
||||
j+=1;
|
||||
std::cout << "drone2" <<std::endl;
|
||||
}
|
||||
if(drone3.ID == data[1].ID){
|
||||
self = drone1;
|
||||
self = drone3;
|
||||
}else{
|
||||
memberDrone[j] = drone3;
|
||||
j+=1;
|
||||
std::cout << "drone3" <<std::endl;
|
||||
}
|
||||
|
||||
for(int i=0;i<index-1;i++){
|
||||
if(memberDrone[i].plane == self.plane){ //load same plane member
|
||||
samePlane[k] = memberDrone[i];
|
||||
k++;
|
||||
for(int i=0;i<index-2;i++){
|
||||
if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){ //load same plane member
|
||||
samePlane[s_index] = memberDrone[i];
|
||||
std::cout << samePlane[s_index].init_location.lat <<std::endl;
|
||||
std::cout << memberDrone[i].ID <<std::endl;
|
||||
s_index++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
|
||||
// int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
|
||||
float d[s_index];
|
||||
int dir,check_index,ref_ID;
|
||||
|
||||
std::cout <<"s_index " <<s_index <<std::endl;
|
||||
for(int i=0 ; i<s_index ; i++){ //caculate each same plane mamber's distance
|
||||
|
||||
dir = direct(data[0],leader_drone,self,samePlane[i]);
|
||||
d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lat - samePlane[i].init_location.lat),2) );
|
||||
d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
|
||||
|
||||
// std::cout << i<<"," <<d[data[i].ID] <<std::endl;
|
||||
std::cout << i<<"," <<d[i] <<std::endl;
|
||||
std::cout << samePlane[i].ID <<std::endl;
|
||||
}
|
||||
|
||||
std::vector<float> _d(d,d+s_index-1);
|
||||
std::vector<float> _d(d,d+s_index);
|
||||
|
||||
check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
|
||||
ref_ID = samePlane[check_index].ID;
|
||||
|
||||
// std::cout <<"Drone " << " is leader " << std::endl;
|
||||
std::cout <<"refID " << ref_ID << std::endl;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -7,19 +7,19 @@ int command;
|
||||
|
||||
RequestClass::RequestClass() : node_handle_(""){
|
||||
|
||||
// mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
|
||||
// &RequestClass::Data_callback, this);
|
||||
mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
|
||||
&RequestClass::Data_callback, this);
|
||||
formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
|
||||
&RequestClass::Message_callback, this);
|
||||
mqtt_data = node_handle_.subscribe("/uav_message", 100,
|
||||
&RequestClass::Data_callback, this);
|
||||
// mqtt_data = node_handle_.subscribe("/uav_message", 100,
|
||||
// &RequestClass::Data_callback, this);
|
||||
|
||||
self_data = node_handle_.subscribe("/self_data_message", 100,
|
||||
&RequestClass::self_callback, this); //test bionic
|
||||
member1_data = node_handle_.subscribe("/member1_data_message", 100,
|
||||
&RequestClass::M1_callback, this);
|
||||
member2_data = node_handle_.subscribe("/member2_data_message", 100,
|
||||
&RequestClass::M2_callback, this);
|
||||
// self_data = node_handle_.subscribe("/self_data_message", 100,
|
||||
// &RequestClass::self_callback, this); //test bionic
|
||||
// member1_data = node_handle_.subscribe("/member1_data_message", 100,
|
||||
// &RequestClass::M1_callback, this);
|
||||
// member2_data = node_handle_.subscribe("/member2_data_message", 100,
|
||||
// &RequestClass::M2_callback, this);
|
||||
}
|
||||
|
||||
RequestClass::~RequestClass() { ros::shutdown(); }
|
||||
@@ -92,50 +92,50 @@ void RequestClass::L_INFO(std::string data){
|
||||
}
|
||||
|
||||
/*test biomic*/
|
||||
void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
|
||||
Bio_INFO(sensor->data);
|
||||
}
|
||||
void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
|
||||
Bio_INFO(sensor->data);
|
||||
}
|
||||
void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
|
||||
Bio_INFO(sensor->data);
|
||||
}
|
||||
// void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
|
||||
// Bio_INFO(sensor->data);
|
||||
// }
|
||||
// void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
|
||||
// Bio_INFO(sensor->data);
|
||||
// }
|
||||
// void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
|
||||
// Bio_INFO(sensor->data);
|
||||
// }
|
||||
|
||||
void RequestClass::Bio_INFO(std::string data){
|
||||
// void RequestClass::Bio_INFO(std::string data){
|
||||
|
||||
j_data = json::parse(data); //open source
|
||||
// j_data = json::parse(data); //open source
|
||||
|
||||
drone_ID = j_data["ID"];
|
||||
std::remove(ID.begin(),ID.end(),self_ID);
|
||||
// drone_ID = j_data["ID"];
|
||||
// std::remove(ID.begin(),ID.end(),self_ID);
|
||||
|
||||
if (drone_ID == self_ID){
|
||||
self_position.lat=j_data["lat"];
|
||||
self_position.lon=j_data["lon"];
|
||||
self_position.alt=j_data["alt"];
|
||||
self_position.heading = j_data["heading"];
|
||||
}else if (drone_ID == ID[0]){
|
||||
M1_position.lat=j_data["lat"];
|
||||
M1_position.lon=j_data["lon"];
|
||||
M1_position.alt=j_data["alt"];
|
||||
M1_position.heading = j_data["heading"];
|
||||
}else if (drone_ID == ID[1]){
|
||||
M2_position.lat=j_data["lat"];
|
||||
M2_position.lon=j_data["lon"];
|
||||
M2_position.alt=j_data["alt"];
|
||||
M2_position.heading = j_data["heading"];
|
||||
}
|
||||
// if (drone_ID == self_ID){
|
||||
// self_position.lat=j_data["lat"];
|
||||
// self_position.lon=j_data["lon"];
|
||||
// self_position.alt=j_data["alt"];
|
||||
// self_position.heading = j_data["heading"];
|
||||
// }else if (drone_ID == ID[0]){
|
||||
// M1_position.lat=j_data["lat"];
|
||||
// M1_position.lon=j_data["lon"];
|
||||
// M1_position.alt=j_data["alt"];
|
||||
// M1_position.heading = j_data["heading"];
|
||||
// }else if (drone_ID == ID[1]){
|
||||
// M2_position.lat=j_data["lat"];
|
||||
// M2_position.lon=j_data["lon"];
|
||||
// M2_position.alt=j_data["alt"];
|
||||
// M2_position.heading = j_data["heading"];
|
||||
// }
|
||||
|
||||
}
|
||||
global_location RequestClass::get_self_GPS(){
|
||||
// }
|
||||
// global_location RequestClass::get_self_GPS(){
|
||||
|
||||
return self_position;
|
||||
}
|
||||
global_location RequestClass::get_M1_GPS(){
|
||||
// return self_position;
|
||||
// }
|
||||
// global_location RequestClass::get_M1_GPS(){
|
||||
|
||||
return M1_position;
|
||||
}
|
||||
global_location RequestClass::get_M2_GPS(){
|
||||
// return M1_position;
|
||||
// }
|
||||
// global_location RequestClass::get_M2_GPS(){
|
||||
|
||||
return M2_position;
|
||||
}
|
||||
// return M2_position;
|
||||
// }
|
||||
+201
-31
@@ -3,37 +3,221 @@
|
||||
#include "class_model/gps_location.h"
|
||||
#include <algorithm>
|
||||
#include <math.h>
|
||||
#include "class_model/DroneStatus.h"
|
||||
|
||||
using namespace std;
|
||||
global_location leader_drone;
|
||||
DroneStatus memberDrone[3];
|
||||
DroneStatus drone1;
|
||||
DroneStatus drone2;
|
||||
DroneStatus drone3;
|
||||
DroneStatus self;
|
||||
DroneStatus samePlane[3];
|
||||
global_location follower_location;
|
||||
|
||||
bool is_leader(global_location data[], size_t index){
|
||||
typedef struct result{
|
||||
int leader_ID;
|
||||
bool is_leader;
|
||||
}result;
|
||||
|
||||
result check_leader(global_location data[], size_t index){
|
||||
|
||||
// int index = sizeof(data)/sizeof(data[0]);
|
||||
float d[index];
|
||||
int check_ID,leader_ID;
|
||||
int check_ID;
|
||||
result _result;
|
||||
|
||||
for(int i=1 ; i<index ; i++){
|
||||
|
||||
d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
|
||||
|
||||
// std::cout << i<<"," <<d[data[i].ID] <<std::endl;
|
||||
std::cout << i<<"," <<d[data[i].ID] <<std::endl;
|
||||
}
|
||||
|
||||
std::vector<float> _d(d+1,d+index-1);
|
||||
std::vector<float> _d(d+1,d+index);
|
||||
|
||||
check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
|
||||
leader_ID = check_ID + 1;
|
||||
_result.leader_ID = check_ID + 1;
|
||||
|
||||
std::cout <<"Drone " << leader_ID << " is leader " << std::endl;
|
||||
// std::cout << _d[0] << " ,"<< _d[1] << " ,"<< _d[2] << std::endl;
|
||||
std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
|
||||
|
||||
if (data[1].ID == leader_ID){
|
||||
return 1;
|
||||
if (data[1].ID == _result.leader_ID){
|
||||
_result.is_leader = 1;
|
||||
return _result;
|
||||
}else{
|
||||
return 0;
|
||||
_result.is_leader = 0;
|
||||
return _result;
|
||||
}
|
||||
|
||||
}
|
||||
void plane(global_location target, global_location leader, global_location follower){
|
||||
|
||||
//caculate drone is locate on R/L plane
|
||||
float slope,c,y;
|
||||
slope = (target.lat-leader.lat)/(target.lon-leader.lon);
|
||||
c = target.lat - slope*target.lon;
|
||||
y = slope * follower.lon + c;
|
||||
|
||||
if(follower.lat >= y){
|
||||
switch (follower.ID){
|
||||
case 1:
|
||||
drone1.init_location.lat = follower.lat;
|
||||
drone1.init_location.lon = follower.lon;
|
||||
drone1.ID = follower.ID;
|
||||
drone1.plane = -1; //right plane
|
||||
break;
|
||||
case 2:
|
||||
drone2.init_location.lat = follower.lat;
|
||||
drone2.init_location.lon = follower.lon;
|
||||
drone2.ID = follower.ID;
|
||||
drone2.plane = -1;
|
||||
break;
|
||||
case 3:
|
||||
drone3.init_location.lat = follower.lat;
|
||||
drone3.init_location.lon = follower.lon;
|
||||
drone3.ID = follower.ID;
|
||||
drone3.plane = -1;
|
||||
break;
|
||||
}
|
||||
}else if(follower_location.lat < y){
|
||||
switch (follower.ID){
|
||||
case 1:
|
||||
drone1.init_location.lat = follower.lat;
|
||||
drone1.init_location.lon = follower.lon;
|
||||
drone1.ID = follower.ID;
|
||||
drone1.plane = 1; //left plane
|
||||
break;
|
||||
case 2:
|
||||
drone2.init_location.lat = follower.lat;
|
||||
drone2.init_location.lon = follower.lon;
|
||||
drone2.ID = follower.ID;
|
||||
drone2.plane = 1;
|
||||
break;
|
||||
case 3:
|
||||
drone3.init_location.lat = follower.lat;
|
||||
drone3.init_location.lon = follower.lon;
|
||||
drone3.ID = follower.ID;
|
||||
drone3.plane = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){
|
||||
|
||||
global_location vector_LT,vector_SM;
|
||||
float m,n,k,cosTheta;
|
||||
vector_LT.lon = target.lon - leader.lon; //vector_LT (leader->target)
|
||||
vector_LT.lat = target.lat - leader.lat;
|
||||
vector_SM.lon = member.init_location.lon - self.init_location.lon; //vector_SM (self->member)
|
||||
vector_SM.lat = member.init_location.lat - self.init_location.lat;
|
||||
k = (vector_LT.lon*vector_SM.lon)+(vector_LT.lat*vector_SM.lat); //k = vector_LT dot vector_SM
|
||||
m = sqrt(pow(vector_LT.lon,2)+pow(vector_LT.lat,2)); //m = |vector_LT|
|
||||
n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2)); //n = |vector_SM|
|
||||
cosTheta = k / (m*n);
|
||||
|
||||
if(cosTheta > 0){
|
||||
return 1;
|
||||
}else{
|
||||
return 10000;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void find_ref_drone(global_location data[], size_t index, int leaderID){
|
||||
|
||||
for(int i=1 ; i<index ; i++){ //check leader drone
|
||||
if(leaderID == data[i].ID){
|
||||
leader_drone = data[i];
|
||||
switch(leaderID){
|
||||
case 1:
|
||||
drone1.init_location.lat = data[i].lat;
|
||||
drone1.init_location.lon = data[i].lon;
|
||||
drone1.ID = data[i].ID;
|
||||
drone1.plane = 0; //center
|
||||
break;
|
||||
case 2:
|
||||
drone2.init_location.lat = data[i].lat;
|
||||
drone2.init_location.lon = data[i].lon;
|
||||
drone2.ID = data[i].ID;
|
||||
drone2.plane = 0;
|
||||
break;
|
||||
case 3:
|
||||
drone3.init_location.lat = data[i].lat;
|
||||
drone3.init_location.lon = data[i].lon;
|
||||
drone3.ID = data[i].ID;
|
||||
drone3.plane = 0;
|
||||
break;
|
||||
|
||||
}
|
||||
// std::cout << data[i].ID << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
for(int i=1 ; i<index ; i++){
|
||||
if(data[i].ID != leader_drone.ID){ //caculate the plane of each drone
|
||||
plane(data[0],leader_drone,data[i]);
|
||||
}
|
||||
}
|
||||
|
||||
int j=0,s_index=0;
|
||||
if(drone1.ID == data[1].ID){ //seperate self and other
|
||||
self = drone1; //problem
|
||||
}else{
|
||||
memberDrone[j] = drone1;
|
||||
j+=1;
|
||||
std::cout << "drone1" <<std::endl;
|
||||
}
|
||||
if(drone2.ID == data[1].ID){
|
||||
self = drone2;
|
||||
}else{
|
||||
memberDrone[j] = drone2;
|
||||
j+=1;
|
||||
std::cout << "drone2" <<std::endl;
|
||||
}
|
||||
if(drone3.ID == data[1].ID){
|
||||
self = drone3;
|
||||
}else{
|
||||
memberDrone[j] = drone3;
|
||||
j+=1;
|
||||
std::cout << "drone3" <<std::endl;
|
||||
}
|
||||
|
||||
for(int i=0;i<index-2;i++){
|
||||
if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){ //load same plane member
|
||||
samePlane[s_index] = memberDrone[i];
|
||||
std::cout << samePlane[s_index].init_location.lat <<std::endl;
|
||||
std::cout << memberDrone[i].ID <<std::endl;
|
||||
s_index++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
|
||||
float d[s_index];
|
||||
int dir,check_index,ref_ID;
|
||||
std::cout <<"s_index " <<s_index <<std::endl;
|
||||
for(int i=0 ; i<s_index ; i++){ //caculate each same plane mamber's distance
|
||||
|
||||
dir = direct(data[0],leader_drone,self,samePlane[i]);
|
||||
d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
|
||||
|
||||
std::cout << i<<"," <<d[i] <<std::endl;
|
||||
std::cout << samePlane[i].ID <<std::endl;
|
||||
}
|
||||
|
||||
std::vector<float> _d(d,d+s_index);
|
||||
|
||||
check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
|
||||
ref_ID = samePlane[check_index].ID;
|
||||
|
||||
std::cout <<"refID " << ref_ID << std::endl;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -59,39 +243,25 @@ int main(int argc,char** argv){
|
||||
target.lat = 0;
|
||||
target.heading = 0;
|
||||
|
||||
self.lat = 1;
|
||||
self.lon = 1;
|
||||
self.lat = 2;
|
||||
self.lon = 2;
|
||||
self.ID = 1;
|
||||
|
||||
M1.lat = 2;
|
||||
M1.lon = 2;
|
||||
M1.lat = 1;
|
||||
M1.lon = 1;
|
||||
M1.ID = 2;
|
||||
|
||||
M2.lat = 6;
|
||||
M2.lon = 6;
|
||||
M2.lat = 5;
|
||||
M2.lon = 5;
|
||||
M2.ID = 3;
|
||||
|
||||
global_location data[]={target,self,M1,M2};
|
||||
size_t index = sizeof(data)/sizeof(data[0]);
|
||||
|
||||
|
||||
std::cout << is_leader(data,sizeof(data)/sizeof(data[0])) <<std::endl;
|
||||
find_ref_drone(data,index,check_leader(data,index).leader_ID);
|
||||
|
||||
// int index = sizeof(data)/sizeof(target); //4
|
||||
// float d[index];
|
||||
// int check_ID,leader_ID;
|
||||
|
||||
// for(int i=1 ; i<index ; i++){
|
||||
|
||||
// d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
|
||||
|
||||
// std::cout << i<<"," <<d[data[i].ID] <<std::endl;
|
||||
// }
|
||||
|
||||
// std::vector<float> _d(d+1,d+index-1);
|
||||
|
||||
// check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));
|
||||
// leader_ID = check_ID + 1;
|
||||
// std::cout <<"Drone " << leader_ID << " is leader " << std::endl;
|
||||
|
||||
|
||||
return 0;
|
||||
|
||||
@@ -0,0 +1,186 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.5">
|
||||
<world name="default">
|
||||
<physics type="ode">
|
||||
<ode>
|
||||
<solver>
|
||||
<type>quick</type>
|
||||
<iters>100</iters>
|
||||
<sor>1.0</sor>
|
||||
</solver>
|
||||
<constraints>
|
||||
<cfm>0.0</cfm>
|
||||
<erp>0.9</erp>
|
||||
<contact_max_correcting_vel>0.1</contact_max_correcting_vel>
|
||||
<contact_surface_layer>0.0</contact_surface_layer>
|
||||
</constraints>
|
||||
</ode>
|
||||
<real_time_update_rate>-1</real_time_update_rate>
|
||||
</physics>
|
||||
|
||||
<scene>
|
||||
<ambient>0.0 0.0 0.0 1.0</ambient>
|
||||
<shadows>0</shadows>
|
||||
</scene>
|
||||
|
||||
<model name="ground_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>5000 5000</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1</mu>
|
||||
<mu2>1</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="runway">
|
||||
<pose>000 0 0.005 0 0 0</pose>
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>1829 45</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Runway</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<visual name="grass">
|
||||
<pose>0 0 -0.1 0 0 0</pose>
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>5000 5000</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grass</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<include>
|
||||
<uri>model://sun</uri>
|
||||
</include>
|
||||
<model name="drone1">
|
||||
<pose> 0 0 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone1</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone2">
|
||||
<pose> 0 5 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone2</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="drone3">
|
||||
<pose> 0 -5 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone3</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<!-- <model name="tree">
|
||||
<pose> 5 -10 0 0 0 3.14</pose>
|
||||
<include>
|
||||
<uri>model://oak_tree</uri>
|
||||
</include>
|
||||
</model> -->
|
||||
|
||||
<plugin name='wind_plugin' filename='libgazebo_wind_plugin.so'>
|
||||
<frameId>base_link</frameId>
|
||||
<robotNamespace/>
|
||||
<windVelocityMean>4.0</windVelocityMean>
|
||||
<windVelocityMax>20.0</windVelocityMax>
|
||||
<windVelocityVariance>0</windVelocityVariance>
|
||||
<windDirectionMean>0 1 0</windDirectionMean>
|
||||
<windDirectionVariance>0</windDirectionVariance>
|
||||
<windGustStart>0</windGustStart>
|
||||
<windGustDuration>0</windGustDuration>
|
||||
<windGustVelocityMean>0</windGustVelocityMean>
|
||||
<windGustVelocityMax>20.0</windGustVelocityMax>
|
||||
<windGustVelocityVariance>0</windGustVelocityVariance>
|
||||
<windGustDirectionMean>1 0 0</windGustDirectionMean>
|
||||
<windGustDirectionVariance>0</windGustDirectionVariance>
|
||||
<windPubTopic>world_wind</windPubTopic>
|
||||
</plugin>
|
||||
<!--<model name="drone4">
|
||||
<pose> 5 0 0 0 0 4.71</pose>
|
||||
<include>
|
||||
<uri>model://drone4</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone5">
|
||||
<pose> 6 0 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone5</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone6">
|
||||
<pose> 6 2 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone6</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone7">
|
||||
<pose> 8 0 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone7</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone8">
|
||||
<pose> 8 2 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone8</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone9">
|
||||
<pose> 12 0 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone9</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone10">
|
||||
<pose> 12 2 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone10</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone11">
|
||||
<pose> 14 0 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone11</uri>
|
||||
</include>
|
||||
</model>
|
||||
<model name="drone12">
|
||||
<pose> 14 2 0 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://drone12</uri>
|
||||
</include>
|
||||
</model> -->
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
|
||||
Reference in New Issue
Block a user