add bio follower function (finish)

protobuf
Xuan0319 3 years ago
parent 9a1f60e5af
commit 83faefde1a

@ -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;

@ -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
}

@ -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::Bio_INFO(std::string data){
j_data = json::parse(data); //open source
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"];
}
}
global_location RequestClass::get_self_GPS(){
return self_position;
}
global_location RequestClass::get_M1_GPS(){
return M1_position;
}
global_location RequestClass::get_M2_GPS(){
return M2_position;
}
// 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){
// j_data = json::parse(data); //open source
// 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"];
// }
// }
// global_location RequestClass::get_self_GPS(){
// return self_position;
// }
// global_location RequestClass::get_M1_GPS(){
// return M1_position;
// }
// global_location RequestClass::get_M2_GPS(){
// return M2_position;
// }

@ -3,37 +3,221 @@
#include "class_model/gps_location.h"
#include <algorithm>
#include <math.h>
#include "class_model/DroneStatus.h"
using namespace std;
bool is_leader(global_location data[], size_t index){
global_location leader_drone;
DroneStatus memberDrone[3];
DroneStatus drone1;
DroneStatus drone2;
DroneStatus drone3;
DroneStatus self;
DroneStatus samePlane[3];
global_location follower_location;
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 << _d[0] << " ,"<< _d[1] << " ,"<< _d[2] << std::endl;
std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
if (data[1].ID == _result.leader_ID){
_result.is_leader = 1;
return _result;
}else{
_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){
std::cout <<"Drone " << leader_ID << " is leader " << std::endl;
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 (data[1].ID == leader_ID){
if(cosTheta > 0){
return 1;
}else{
return 0;
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;
// 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;
// }
find_ref_drone(data,index,check_leader(data,index).leader_ID);
// 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>
Loading…
Cancel
Save