test route finish

class_model/include/class_model/command.h

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

class_model/include/class_model/follower.h

@@ -65,7 +65,7 @@ private:
     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,pre_plane = 0;
 
     void calculate_position(float k,float theta,int plane);
     void plane(global_location* target, global_location* leader, global_location* follower);

class_model/src/Leader.cpp

@@ -15,8 +15,9 @@ void LeaderClass::leader(global_location target){
     flag = 0;
     if(pre_target.lon != target.lon || pre_target.lat != target.lat){
         heading_status = 0;
+        ROS_INFO("heading_flag");
     }
-
+    heading_status = 0;
     while(true){
         if(flag==0){
             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("%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("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 heading = GPS_object.get_heading();
-    float error_yaw = 0.1;
+    float error_yaw = 0.2;
     error_heading =  target_heading - heading;
     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;
         heading_status = 1;
     }

class_model/src/bio_main.cpp

@@ -74,8 +74,22 @@ int main(int argc, char **argv) {
 
         if(checkLeader == 1){
             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{
             ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
             follower_object.follower(member,m_index,ref_ID);   //follow reference drone position

class_model/src/command.cpp

@@ -10,7 +10,9 @@ CommandClass::CommandClass() : node_handle_("~"){
 	}
 
     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(); }
@@ -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){
     
-    // 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.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);
 	double begin = ros::Time::now().toSec();

class_model/src/follower.cpp

@@ -48,15 +48,19 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
     //calculate drone is locate on R/L plane
 
     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_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.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);
@@ -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<< "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){
             case 1:
                 drone1.init_location.lat = follower->lat;
@@ -109,8 +113,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
                 drone5.plane = 1;        
                 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){
             case 1:
                 drone1.init_location.lat = follower->lat;
@@ -195,7 +199,7 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
     // }
     
     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;
     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) );
 
         // 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); 
@@ -338,8 +342,14 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
     float heading = GPS_object.get_heading();
     
     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  
     float transfer[2][2]={
                             cos(phi),-sin(phi),

class_model/src/formation.cpp

@@ -687,8 +687,8 @@ void FormationClass::go2target(float x,float y){
     }
 
     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("%f,%f",drone_speed.x,drone_speed.y);

iq_sim/worlds/three_drone.world

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