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

test route finish

Browse Source
protobuf
Xuan0319 3 years ago
parent 2efa5165de
commit 8831cb7e6e
8 changed files with 72 additions and 38 deletions
Show Stats Download Patch File Download Diff File

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

@ -6,7 +6,7 @@
#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> // #include <geographic_msgs/GeoPoseStamped.h>
class CommandClass { class CommandClass {
public: public:
@ -25,7 +25,7 @@ private:
ros::NodeHandle node_handle_; ros::NodeHandle node_handle_;
geometry_msgs::TwistStamped msg; geometry_msgs::TwistStamped msg;
mavros_msgs::GlobalPositionTarget goal_position; // mavros_msgs::GlobalPositionTarget goal_position;
global_location current_location; global_location current_location;
global_location target_location; global_location target_location;
global_location origin_location; global_location origin_location;

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

@ -65,7 +65,7 @@ private:
global_location pre_tar,ref_point; // function plane()'s variable global_location pre_tar,ref_point; // function plane()'s variable
float slope,c,y; // function plane()'s variable float slope,c,y; // function plane()'s variable
global_location pre_target; // function plane()'s variable global_location pre_target; // function plane()'s variable
int flag = 0; int flag = 0,pre_plane = 0;
void calculate_position(float k,float theta,int plane); void calculate_position(float k,float theta,int plane);
void plane(global_location* target, global_location* leader, global_location* follower); void plane(global_location* target, global_location* leader, global_location* follower);

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

@ -15,8 +15,9 @@ void LeaderClass::leader(global_location target){
flag = 0; flag = 0;
if(pre_target.lon != target.lon || pre_target.lat != target.lat){ if(pre_target.lon != target.lon || pre_target.lat != target.lat){
heading_status = 0; heading_status = 0;
ROS_INFO("heading_flag");
} }
heading_status = 0;
while(true){ while(true){
if(flag==0){ if(flag==0){
go2target(target.lon,target.lat); go2target(target.lon,target.lat);
@ -103,10 +104,10 @@ void LeaderClass::go2target(float target_lon,float target_lat){
// 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);
// ROS_INFO("heading:%f,target_heading:%f",heading,target_heading); ROS_INFO("heading:%f,target_heading:%f",heading,target_heading);
// ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat); // ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat);
// ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5); // ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5);
// ROS_INFO("************************************"); ROS_INFO("************************************");
@ -116,11 +117,11 @@ 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.1; float error_yaw = 0.2;
error_heading = target_heading - heading; error_heading = target_heading - heading;
error_yaw = check_direction(error_heading)*error_yaw*0.5; error_yaw = check_direction(error_heading)*error_yaw*0.5;
if (error_heading < 5 && error_heading > -5 ){ if (error_heading < 3 && error_heading > -3 ){
error_yaw = 0; error_yaw = 0;
heading_status = 1; heading_status = 1;
} }

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

@ -74,8 +74,22 @@ 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; // for(int i=0;i<=360;i++){
target.lat = config["routh2"]["y"].as<float>()*1e7; // target.lon = config["routh1"]["x"].as<float>()*1e7 + 300*sin(i*0.2);
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 300*cos(i*0.2);
// leader_object.leader(target);
// std::cout<< "radin: " << i*3.14/180 <<std::endl;
// }
target.lon = config["routh1"]["x"].as<float>()*1e7 + 500;
target.lat = config["routh1"]["y"].as<float>()*1e7 ;
leader_object.leader(target);
target.lon = config["routh1"]["x"].as<float>()*1e7 ;
target.lat = config["routh1"]["y"].as<float>()*1e7 -500;
leader_object.leader(target);
target.lon = config["routh1"]["x"].as<float>()*1e7 -500;
target.lat = config["routh1"]["y"].as<float>()*1e7 ;
leader_object.leader(target);
}else{ }else{
ref_ID = follower_object.find_ref_drone(pos,index,leaderID); 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,9 @@ 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<geographic_msgs::GeoPoseStamped>(ros_namespace+"/mavros/setpoint_position/global",10); // gps_command=node_handle_.advertise<geographic_msgs::GeoPoseStamped>(ros_namespace+"/mavros/setpoint_position/global",10);
gps_command=node_handle_.advertise<mavros_msgs::GlobalPositionTarget>(ros_namespace+"/mavros/setpoint_raw/global",10);
} }
CommandClass::~CommandClass() { ros::shutdown(); } CommandClass::~CommandClass() { ros::shutdown(); }
@ -98,22 +100,29 @@ void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second
void CommandClass::set_global_position(double lon,double lat,float alt,float yaw){ 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_REL_ALT; goal_position.coordinate_frame = mavros_msgs::GlobalPositionTarget::FRAME_GLOBAL_REL_ALT;
// goal_position.type_mask = goal_position.IGNORE_VZ | goal_position.IGNORE_YAW_RATE; goal_position.type_mask = mavros_msgs::GlobalPositionTarget::IGNORE_VX |
// goal_position.latitude = lat; mavros_msgs::GlobalPositionTarget::IGNORE_VY |
// goal_position.longitude = lon; mavros_msgs::GlobalPositionTarget::IGNORE_VZ |
// goal_position.altitude = alt; mavros_msgs::GlobalPositionTarget::IGNORE_AFX |
// goal_position.yaw = yaw; mavros_msgs::GlobalPositionTarget::IGNORE_AFY |
mavros_msgs::GlobalPositionTarget::IGNORE_AFZ |
mavros_msgs::GlobalPositionTarget::FORCE |
mavros_msgs::GlobalPositionTarget::IGNORE_YAW_RATE;
goal_position.latitude = lat;
goal_position.longitude = lon;
goal_position.altitude = alt;
goal_position.yaw = yaw;
// goal_position.header.frame_id = "map"; // goal_position.header.frame_id = "map";
// // goal_position.type_mask = 65535; // goal_position.type_mask = 65535;
geographic_msgs::GeoPoseStamped goal_position; // geographic_msgs::GeoPoseStamped goal_position;
goal_position.header.frame_id = "map"; // goal_position.header.frame_id = "map";
goal_position.pose.position.latitude = lat; // goal_position.pose.position.latitude = lat;
goal_position.pose.position.longitude = lon; // goal_position.pose.position.longitude = lon;
goal_position.pose.position.altitude = 67; // goal_position.pose.position.altitude = 67;
ROS_INFO("%f,%f",lat,lon); ROS_INFO("%f,%f",lat,lon);
double begin = ros::Time::now().toSec(); double begin = ros::Time::now().toSec();

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

@ -48,15 +48,19 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
//calculate drone is locate on R/L plane //calculate drone is locate on R/L plane
float phi = leader->heading/100 ; float phi = leader->heading/100 ;
phi = phi*3.14/180; // phi = phi*3.14/180;
if (phi > 355 && phi < 5){
phi = 0;
}
// float ref_y = leader->lat + 5000; // float ref_y = leader->lat + 5000;
// float ref_x = leader->lon + 1; // float ref_x = leader->lon + 1;
// ref_point.lon = (cos(-phi)*leader->lon + sin(-phi)*ref_y); // ref_point.lon = (cos(-phi)*leader->lon + sin(-phi)*ref_y);
// ref_point.lat = (-sin(-phi)*leader->lon + cos(-phi)*ref_y); // ref_point.lat = (-sin(-phi)*leader->lon + cos(-phi)*ref_y);
ref_point.lon = 5000*sin(phi) + leader->lon; ref_point.lon = 5000*sin(phi*3.14/180) + leader->lon + 1;
ref_point.lat = 5000*cos(phi) + leader->lat; ref_point.lat = 5000*cos(phi*3.14/180) + leader->lat;
slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon); slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon);
@ -75,8 +79,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
// std::cout<< follower.ID << ","<< follower.lat <<","<< y <<std::endl; // std::cout<< follower.ID << ","<< follower.lat <<","<< y <<std::endl;
std::cout<< "slope: " << slope <<std::endl; std::cout<< "slope: " << slope <<std::endl;
if((follower->lat <= y && slope>0) || (follower->lat > y && slope<0)){ if((follower->lat <= y && phi < 180) || (follower->lat > y && phi >= 180)){
std::cout <<"drone :"<<follower->ID <<", plane: 1" <<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;
@ -109,8 +113,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
drone5.plane = 1; drone5.plane = 1;
break; break;
} }
}else if((follower->lat > y && slope>0) || (follower->lat <= y && slope<0)){ }else if((follower->lat > y && phi < 180) || (follower->lat <= y && phi >= 180)){
std::cout <<"drone :"<<follower->ID <<", plane: -1" <<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;
@ -195,7 +199,7 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
// } // }
cosTheta = k / (m*n); cosTheta = k / (m*n);
std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl; // std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl;
// std::cout <<"drone "<< self.ID <<"cosTheta: "<< cosTheta << std::endl; // std::cout <<"drone "<< self.ID <<"cosTheta: "<< cosTheta << std::endl;
if(cosTheta >= 0){ if(cosTheta >= 0){
@ -313,7 +317,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 <<"drone" <<samePlane[i].ID<<" ,d = "<<d[i] <<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);
@ -338,8 +342,14 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
float heading = GPS_object.get_heading(); float heading = GPS_object.get_heading();
float phi = deg_phi*3.14/180; //degree-->radian float phi = deg_phi*3.14/180; //degree-->radian
theta = plane*theta*3.14/180;
// if(deg_phi >= 180 && deg_phi < 350){
// plane = plane*-1;
// }else if((deg_phi >= 350 || deg_phi < 5) && pre_plane != 0){
// plane = pre_plane;
// }
theta = plane*theta*3.14/180;
pre_plane = plane;
double Pf[]={},Pl[]={refpos.lon ,refpos.lat}; //transfer maxtrix double Pf[]={},Pl[]={refpos.lon ,refpos.lat}; //transfer maxtrix
float transfer[2][2]={ float transfer[2][2]={
cos(phi),-sin(phi), cos(phi),-sin(phi),

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

@ -687,8 +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); // command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,target_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);

4
iq_sim/worlds/three_drone.world
Unescape Escape View File

@ -89,14 +89,14 @@
</include> </include>
</model> </model>
<model name="drone2"> <model name="drone2">
<pose> 0 5 0 0 0 0</pose> <pose> 0 -5 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 -5 0 0 0 0</pose> <pose> 0 5 0 0 0 0</pose>
<include> <include>
<uri>model://drone3</uri> <uri>model://drone3</uri>
</include> </include>

Write Preview
Loading…
Cancel
Save