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

bio drone debug complete

Browse Source
protobuf
Xuan0319 3 years ago
parent 17e375a850
commit 2efa5165de
16 changed files with 169 additions and 61 deletions
Show Stats Download Patch File Download Diff File

1
class_model/include/class_model/Leader.h
Unescape Escape View File

@ -35,6 +35,7 @@ private:
ros::NodeHandle node_handle_; ros::NodeHandle node_handle_;
global_location target_location; global_location target_location;
global_location pre_target;
global_location follower_location; global_location follower_location;
global_location leader_location; global_location leader_location;
global_location current_location; global_location current_location;

4
class_model/include/class_model/command.h
Unescape Escape View File

@ -6,18 +6,20 @@
#include <geometry_msgs/TwistStamped.h> #include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/GlobalPositionTarget.h> #include <mavros_msgs/GlobalPositionTarget.h>
#include <class_model/sensor.h> #include <class_model/sensor.h>
#include <geographic_msgs/GeoPoseStamped.h>
class CommandClass { class CommandClass {
public: public:
CommandClass(); CommandClass();
virtual ~CommandClass(); virtual ~CommandClass();
void set_global_position(float lat,float lon,float alt); void set_global_position(double lon,double lat,float alt,float yaw);
void set_velocity(float x,float y,float alt,float yaw,float second); void set_velocity(float x,float y,float alt,float yaw,float second);
void fix_velocity(float x,float y,float alt,float yaw,float second); void fix_velocity(float x,float y,float alt,float yaw,float second);
void velocity_init(); void velocity_init();
void set_target_position(float x,float y); void set_target_position(float x,float y);
ThreadGPSClass gps_object; ThreadGPSClass gps_object;
private: private:
// ROS NodeHandle // ROS NodeHandle
ros::NodeHandle node_handle_; ros::NodeHandle node_handle_;

6
class_model/include/class_model/follower.h
Unescape Escape View File

@ -40,7 +40,7 @@ public:
int find_ref_drone(global_location** data, size_t index, int leaderID); int find_ref_drone(global_location** data, size_t index, int leaderID);
float lon[3],lat[3]; float lon[3],lat[3];
global_location vector_LT,vector_SM; global_location vector_LT,vector_SM,pre_LT,vector_LR;
private: private:
@ -61,6 +61,10 @@ private:
float error_lon,error_lat; float error_lon,error_lat;
float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
float ignore_small = 0.50; float ignore_small = 0.50;
float m,n,k,cosTheta,pre_m,pre_k,u,LTcos; // function direct()'s variable
global_location pre_tar,ref_point; // function plane()'s variable
float slope,c,y; // function plane()'s variable
global_location pre_target; // function plane()'s variable
int flag = 0; int flag = 0;
void calculate_position(float k,float theta,int plane); void calculate_position(float k,float theta,int plane);

11
class_model/src/Leader.cpp
Unescape Escape View File

@ -13,12 +13,17 @@ void LeaderClass::leader(global_location target){
std_msgs::String message; std_msgs::String message;
flag = 0; flag = 0;
// heading_status = 0; if(pre_target.lon != target.lon || pre_target.lat != target.lat){
heading_status = 0;
}
while(true){ while(true){
if(flag==0){ if(flag==0){
go2target(target.lon,target.lat); go2target(target.lon,target.lat);
}else{ }else{
// next_command.publish(message); // next_command.publish(message);
pre_target.lon = target.lon;
pre_target.lat = target.lat;
ROS_INFO("break"); ROS_INFO("break");
break; break;
} }
@ -111,9 +116,9 @@ void LeaderClass::face2target(float target_heading){
float error_heading; float error_heading;
float heading = GPS_object.get_heading(); float heading = GPS_object.get_heading();
float error_yaw = 0.2; float error_yaw = 0.1;
error_heading = target_heading - heading; error_heading = target_heading - heading;
error_yaw = check_direction(error_heading)*error_yaw; error_yaw = check_direction(error_heading)*error_yaw*0.5;
if (error_heading < 5 && error_heading > -5 ){ if (error_heading < 5 && error_heading > -5 ){
error_yaw = 0; error_yaw = 0;

7
class_model/src/bio_main.cpp
Unescape Escape View File

@ -22,7 +22,7 @@ int main(int argc, char **argv) {
global_location target,self,M1,M2,M3,M4; global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader,flag; int ref_ID,checkLeader,leaderID,flag;
std::string type = "",pre_type ="x"; std::string type = "",pre_type ="x";
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point)
@ -58,6 +58,7 @@ int main(int argc, char **argv) {
// } // }
checkLeader = check_leader(*pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
leaderID = check_leader(*pos,index).leader_ID;
if(checkLeader != 1){ if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
} }
@ -73,8 +74,10 @@ int main(int argc, char **argv) {
if(checkLeader == 1){ if(checkLeader == 1){
leader_object.leader(target); leader_object.leader(target);
target.lon = config["routh2"]["x"].as<float>()*1e7;
target.lat = config["routh2"]["y"].as<float>()*1e7;
}else{ }else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
follower_object.follower(member,m_index,ref_ID); //follow reference drone position follower_object.follower(member,m_index,ref_ID); //follow reference drone position
} }

6
class_model/src/bio_main2.cpp
Unescape Escape View File

@ -22,7 +22,7 @@ int main(int argc, char **argv) {
global_location target,self,M1,M2,M3,M4; global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader,flag; int ref_ID,checkLeader,leaderID,flag;
std::string type = "",pre_type ="x"; std::string type = "",pre_type ="x";
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point)
@ -58,6 +58,8 @@ int main(int argc, char **argv) {
// } // }
checkLeader = check_leader(*pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
leaderID = check_leader(*pos,index).leader_ID;
if(checkLeader != 1){ if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
} }
@ -74,7 +76,7 @@ int main(int argc, char **argv) {
if(checkLeader == 1){ if(checkLeader == 1){
leader_object.leader(target); leader_object.leader(target);
}else{ }else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
follower_object.follower(member,m_index,ref_ID); //follow reference drone position follower_object.follower(member,m_index,ref_ID); //follow reference drone position
} }

7
class_model/src/bio_main3.cpp
Unescape Escape View File

@ -22,7 +22,7 @@ int main(int argc, char **argv) {
global_location target,self,M1,M2,M3,M4; global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader,flag; int ref_ID,checkLeader,leaderID,flag;
std::string type = "",pre_type ="x"; std::string type = "",pre_type ="x";
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point)
@ -58,6 +58,7 @@ int main(int argc, char **argv) {
// } // }
checkLeader = check_leader(*pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
leaderID = check_leader(*pos,index).leader_ID;
if(checkLeader != 1){ if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
} }
@ -72,9 +73,9 @@ int main(int argc, char **argv) {
M4 = request_object.get_M4_GPS(); M4 = request_object.get_M4_GPS();
if(checkLeader == 1){ if(checkLeader == 1){
// leader_object.l/eader(target); leader_object.leader(target);
}else{ }else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
follower_object.follower(member,m_index,ref_ID); //follow reference drone position follower_object.follower(member,m_index,ref_ID); //follow reference drone position
} }

5
class_model/src/bio_main4.cpp
Unescape Escape View File

@ -22,7 +22,7 @@ int main(int argc, char **argv) {
global_location target,self,M1,M2,M3,M4; global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader,flag; int ref_ID,checkLeader,leaderID,flag;
std::string type = "",pre_type ="x"; std::string type = "",pre_type ="x";
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point)
@ -58,6 +58,7 @@ int main(int argc, char **argv) {
// } // }
checkLeader = check_leader(*pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
leaderID = check_leader(*pos,index).leader_ID;
if(checkLeader != 1){ if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
} }
@ -74,7 +75,7 @@ int main(int argc, char **argv) {
if(checkLeader == 1){ if(checkLeader == 1){
leader_object.leader(target); leader_object.leader(target);
}else{ }else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
follower_object.follower(member,m_index,ref_ID); //follow reference drone position follower_object.follower(member,m_index,ref_ID); //follow reference drone position
} }

5
class_model/src/bio_main5.cpp
Unescape Escape View File

@ -22,7 +22,7 @@ int main(int argc, char **argv) {
global_location target,self,M1,M2,M3,M4; global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader,flag; int ref_ID,checkLeader,leaderID,flag;
std::string type = "",pre_type ="x"; std::string type = "",pre_type ="x";
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point)
@ -58,6 +58,7 @@ int main(int argc, char **argv) {
// } // }
checkLeader = check_leader(*pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
leaderID = check_leader(*pos,index).leader_ID;
if(checkLeader != 1){ if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
} }
@ -74,7 +75,7 @@ int main(int argc, char **argv) {
if(checkLeader == 1){ if(checkLeader == 1){
leader_object.leader(target); leader_object.leader(target);
}else{ }else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
follower_object.follower(member,m_index,ref_ID); //follow reference drone position follower_object.follower(member,m_index,ref_ID); //follow reference drone position
} }

37
class_model/src/command.cpp
Unescape Escape View File

@ -10,7 +10,7 @@ CommandClass::CommandClass() : node_handle_("~"){
} }
velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",10); velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",10);
gps_command=node_handle_.advertise<mavros_msgs::GlobalPositionTarget>(ros_namespace+"/mavros/setpoint_raw/global",10); gps_command=node_handle_.advertise<geographic_msgs::GeoPoseStamped>(ros_namespace+"/mavros/setpoint_position/global",10);
} }
CommandClass::~CommandClass() { ros::shutdown(); } CommandClass::~CommandClass() { ros::shutdown(); }
@ -96,15 +96,34 @@ void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second
} }
void CommandClass::set_global_position(float lat,float lon,float alt){ void CommandClass::set_global_position(double lon,double lat,float alt,float yaw){
mavros_msgs::GlobalPositionTarget goal_position; // mavros_msgs::GlobalPositionTarget goal_position;
goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_TERRAIN_ALT; // goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_REL_ALT;
goal_position.type_mask = goal_position.IGNORE_VX | goal_position.IGNORE_VY | goal_position.IGNORE_VZ | goal_position.IGNORE_AFX | goal_position.IGNORE_AFY | goal_position.IGNORE_AFZ | goal_position.IGNORE_YAW | goal_position.IGNORE_YAW_RATE; // goal_position.type_mask = goal_position.IGNORE_VZ | goal_position.IGNORE_YAW_RATE;
goal_position.latitude = lat; // goal_position.latitude = lat;
goal_position.longitude = lon; // goal_position.longitude = lon;
goal_position.altitude = alt; // goal_position.altitude = alt;
gps_command.publish(goal_position); // goal_position.yaw = yaw;
// goal_position.header.frame_id = "map";
// // goal_position.type_mask = 65535;
geographic_msgs::GeoPoseStamped goal_position;
goal_position.header.frame_id = "map";
goal_position.pose.position.latitude = lat;
goal_position.pose.position.longitude = lon;
goal_position.pose.position.altitude = 67;
ROS_INFO("%f,%f",lat,lon);
double begin = ros::Time::now().toSec();
while(true){
gps_command.publish(goal_position);
double now = ros::Time::now().toSec();
if((now-begin)>0.5){
break;
}
}
} }

4
class_model/src/config.yaml
Unescape Escape View File

@ -4,6 +4,10 @@ routh1:
x: 120.6743161 #(24.1218859, 120.6743161) x: 120.6743161 #(24.1218859, 120.6743161)
y: 24.1219310 y: 24.1219310
z: 0.0 z: 0.0
routh2:
x: 120.6743661 #(24.1218859, 120.6743161)
y: 24.1219310
z: 0.0
formation: formation:
distance: 4.0 distance: 4.0
angle: 45 angle: 45

115
class_model/src/follower.cpp
Unescape Escape View File

@ -45,15 +45,38 @@ void FollowerClass::follower(global_location** member_data, size_t index, int re
void FollowerClass::plane(global_location* target, global_location* leader, global_location* follower){ void FollowerClass::plane(global_location* target, global_location* leader, global_location* follower){
//caculate drone is locate on R/L plane //calculate drone is locate on R/L plane
float slope,c,y;
slope = (target->lat-leader->lat)/(target->lon-leader->lon); float phi = leader->heading/100 ;
phi = phi*3.14/180;
// float ref_y = leader->lat + 5000;
// float ref_x = leader->lon + 1;
// ref_point.lon = (cos(-phi)*leader->lon + sin(-phi)*ref_y);
// ref_point.lat = (-sin(-phi)*leader->lon + cos(-phi)*ref_y);
ref_point.lon = 5000*sin(phi) + leader->lon;
ref_point.lat = 5000*cos(phi) + leader->lat;
slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon);
c = leader->lat - slope*leader->lon; c = leader->lat - slope*leader->lon;
// if((target->lat != pre_target.lat) || (target->lon != pre_target.lon)){
// slope = (target->lat-leader->lat)/(target->lon-leader->lon);
// c = leader->lat - slope*leader->lon;
// }
y = slope * follower->lon + c; y = slope * follower->lon + c;
pre_target.lat = target->lat;
pre_target.lon = target->lon;
// std::cout<< follower.ID << ","<< follower.lat <<","<< y <<std::endl; // std::cout<< follower.ID << ","<< follower.lat <<","<< y <<std::endl;
std::cout<< "slope: " << slope <<std::endl;
if(follower->lat <= y){ if((follower->lat <= y && slope>0) || (follower->lat > y && slope<0)){
// std::cout<< follower.lat <<","<< y <<std::endl; std::cout <<"drone :"<<follower->ID <<", plane: 1" <<std::endl;
switch (follower->ID){ switch (follower->ID){
case 1: case 1:
drone1.init_location.lat = follower->lat; drone1.init_location.lat = follower->lat;
@ -86,8 +109,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
drone5.plane = 1; drone5.plane = 1;
break; break;
} }
}else if(follower->lat > y){ }else if((follower->lat > y && slope>0) || (follower->lat <= y && slope<0)){
// std::cout<< follower.lat <<","<< y <<std::endl; std::cout <<"drone :"<<follower->ID <<", plane: -1" <<std::endl;
switch (follower->ID){ switch (follower->ID){
case 1: case 1:
drone1.init_location.lat = follower->lat; drone1.init_location.lat = follower->lat;
@ -126,22 +149,56 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
} }
int FollowerClass::direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member){ int FollowerClass::direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member){
//Calculate whether the vector (SM) of self and member N is the same as the flight direction (LT)
float m,n,k,cosTheta; float phi = leader->heading/100 ;
phi = phi*3.14/180;
// float ref_y = leader->lat + 5000;
// float ref_x = leader->lon + 1;
ref_point.lon = 5000*sin(phi) + leader->lon;
ref_point.lat = 5000*cos(phi) + leader->lat;
// vector_LT.lon = target->lon - leader->lon; //vector_LT (leader->target)
// vector_LT.lat = target->lat - leader->lat;
if(flag == 0){
vector_LT.lon = target->lon - leader->lon; //vector_LT (leader->target)
vector_LT.lat = target->lat - leader->lat;
flag = 1;
}
vector_SM.lon = member.init_location.lon - self.init_location.lon; //vector_SM (self->member) 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; 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| vector_LR.lon = ref_point.lon - leader->lon; //vector_LR (leader->Ref)
vector_LR.lat = ref_point.lat - leader->lat;
k = (vector_LR.lon*vector_SM.lon)+(vector_LR.lat*vector_SM.lat); //k = vector_LT dot vector_SM
m = sqrt(pow(vector_LR.lon,2)+pow(vector_LR.lat,2)); //m = |vector_LT|
n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2)); //n = |vector_SM| n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2)); //n = |vector_SM|
// pre_m = sqrt(pow(pre_LT.lon,2)+pow(pre_LT.lat,2));
// pre_k = (pre_LT.lon*vector_SM.lon)+(pre_LT.lat*vector_SM.lat);
// if(pre_m != 0 && ((target->lat == pre_tar.lat) && (target->lon == pre_tar.lon))){
// u = (vector_LT.lon*pre_LT.lon)+(vector_LT.lat*pre_LT.lat);
// LTcos = u / (m*pre_m);
// if (LTcos < 0){
// m = pre_m;
// k = pre_k;
// std::cout <<"memberID: "<<member.ID << " fix" << std::endl;
// }
// std::cout <<"dir_test "<< std::endl;
// }else{
// pre_LT = vector_LT;
// pre_tar.lat = target->lat;
// pre_tar.lon = target->lon;
// }
cosTheta = k / (m*n); cosTheta = k / (m*n);
std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl;
if(cosTheta > 0){ // std::cout <<"drone "<< self.ID <<"cosTheta: "<< cosTheta << std::endl;
if(cosTheta >= 0){
return 1; return 1;
}else{ }else{
return 10000; return 10000;
@ -239,7 +296,7 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){ //load same plane member if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){ //load same plane member
samePlane[s_index] = memberDrone[i]; samePlane[s_index] = memberDrone[i];
// std::cout << samePlane[s_index].init_location.lat <<std::endl; // std::cout << samePlane[s_index].init_location.lat <<std::endl;
// std::cout << memberDrone[i].ID <<std::endl; // std::cout <<"drone"<<self.ID<<", memberdrone"<< memberDrone[i].ID<<", plane: "<< memberDrone[i].plane <<std::endl;
s_index++; s_index++;
} }
@ -256,7 +313,7 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
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) ); 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 << i<<"," <<d[i] <<std::endl;
// std::cout << samePlane[i].ID <<std::endl; std::cout <<"drone" <<samePlane[i].ID<<" ,d = "<<d[i] <<std::endl;
} }
std::vector<float> _d(d,d+s_index); std::vector<float> _d(d,d+s_index);
@ -264,8 +321,8 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value'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; ref_ID = samePlane[check_index].ID;
std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << std::endl; std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << ",mem_index: "<< s_index << std::endl;
std::cout <<"\n"<< std::endl;
return ref_ID; return ref_ID;
} }
@ -306,7 +363,7 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
float error_degree = deg_phi - heading; float error_degree = deg_phi - heading;
float error_yaw = 0.2; //CCW float error_yaw = 0.4; //CCW
if (error_degree >= 360){ if (error_degree >= 360){
error_degree -= 360; error_degree -= 360;
@ -329,17 +386,17 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
error_yaw = 0; error_yaw = 0;
} }
if (error_lon < -1){ if (error_lon < -2){
error_lon = -1; error_lon = -2;
} }
if (error_lon > 1){ if (error_lon > 2){
error_lon = 1; error_lon = 2;
} }
if (error_lat < -1){ if (error_lat < -2){
error_lat = -1; error_lat = -2;
} }
if (error_lat > 1){ if (error_lat > 2){
error_lat = 1; error_lat = 2;
} }
// ROS_INFO("%d,%f,%f",refpos.ID,refpos.lon ,refpos.lat ); // ROS_INFO("%d,%f,%f",refpos.ID,refpos.lon ,refpos.lat );

7
class_model/src/formation.cpp
Unescape Escape View File

@ -552,7 +552,9 @@ void FormationClass::calculate_position(float k,float theta,int direction){
// // ROS_INFO("error_yaw:%f",error_yaw); // // ROS_INFO("error_yaw:%f",error_yaw);
// ROS_INFO("************************************"); // ROS_INFO("************************************");
command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3); // command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3);
command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
// command_object.set_velocity(0,0,0,error_yaw,0.1);
} }
void FormationClass::spherical_coordinate(float k,float theta,float psi){ void FormationClass::spherical_coordinate(float k,float theta,float psi){
@ -685,7 +687,8 @@ void FormationClass::go2target(float x,float y){
} }
PubData_object.publishData(drone_speed); PubData_object.publishData(drone_speed);
command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1); // command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,heading);
// ROS_INFO("slope:%f,%f,%f",slope,x,y); // ROS_INFO("slope:%f,%f,%f",slope,x,y);
ROS_INFO("%f,%f",drone_speed.x,drone_speed.y); ROS_INFO("%f,%f",drone_speed.x,drone_speed.y);

