Xuan0319/DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add find_reference_drone function(unfinished)

Browse Source
This commit is contained in:
Xuan0319
2023-02-16 23:04:18 +08:00
parent 272060ec4e
commit f75b57b8ea
4 changed files with 187 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/include/class_model/FindLeader.h
+15 -7
View File
@@ -2,10 +2,16 @@
#include "class_model/DroneStatus.h"
#include <iterator>
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){
float d[index];
int check_ID,leader_ID;
int check_ID;
result _result;
for(int i=1 ; i<index ; i++){
@@ -17,14 +23,16 @@ bool is_leader(global_location data[], size_t index){
std::vector<float> _d(d+1,d+index-1);
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 <<"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;
}
}
class_model/include/class_model/follower.h
+6 -1
View File
@@ -24,6 +24,8 @@ public:
DroneStatus drone1;
DroneStatus drone2;
DroneStatus drone3;
DroneStatus self;
DroneStatus samePlane[3];
void follower();
@@ -37,9 +39,12 @@ private:
global_location target_location;
global_location follower_location;
global_location leader_location;
global_location leader_drone;
DroneStatus memberDrone[];
void calculate_position(float k,float theta);
void plane(global_location target_location,global_location leader_location,global_location follower_location);
void plane(global_location target, global_location leader, global_location follower);
void find_ref_drone(global_location data[], size_t index, int leaderID);
void choose_leader();
void follow();
void get_location();
class_model/src/bio_main.cpp
+5 -4
View File
@@ -26,8 +26,6 @@ int main(int argc, char **argv) {
// SelectionClass selection_object;
global_location target,self,M1,M2;
global_location pos[] = {target,self,M1,M2};
size_t index = sizeof(pos)/sizeof(pos[0]);
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point)
target.lon = config["routh1"]["x"].as<float>();
@@ -37,10 +35,13 @@ int main(int argc, char **argv) {
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
if(is_leader(pos,index) == 1){
global_location pos[] = {target,self,M1,M2};
size_t index = sizeof(pos)/sizeof(pos[0]);
if(check_leader(pos,index).is_leader == 1){
//fly2target
}else{
//find reference drone
//find reference drone(pos,index,leader_ID);
}
class_model/src/follower.cpp
+160 -38
View File
@@ -14,24 +14,146 @@ void FollowerClass::follower(){
}
void FollowerClass::plane(global_location target_location,global_location leader_location,global_location follower_location){
void FollowerClass::plane(global_location target, global_location leader, global_location follower){
//caculate drone is locate on R/L plane
float slope,c,y;
slope = (target_location.lat-leader_location.lat)/(target_location.lon-leader_location.lon);
c = target_location.lat - slope*target_location.lon;
y = slope * follower_location.lon + c;
y = slope * follower.lon + c;
if(follower_location.lat >= y){
drone1.plane = 1; //right plane
if(follower.lat > y){
switch (follower.ID){
case 1:
drone1.init_location.lat = follower.lat;
drone1.init_location.lon = follower.lon;
drone1.plane = -1; //right plane
break;
case 2:
drone2.init_location.lat = follower.lat;
drone2.init_location.lon = follower.lon;
drone2.plane = -1;
break;
case 3:
drone3.init_location.lat = follower.lat;
drone3.init_location.lon = follower.lon;
drone3.plane = -1;
break;
}
}else if(follower_location.lat < y){
drone1.plane = 0; //left plane
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.plane = 1;
break;
case 3:
drone3.init_location.lat = follower.lat;
drone3.init_location.lon = follower.lon;
drone3.plane = 1;
break;
}
}
choose_leader();
}
void FollowerClass::choose_leader(){
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){
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;
}
}
}
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,k=0;
if(drone1.ID == data[1].ID){ //seperate self and other
self = drone1;
}else{
memberDrone[j] = drone1;
j+=1;
}
if(drone2.ID == data[1].ID){
self = drone2;
}else{
memberDrone[j] = drone2;
j+=1;
}
if(drone3.ID == data[1].ID){
self = drone1;
}else{
memberDrone[j] = drone3;
j+=1;
}
for(int i=0;i<index-1;i++){
if(memberDrone[i].plane == self.ID){
samePlane[k] = memberDrone[i];
k++;
}
}
int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
float d[s_index];
int check_ID;
for(int i=0 ; i<s_index ; i++){
d[i] = sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lat - samePlane[i].init_location.lat),2) );
// std::cout << i<<"," <<d[data[i].ID] <<std::endl;
}
std::vector<float> _d(d,d+s_index-1);
check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
// std::cout <<"Drone " << " is leader " << std::endl;
@@ -46,42 +168,42 @@ void FollowerClass::get_location(){
}
void FollowerClass::calculate_position(float k,float theta){
// void FollowerClass::calculate_position(float k,float theta){
theta = theta*3.14/180;
float phi = request_object.get_leader_heading()/100;
phi = phi*3.14/180;
leader_location=request_object.get_leader_GPS();
follower_location=GPS_object.gps_position();
// theta = theta*3.14/180;
// float phi = request_object.get_leader_heading()/100;
// phi = phi*3.14/180;
// leader_location=request_object.get_leader_GPS();
// follower_location=GPS_object.gps_position();
double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};
float transfer[2][2]={
cos(phi),-sin(phi),
sin(phi),cos(phi)
};
float Q[2]={k*sin(theta),k*cos(theta)};
float T[2]={1,-1};
// double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};
// float transfer[2][2]={
// cos(phi),-sin(phi),
// sin(phi),cos(phi)
// };
// float Q[2]={k*sin(theta),k*cos(theta)};
// float T[2]={1,-1};
Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
// Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
// Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
float error_x = Pf[0] - (follower_location.lon*1e+5);
float error_y = Pf[1] - (follower_location.lat*1e+5);
// float error_x = Pf[0] - (follower_location.lon*1e+5);
// float error_y = Pf[1] - (follower_location.lat*1e+5);
if (error_x < 0.3 & error_x > -0.3){
error_x = 0;
}
if (error_y < 0.3 & error_y > -0.3){
error_y = 0;
}
// if (error_x < 0.3 & error_x > -0.3){
// error_x = 0;
// }
// if (error_y < 0.3 & error_y > -0.3){
// error_y = 0;
// }
// ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
ROS_INFO("%f,%f",Pf[0],Pf[1]);
ROS_INFO("%f,%f",error_x,error_y);
ROS_INFO("************************************");
// // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
// ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
// ROS_INFO("%f,%f",Pf[0],Pf[1]);
// ROS_INFO("%f,%f",error_x,error_y);
// ROS_INFO("************************************");
command_object.fix_velocity(error_x,error_y,0,0,0.1);
// sleep(0.5);
// command_object.fix_velocity(error_x,error_y,0,0,0.1);
// // sleep(0.5);
}
// }