test bio change leader success

class_model/include/class_model/convert_degree.h

@@ -5,7 +5,7 @@ float ConvertDeg(float x ,float y){
         degree = 90-(atan(y/x)*180/3.14);
     }
     if(x<0 && y>0){
-        degree = atan(y/x)*180/3.14 + 360;
+        degree = abs(atan(y/x)*180/3.14) + 270;
     }
     if(x<0 && y<0){
         degree = (atan(x/y)*180/3.14) + 180;

class_model/include/class_model/follower.h

@@ -1,7 +1,9 @@
 /*Follow the leader in a fixed formation */
 #ifndef FOLLOWER_H
 #define FOLLOWER_H
-#define swarmNum 5
+#define swarmNum 3
+#define changeLeader
+// #define NoChangeLeader
 #include <ros/ros.h>
 #include "class_model/sensor.h"
 #include "class_model/mode.h"
@@ -38,6 +40,7 @@ public:
     
     void follower(global_location** member_data, size_t index, int refID);
     int find_ref_drone(global_location** data, size_t index, int leaderID);
+    void face2target(global_location* target, global_location* leader);
 
     float lon[3],lat[3];
     global_location vector_LT,vector_SM,pre_LT,vector_LR;
@@ -66,6 +69,8 @@ private:
     float slope,c,y;              // function plane()'s variable
     global_location pre_target;  // function plane()'s variable
     int flag = 0,pre_plane = 0;
+    float error_heading, error_yaw ;  //function face2target()'s variable
+    int heading_status = 0;          //function face2target()'s variable
 
     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

@@ -16,8 +16,6 @@ void LeaderClass::leader(global_location target){
     if(pre_target.lon != target.lon || pre_target.lat != target.lat){
         heading_status = 0;
         ROS_INFO("heading_flag");
-        std::cout <<"x: "<< target.lon << std::endl;
-        std::cout <<"y: "<< target.lat << std::endl;
 
     }
     // heading_status = 0;
@@ -67,6 +65,8 @@ void LeaderClass::go2target(float target_lon,float target_lat){
     y = (target_lat/100) - (current_location.lat*1e5);
     target_heading = ConvertDeg(x,y);
 
+    // ROS_INFO("x:%f,y:%f,target_heading:%f",x,y,target_heading);  
+
     // float error_yaw = 0.2;
     // error_heading =  target_heading - heading;
     // error_yaw = check_direction(error_heading)*error_yaw;              //check yaw direction

class_model/src/bio_main.cpp

@@ -3,6 +3,9 @@
 #include "class_model/follower.h"
 #include "yaml-cpp/yaml.h"
 
+#define changeLeader
+// #define NoChangeLeader
+
 int main(int argc, char **argv) {
 
     // Init ROS node
@@ -42,8 +45,10 @@ int main(int argc, char **argv) {
     M4 = request_object.get_M4_GPS();
 
 
-    global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
+    // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
-    global_location* member[] = {&M1,&M2,&M3,&M4};
+    // global_location* member[] = {&M1,&M2,&M3,&M4};
+    global_location* pos[] = {&target,&self,&M1,&M2};
+    global_location* member[] = {&M1,&M2};
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 
@@ -68,9 +73,11 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
+    #ifdef changeLeader   
+
     while(ros::ok()){
 
-        // type = receiver_object.check_command();
+        type = receiver_object.check_command();
 
             target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
             target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
@@ -79,6 +86,8 @@ int main(int argc, char **argv) {
             leaderID = check_leader(*pos,index).leader_ID;
             leaderIndex = check_leader(*pos,index).leader_index;
 
+            // follower_object.face2target(pos[0], pos[leaderIndex]);
+         
             // std::cout << leaderID << std::endl;
             // std::cout << target.lon << std::endl;
             errorX = abs(pos[leaderIndex]->lon - target.lon) ;
@@ -95,22 +104,10 @@ int main(int argc, char **argv) {
             M4 = request_object.get_M4_GPS();
 
             if(checkLeader == 1){
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 ;
+
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
+                std::cout << "Drone1 is Leader" << std::endl;
                 leader_object.leader(target);
 
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
-                // leader_object.leader(target);
-
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
-                // 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);
-
             }else{
                 ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
                 follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
@@ -122,22 +119,68 @@ int main(int argc, char **argv) {
             // std::cout <<"x: "<< errorX << std::endl;
             // std::cout <<"y: "<< errorY << std::endl;
         }
-
+        sleep(3);
-        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
                 routeIndex++;
-            std::cout <<"routeIndex: "<<routeIndex<<std::endl;
+                checkLeader = 0;
+                std::cout <<"Drone1routeIndex: "<<routeIndex<<std::endl;
             // OnTarget = 0;
             }
-        while (routeIndex >= routeNum){
+        // }
-            mode_object.set_Mode("LAND");
+        // type = "x";
-            std::cout <<"LAND"<<std::endl;
+        // while (routeIndex >= routeNum){
-        }
+        //     // mode_object.set_Mode("LAND");
+        //     // std::cout <<"LAND"<<std::endl;
+        // }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
     }
+    #endif
+
+    #ifdef NoChangeLeader
+
+    while(ros::ok()){
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
+
+        if(checkLeader == 1){
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
+            leader_object.leader(target);
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 ;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000; 
+            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
+        }
+
+        
+    }
+    #endif
+
     ros::waitForShutdown();
 
     return 0;
 
 }
+

class_model/src/bio_main2.cpp

@@ -3,6 +3,9 @@
 #include "class_model/follower.h"
 #include "yaml-cpp/yaml.h"
 
+#define changeLeader
+// #define NoChangeLeader
+
 int main(int argc, char **argv) {
 
     // Init ROS node
@@ -21,7 +24,7 @@ int main(int argc, char **argv) {
     ReceiverClass receiver_object;
     
 
-    global_location target,self,M1,M2,M3,M4;
+    global_location target,self,M1,M2,M3,M4,leader;
     int ref_ID,checkLeader,leaderID,leaderIndex,flag;
     int routeNum,routeIndex,OnTarget;
     float errorX,errorY;
@@ -42,8 +45,10 @@ int main(int argc, char **argv) {
     M4 = request_object.get_M4_GPS();
 
 
-    global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
+    // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
-    global_location* member[] = {&M1,&M2,&M3,&M4};
+    // global_location* member[] = {&M1,&M2,&M3,&M4};
+    global_location* pos[] = {&target,&self,&M1,&M2};
+    global_location* member[] = {&M1,&M2};
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 
@@ -68,9 +73,11 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
+    #ifdef changeLeader   
+
     while(ros::ok()){
 
-        // type = receiver_object.check_command();
+        type = receiver_object.check_command();
 
             target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
             target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
@@ -79,6 +86,8 @@ int main(int argc, char **argv) {
             leaderID = check_leader(*pos,index).leader_ID;
             leaderIndex = check_leader(*pos,index).leader_index;
 
+            // follower_object.face2target(pos[0], pos[leaderIndex]);
+         
             // std::cout << leaderID << std::endl;
             // std::cout << target.lon << std::endl;
             errorX = abs(pos[leaderIndex]->lon - target.lon) ;
@@ -95,22 +104,10 @@ int main(int argc, char **argv) {
             M4 = request_object.get_M4_GPS();
 
             if(checkLeader == 1){
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 ;
+
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
+                std::cout << "Drone2 is Leader" << std::endl;
                 leader_object.leader(target);
 
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
-                // leader_object.leader(target);
-
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
-                // 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);
-
             }else{
                 ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
                 follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
@@ -122,23 +119,67 @@ int main(int argc, char **argv) {
             // std::cout <<"x: "<< errorX << std::endl;
             // std::cout <<"y: "<< errorY << std::endl;
         }
-
+        sleep(3);
-        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
                 routeIndex++;
+                checkLeader = 0;
+                std::cout <<"Drone2routeIndex: "<<routeIndex<<std::endl;
             // OnTarget = 0;
             }
-        while (routeIndex >= routeNum){
+        // }
-            mode_object.set_Mode("LAND");
+        // type = "x";
-            std::cout <<"LAND"<<std::endl;
+        // while (routeIndex >= routeNum){
-        }
+        //     // mode_object.set_Mode("LAND");
+        //     // std::cout <<"LAND"<<std::endl;
+        // }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
-
-        // mode_object.set_Mode("LAND");
     }
+    #endif
+
+    #ifdef NoChangeLeader
+
+    while(ros::ok()){
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
+
+        if(checkLeader == 1){
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
+            leader_object.leader(target);
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 ;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000; 
+            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
+        }
+        
+    }
+    #endif
+
     ros::waitForShutdown();
 
     return 0;
 
 }
+

class_model/src/bio_main3.cpp

@@ -3,6 +3,9 @@
 #include "class_model/follower.h"
 #include "yaml-cpp/yaml.h"
 
+#define changeLeader
+// #define NoChangeLeader
+
 int main(int argc, char **argv) {
 
     // Init ROS node
@@ -21,7 +24,7 @@ int main(int argc, char **argv) {
     ReceiverClass receiver_object;
     
 
-    global_location target,self,M1,M2,M3,M4;
+    global_location target,self,M1,M2,M3,M4,leader;
     int ref_ID,checkLeader,leaderID,leaderIndex,flag;
     int routeNum,routeIndex,OnTarget;
     float errorX,errorY;
@@ -42,8 +45,10 @@ int main(int argc, char **argv) {
     M4 = request_object.get_M4_GPS();
 
 
-    global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
+    // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
-    global_location* member[] = {&M1,&M2,&M3,&M4};
+    // global_location* member[] = {&M1,&M2,&M3,&M4};
+    global_location* pos[] = {&target,&self,&M1,&M2};
+    global_location* member[] = {&M1,&M2};
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 
@@ -68,9 +73,11 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
+    #ifdef changeLeader   
+
     while(ros::ok()){
 
-        // type = receiver_object.check_command();
+        type = receiver_object.check_command();
 
             target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
             target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
@@ -79,6 +86,8 @@ int main(int argc, char **argv) {
             leaderID = check_leader(*pos,index).leader_ID;
             leaderIndex = check_leader(*pos,index).leader_index;
 
+            // follower_object.face2target(pos[0], pos[leaderIndex]);
+         
             // std::cout << leaderID << std::endl;
             // std::cout << target.lon << std::endl;
             errorX = abs(pos[leaderIndex]->lon - target.lon) ;
@@ -95,22 +104,10 @@ int main(int argc, char **argv) {
             M4 = request_object.get_M4_GPS();
 
             if(checkLeader == 1){
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 ;
+
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
+                std::cout << "Drone3 is Leader" << std::endl;
                 leader_object.leader(target);
 
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
-                // leader_object.leader(target);
-
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
-                // 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);
-
             }else{
                 ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
                 follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
@@ -122,23 +119,67 @@ int main(int argc, char **argv) {
             // std::cout <<"x: "<< errorX << std::endl;
             // std::cout <<"y: "<< errorY << std::endl;
         }
-
+        sleep(3);
-        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
                 routeIndex++;
+                checkLeader = 0;
+                std::cout <<"Drone3routeIndex: "<<routeIndex<<std::endl;
             // OnTarget = 0;
             }
-        while (routeIndex >= routeNum){
+        // }
-            mode_object.set_Mode("LAND");
+        // type = "x";
-            std::cout <<"LAND"<<std::endl;
+        // while (routeIndex >= routeNum){
-        }
+        //     // mode_object.set_Mode("LAND");
+        //     // std::cout <<"LAND"<<std::endl;
+        // }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
-
-        // mode_object.set_Mode("LAND");
     }
+    #endif
+
+    #ifdef NoChangeLeader
+
+    while(ros::ok()){
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
+
+        if(checkLeader == 1){
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
+            leader_object.leader(target);
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 ;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000; 
+            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
+        }
+        
+    }
+    #endif
+
     ros::waitForShutdown();
 
     return 0;
 
 }
+

class_model/src/bio_main4.cpp

@@ -3,6 +3,9 @@
 #include "class_model/follower.h"
 #include "yaml-cpp/yaml.h"
 
+#define changeLeader
+// #define NoChangeLeader
+
 int main(int argc, char **argv) {
 
     // Init ROS node
@@ -21,7 +24,7 @@ int main(int argc, char **argv) {
     ReceiverClass receiver_object;
     
 
-    global_location target,self,M1,M2,M3,M4;
+    global_location target,self,M1,M2,M3,M4,leader;
     int ref_ID,checkLeader,leaderID,leaderIndex,flag;
     int routeNum,routeIndex,OnTarget;
     float errorX,errorY;
@@ -44,6 +47,8 @@ int main(int argc, char **argv) {
 
     global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
     global_location* member[] = {&M1,&M2,&M3,&M4};
+    // global_location* pos[] = {&target,&self,&M1,&M2};
+    // global_location* member[] = {&M1,&M2};
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 
@@ -68,9 +73,11 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
+    #ifdef changeLeader   
+
     while(ros::ok()){
 
-        // type = receiver_object.check_command();
+        type = receiver_object.check_command();
 
             target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
             target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
@@ -95,22 +102,10 @@ int main(int argc, char **argv) {
             M4 = request_object.get_M4_GPS();
 
             if(checkLeader == 1){
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 ;
+
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
+                std::cout << "Drone4 is Leader" << std::endl;
                 leader_object.leader(target);
 
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
-                // leader_object.leader(target);
-
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
-                // 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);
-
             }else{
                 ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
                 follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
@@ -123,22 +118,65 @@ int main(int argc, char **argv) {
             // std::cout <<"y: "<< errorY << std::endl;
         }
 
-        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
                 routeIndex++;
+                checkLeader = 0;
+                std::cout <<"routeIndex: "<<routeIndex<<std::endl;
             // OnTarget = 0;
             }
-        while (routeIndex >= routeNum){
+        // }
-            mode_object.set_Mode("LAND");
+        // type = "x";
-            std::cout <<"LAND"<<std::endl;
+        // while (routeIndex >= routeNum){
-        }
+        //     // mode_object.set_Mode("LAND");
+        //     // std::cout <<"LAND"<<std::endl;
+        // }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
-
-        // mode_object.set_Mode("LAND");
     }
+    #endif
+
+    #ifdef NoChangeLeader
+
+    while(ros::ok()){
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
+
+        if(checkLeader == 1){
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
+            leader_object.leader(target);
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 ;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000; 
+            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
+        }
+        
+    }
+    #endif
+
     ros::waitForShutdown();
 
     return 0;
 
 }
+

class_model/src/bio_main5.cpp

@@ -3,6 +3,9 @@
 #include "class_model/follower.h"
 #include "yaml-cpp/yaml.h"
 
+#define changeLeader
+// #define NoChangeLeader
+
 int main(int argc, char **argv) {
 
     // Init ROS node
@@ -21,7 +24,7 @@ int main(int argc, char **argv) {
     ReceiverClass receiver_object;
     
 
-    global_location target,self,M1,M2,M3,M4;
+    global_location target,self,M1,M2,M3,M4,leader;
     int ref_ID,checkLeader,leaderID,leaderIndex,flag;
     int routeNum,routeIndex,OnTarget;
     float errorX,errorY;
@@ -44,6 +47,8 @@ int main(int argc, char **argv) {
 
     global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
     global_location* member[] = {&M1,&M2,&M3,&M4};
+    // global_location* pos[] = {&target,&self,&M1,&M2};
+    // global_location* member[] = {&M1,&M2};
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 
@@ -68,9 +73,11 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
+    #ifdef changeLeader   
+
     while(ros::ok()){
 
-        // type = receiver_object.check_command();
+        type = receiver_object.check_command();
 
             target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
             target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
@@ -95,22 +102,10 @@ int main(int argc, char **argv) {
             M4 = request_object.get_M4_GPS();
 
             if(checkLeader == 1){
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 ;
+
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
+                std::cout << "Drone5 is Leader" << std::endl;
                 leader_object.leader(target);
 
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
-                // leader_object.leader(target);
-
-                // target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-                // target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
-                // 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);
-
             }else{
                 ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
                 follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
@@ -123,22 +118,66 @@ int main(int argc, char **argv) {
             // std::cout <<"y: "<< errorY << std::endl;
         }
 
-        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
                 routeIndex++;
+                checkLeader = 0;
+                std::cout <<"routeIndex: "<<routeIndex<<std::endl;
             // OnTarget = 0;
             }
-        while (routeIndex >= routeNum){
+        // }
-            mode_object.set_Mode("LAND");
+        // type = "x";
-            std::cout <<"LAND"<<std::endl;
+        // while (routeIndex >= routeNum){
-        }
+        //     // mode_object.set_Mode("LAND");
+        //     // std::cout <<"LAND"<<std::endl;
+        // }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
-
-        // mode_object.set_Mode("LAND");
     }
+    #endif
+
+    #ifdef NoChangeLeader
+
+    while(ros::ok()){
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
+
+        if(checkLeader == 1){
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
+            leader_object.leader(target);
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
+            leader_object.leader(target);
+
+
+            target.lon = config["route1"]["x"].as<float>()*1e7 ;
+            target.lat = config["route1"]["y"].as<float>()*1e7 - 1000; 
+            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
+        }
+        
+    }
+    #endif
+
     ros::waitForShutdown();
 
     return 0;
 
 }
+

class_model/src/config.yaml

@@ -2,15 +2,15 @@ DroneParam:
   ID: 1 
 route1:
   x: 120.6743161             #(24.1218859, 120.6743161)
-  y: 24.1219860
+  y: 24.1220860
   z: 0.0
 route2:
-  x: 120.6744161             #(24.1218859, 120.6743161)
+  x: 120.6746461             #(24.1218859, 120.6743161)
-  y: 24.1219860
+  y: 24.1218860
   z: 0.0
 route3:
-  x: 120.6744161             #(24.1218859, 120.6743161)
+  x: 120.6743161             #(24.1218859, 120.6743161)
-  y: 24.1218859
+  y: 24.1217360
   z: 0.0
 route4:
   x: 120.6743161             #(24.1218859, 120.6743161)

class_model/src/follower.cpp

@@ -62,22 +62,28 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
     ref_point.lon = 5000*sin(phi*3.14/180) + leader->lon + 1;
     ref_point.lat = 5000*cos(phi*3.14/180) + leader->lat;
 
+    #ifdef NoChangeLeader
+    slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon);        // Leader-Ref
+    c = leader->lat - slope*leader->lon;
+    #endif
 
+    #ifdef changeLeader
+    if((target->lat != pre_target.lat) || (target->lon != pre_target.lon)){    // Leader-Target
+        face2target(target,leader);
+        // slope = (target->lat-leader->lat)/(target->lon-leader->lon+1);
+        // c = leader->lat - slope*leader->lon;
+        pre_target.lat = target->lat;
+        pre_target.lon = target->lon;
+        ROS_INFO("plane");
+    }
     slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon);
     c = leader->lat - slope*leader->lon;
-    
+    #endif
-
-    // 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;
 
-    pre_target.lat = target->lat;
-    pre_target.lon = target->lon;
     // 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 && phi < 180) || (follower->lat > y && phi >= 180)){ 
         // std::cout <<"drone :"<<follower->ID <<", plane: 1" <<std::endl;              
@@ -162,41 +168,53 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
 
     // 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;
-
     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;
 
+    #ifdef NoChangeLeader
+    /**************Leader-Ref*****************/
     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
+    k = (vector_LR.lon*vector_SM.lon)+(vector_LR.lat*vector_SM.lat);     //k = vector_LR dot vector_SM
-    m = sqrt(pow(vector_LR.lon,2)+pow(vector_LR.lat,2));     //m = |vector_LT|
+    m = sqrt(pow(vector_LR.lon,2)+pow(vector_LR.lat,2));     //m = |vector_LR|
+    n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2));     //n = |vector_SM|
+    /*******************************************/
+    #endif
+    #ifdef changeLeader
+    /**********Leader-Target***********/
+    vector_LT.lon = target->lon - leader->lon;       //vector_LT (leader->target)     
+    vector_LT.lat = target->lat - leader->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|
     
-    // pre_m = sqrt(pow(pre_LT.lon,2)+pow(pre_LT.lat,2));     
+    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);
+    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))){
+    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);
+        u = (vector_LT.lon*pre_LT.lon)+(vector_LT.lat*pre_LT.lat);
-    //     LTcos = u / (m*pre_m);
+        LTcos = u / (m*pre_m);
-    //     if (LTcos < 0){
+        if (LTcos < 0){
-    //         m = pre_m;
+            m = pre_m;
-    //         k = pre_k;
+            k = pre_k;
             // std::cout <<"memberID: "<<member.ID << " fix" << std::endl;
-    //     }
+        }
         // std::cout <<"dir_test "<< std::endl;
 
-    // }else{
+    }else{
-    //     pre_LT = vector_LT;
+        pre_LT = vector_LT;
-    //     pre_tar.lat = target->lat;
+        pre_tar.lat = target->lat;
-    //     pre_tar.lon = target->lon;
+        pre_tar.lon = target->lon;
 
-    // }
+    }
+    /***************************************/
+    #endif
     
     cosTheta = k / (m*n);
     // std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl;
@@ -280,20 +298,20 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
         j+=1;
         // std::cout << "drone3" <<std::endl;
     }
-    if(drone4.ID == data[1]->ID){
+    // if(drone4.ID == data[1]->ID){
-        self = drone4;
+    //     self = drone4;
-    }else{
+    // }else{
-        memberDrone[j] = drone4;
+    //     memberDrone[j] = drone4;
-        j+=1;
+    //     j+=1;
-        // std::cout << "drone4" <<std::endl;
+    //     // std::cout << "drone4" <<std::endl;
-    }
+    // }
-    if(drone5.ID == data[1]->ID){
+    // if(drone5.ID == data[1]->ID){
-        self = drone5;
+    //     self = drone5;
-    }else{
+    // }else{
-        memberDrone[j] = drone5;
+    //     memberDrone[j] = drone5;
-        j+=1;
+    //     j+=1;
-        // std::cout << "drone4" <<std::endl;
+    //     // std::cout << "drone4" <<std::endl;
-    }   
+    // }   
     
 
     for(int i=0;i<index-2;i++){
@@ -325,7 +343,7 @@ 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
     ref_ID = samePlane[check_index].ID;
 
-    // std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " <<  ref_ID << ",mem_index: "<< s_index << 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;   
 
@@ -361,15 +379,15 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
     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;
     
-    error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error (PID)
+    // error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error (PID)
-    error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
+    // error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
     
 
     // float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] );       //caculate error
     // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
 
-    // error_lon = Pf[0] - (follower_location.lon*1e+5);            //caculate error
+    error_lon = Pf[0] - (follower_location.lon*1e+5);            //caculate error
-    // error_lat = Pf[1] - (follower_location.lat*1e+5);
+    error_lat = Pf[1] - (follower_location.lat*1e+5);
 
     
     float error_degree = deg_phi - heading;
@@ -425,3 +443,35 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
     command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1);
     // command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
 }
+
+void FollowerClass::face2target(global_location* target, global_location* leader){
+
+    float x,y,target_heading;
+    float heading = GPS_object.get_heading();
+    heading_status = 0;
+    error_yaw = 0.2;
+    x = (target->lon/100) - (leader->lon/100);
+    y = (target->lat/100) - (leader->lat/100);
+    target_heading = ConvertDeg(x,y);
+
+    error_heading =  target_heading - heading;
+    error_yaw = check_direction(error_heading)*error_yaw*0.5; 
+
+    if(error_heading < -355 || error_heading > 355){
+        error_heading = 0;
+    }
+
+    // ROS_INFO("x:%f,y:%f,target_heading:%f,error_heading:%f",x,y,target_heading,error_heading); 
+
+    while(heading_status != 1){
+        heading = GPS_object.get_heading();
+        error_heading =  target_heading - heading;
+        if (error_heading < 3 && error_heading > -3 ){
+            error_yaw = 0;
+            heading_status = 1;
+        }
+        command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
+        // ROS_INFO("error_yaw:%f",error_yaw); 
+    }
+
+}

class_model/src/receiver.cpp

@@ -8,7 +8,7 @@ ReceiverClass::ReceiverClass() : node_handle_("~"){
 	}else{
 		node_handle_.getParam("namespace", ros_namespace);
 	}
-    mqtt_sub = node_handle_.subscribe(ros_namespace+"/cmd_receiver",10,&ReceiverClass::cmd_receiver, this);
+    mqtt_sub = node_handle_.subscribe("/cmd_receiver",10,&ReceiverClass::cmd_receiver, this);
 }
 
 ReceiverClass::~ReceiverClass() { ros::shutdown(); }

iq_sim/worlds/three_drone.world

@@ -89,14 +89,14 @@
       </include>
     </model>
     <model name="drone2">
-      <pose> 0 -5 0 0 0 0</pose>
+      <pose> -1 4 0 0 0 0</pose>
       <include>
         <uri>model://drone2</uri>
       </include>
     </model>
 
     <model name="drone3">
-      <pose> 0 5 0 0 0 0</pose>
+      <pose> -1 -4 0 0 0 0</pose>
       <include>
         <uri>model://drone3</uri>
       </include>