1
class_model/src/main.cpp
Unescape Escape View File

@ -62,6 +62,7 @@ int main(int argc, char **argv) {
if(type == "init"){ if(type == "init"){
select_formation.goose_formation(); select_formation.goose_formation();
ROS_INFO("init formation"); ROS_INFO("init formation");
pre_type ="init";
}else if(type == "line"){ }else if(type == "line"){
select_formation.line_formation(); select_formation.line_formation();
pre_type ="line"; pre_type ="line";

6
class_model/src/test_lib.cpp
Unescape Escape View File

@ -264,10 +264,14 @@ int main(int argc,char** argv){
M2.lat = 1; M2.lat = 1;
M2.ID = 3; M2.ID = 3;
std::cout<< data[3].lon<<std::endl; // std::cout<< data[3].lon<<std::endl;
// find_ref_drone(data,index,check_leader(data,index).leader_ID); // find_ref_drone(data,index,check_leader(data,index).leader_ID);
// float theta = -45;
// theta = theta *3.14/180;
// std::cout<< 3*sin(theta)<<","<< 3*cos(theta) <<std::endl;

8
iq_sim/worlds/five_drone.world
Unescape Escape View File

@ -124,25 +124,25 @@
</include> </include>
</model> </model>
<model name="drone2"> <model name="drone2">
<pose> 0 4 0 0 0 0</pose> <pose> -1 4 0 0 0 0</pose>
<include> <include>
<uri>model://drone2</uri> <uri>model://drone2</uri>
</include> </include>
</model> </model>
<model name="drone3"> <model name="drone3">
<pose> 0 8 0 0 0 0</pose> <pose> -2 8 0 0 0 0</pose>
<include> <include>
<uri>model://drone3</uri> <uri>model://drone3</uri>
</include> </include>
</model> </model>
<model name="drone4"> <model name="drone4">
<pose> 0 -4 0 0 0 0</pose> <pose> -1 -4 0 0 0 0</pose>
<include> <include>
<uri>model://drone4</uri> <uri>model://drone4</uri>
</include> </include>
</model> </model>
<model name="drone5"> <model name="drone5">
<pose> 0 -8 0 0 0 0</pose> <pose> -2 -8 0 0 0 0</pose>
<include> <include>
<uri>model://drone5</uri> <uri>model://drone5</uri>
</include> </include>

Write Preview
Loading…
Cancel
Save