change leader when target change

class_model/CMakeLists.txt

@@ -294,7 +294,7 @@ add_dependencies(follower_lib main_generate_messages_cpp)
 
 ##################
 add_executable(main src/main.cpp)
-target_link_libraries(main ${catkin_LIBRARIES} mission_lib PubInfo_lib)
+target_link_libraries(main ${catkin_LIBRARIES} mission_lib PubInfo_lib )
 add_dependencies(main class_model_generate_messages_cpp)
 
 add_executable(bio_main src/bio_main.cpp)

class_model/include/class_model/FindLeader.h

@@ -3,7 +3,7 @@
 #include <iterator> 
 
 typedef struct result{
-    int leader_ID;
+    int leader_ID,leader_index;
     bool is_leader;
 }result;
 
@@ -25,6 +25,12 @@ result check_leader(global_location* data, size_t index){
     check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
     _result.leader_ID = check_ID + 1;
 
+    for(int j=1 ; j<index ;j++){
+        if(_result.leader_ID == data[j].ID){
+            _result.leader_index = j;
+        }
+    }
+    
     // std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
     
     if (data[1].ID == _result.leader_ID){

class_model/include/class_model/Leader.h

@@ -29,6 +29,7 @@ public:
     
     void leader(global_location target);
     void go2target(float x,float y);
+    int isOnTarget();
 
 private:
     // ROS NodeHandle
@@ -41,6 +42,7 @@ private:
     global_location current_location;
     velocity drone_speed;
     int flag = 0,heading_status = 0,buf = 0;
+    int OnTarget = 0;
     float pre_alt,cur_alt;
     float error_lon,error_lat;
     float leader_pid[3] = {1 , 0.000000000001 ,0};

class_model/include/class_model/command.h

@@ -1,7 +1,7 @@
 /*Drone Control Command*/
 #ifndef COMMAND_H
 #define COMMAND_H
-#define DEBUG
+// #define DEBUG
 #include <ros/ros.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <mavros_msgs/GlobalPositionTarget.h>

class_model/include/class_model/follower.h

@@ -1,7 +1,7 @@
 /*Follow the leader in a fixed formation */
 #ifndef FOLLOWER_H
 #define FOLLOWER_H
-
+#define swarmNum 5
 #include <ros/ros.h>
 #include "class_model/sensor.h"
 #include "class_model/mode.h"
@@ -34,7 +34,7 @@ public:
     DroneStatus drone4;
     DroneStatus drone5;
     DroneStatus self;
-    DroneStatus samePlane[5];
+    DroneStatus samePlane[swarmNum];
     
     void follower(global_location** member_data, size_t index, int refID);
     int find_ref_drone(global_location** data, size_t index, int leaderID);
@@ -52,7 +52,7 @@ private:
     global_location leader_location;
     global_location leader_drone;
     global_location refpos;
-    DroneStatus memberDrone[5];
+    DroneStatus memberDrone[swarmNum];
     global_location (RequestClass::*ref)();
 
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");

class_model/include/class_model/formation.h

@@ -1,7 +1,7 @@
 /*Follow the leader in a fixed formation */
 #ifndef FORMATION_H
 #define FORMATION_H
-#define DEBUG
+// #define DEBUG
 #include <ros/ros.h>
 #include "class_model/sensor.h"
 #include "class_model/mode.h"

class_model/src/Leader.cpp

@@ -20,11 +20,13 @@ void LeaderClass::leader(global_location target){
     heading_status = 0;
     while(true){
         if(flag==0){
+            OnTarget = 0;
             go2target(target.lon,target.lat);
         }else{
             // next_command.publish(message);
             pre_target.lon = target.lon;
             pre_target.lat = target.lat;
+            OnTarget = 1;
             ROS_INFO("break");
             break;
         }
@@ -100,14 +102,14 @@ void LeaderClass::go2target(float target_lon,float target_lat){
     }
 
     // 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);
     
     // 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("************************************");
     
 
 
@@ -125,6 +127,10 @@ void LeaderClass::face2target(float target_heading){
         error_yaw = 0;
         heading_status = 1;
     }
-    // command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
+    command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
     // ROS_INFO("face2target...%f",target_heading);
 }
+
+int LeaderClass::isOnTarget(){
+    return OnTarget;
+}

class_model/src/bio_main.cpp

@@ -21,13 +21,18 @@ int main(int argc, char **argv) {
     ReceiverClass receiver_object;
     
 
-    global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,leaderID,flag;
+    global_location target,self,M1,M2,M3,M4,leader;
+    int ref_ID,checkLeader,leaderID,leaderIndex,flag;
+    int routeNum,routeIndex,OnTarget;
+    float errorX,errorY;
     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)
-    target.lon = config["routh1"]["x"].as<float>()*1e7;
-    target.lat = config["routh1"]["y"].as<float>()*1e7;
+    target.lon = config["route1"]["x"].as<float>()*1e7;
+    target.lat = config["route1"]["y"].as<float>()*1e7;
+    routeNum = config["routeNum"]["num"].as<int>();
+
+    std::string route[4]={"route1","route2","route3","route4"};
 
     sleep(5);
     self = request_object.get_self_GPS();
@@ -66,39 +71,67 @@ int main(int argc, char **argv) {
     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["routh1"]["x"].as<float>()*1e7 ;
-            target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
-            leader_object.leader(target);
+            target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
+            target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
 
-            target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-            target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
-            leader_object.leader(target);
+            checkLeader = check_leader(*pos,index).is_leader;
+            leaderID = check_leader(*pos,index).leader_ID;
+            leaderIndex = check_leader(*pos,index).leader_index;
          
-            target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
-            target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
-            leader_object.leader(target);
+            std::cout << leaderID << std::endl;
+            std::cout << target.lon << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
 
-            target.lon = config["routh1"]["x"].as<float>()*1e7 ;
-            target.lat = config["routh1"]["y"].as<float>()*1e7 - 500; 
-            leader_object.leader(target);
+        while(errorX > 100 || errorY > 100){
 
-        }else{
-            ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
-            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
+            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["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 + 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
+            }
+            // OnTarget = leader_object.isOnTarget();
+            // std::cout <<"tar: "<< OnTarget << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
         }
 
+        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+            routeIndex++;
+            // OnTarget = 0;
+        }else if(routeIndex >= routeNum){
+            mode_object.set_Mode("LAND");
+        }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
-
-        
     }
     ros::waitForShutdown();
 

class_model/src/bio_main2.cpp

@@ -22,12 +22,17 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,leaderID,flag;
+    int ref_ID,checkLeader,leaderID,leaderIndex,flag;
+    int routeNum,routeIndex,OnTarget;
+    float errorX,errorY;
     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)
-    target.lon = config["routh1"]["x"].as<float>()*1e7;
-    target.lat = config["routh1"]["y"].as<float>()*1e7;
+    target.lon = config["route1"]["x"].as<float>()*1e7;
+    target.lat = config["route1"]["y"].as<float>()*1e7;
+    routeNum = config["routeNum"]["num"].as<int>();
+
+    std::string route[4]={"route1","route2","route3","route4"};
 
     sleep(5);
     self = request_object.get_self_GPS();
@@ -57,9 +62,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){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }   
@@ -67,24 +71,69 @@ int main(int argc, char **argv) {
     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){
-            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
+            target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
+            target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
+
+            checkLeader = check_leader(*pos,index).is_leader;
+            leaderID = check_leader(*pos,index).leader_ID;
+            leaderIndex = check_leader(*pos,index).leader_index;
+         
+            std::cout << leaderID << std::endl;
+            std::cout << target.lon << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
+
+        while(errorX > 100 || errorY > 100){
+
+            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["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 + 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
+            }
+            // OnTarget = leader_object.isOnTarget();
+            // std::cout <<"tar: "<< OnTarget << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
         }
 
+        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+            routeIndex++;
+            // OnTarget = 0;
+        }else if(routeIndex >= routeNum){
+            mode_object.set_Mode("LAND");
+        }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
 
-        
+        // mode_object.set_Mode("LAND");
     }
     ros::waitForShutdown();
 

class_model/src/bio_main3.cpp

@@ -22,12 +22,17 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,leaderID,flag;
+    int ref_ID,checkLeader,leaderID,leaderIndex,flag;
+    int routeNum,routeIndex,OnTarget;
+    float errorX,errorY;
     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)
-    target.lon = config["routh1"]["x"].as<float>()*1e7;
-    target.lat = config["routh1"]["y"].as<float>()*1e7;
+    target.lon = config["route1"]["x"].as<float>()*1e7;
+    target.lat = config["route1"]["y"].as<float>()*1e7;
+    routeNum = config["routeNum"]["num"].as<int>();
+
+    std::string route[4]={"route1","route2","route3","route4"};
 
     sleep(5);
     self = request_object.get_self_GPS();
@@ -57,7 +62,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){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
@@ -66,24 +71,69 @@ int main(int argc, char **argv) {
     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){
-            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
+            target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
+            target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
+
+            checkLeader = check_leader(*pos,index).is_leader;
+            leaderID = check_leader(*pos,index).leader_ID;
+            leaderIndex = check_leader(*pos,index).leader_index;
+         
+            std::cout << leaderID << std::endl;
+            std::cout << target.lon << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
+
+        while(errorX > 100 || errorY > 100){
+
+            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["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 + 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
+            }
+            // OnTarget = leader_object.isOnTarget();
+            // std::cout <<"tar: "<< OnTarget << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
         }
 
+        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+            routeIndex++;
+            // OnTarget = 0;
+        }else if(routeIndex >= routeNum){
+            mode_object.set_Mode("LAND");
+        }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
 
-        
+        // mode_object.set_Mode("LAND");
     }
     ros::waitForShutdown();
 

class_model/src/bio_main4.cpp

@@ -22,12 +22,17 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,leaderID,flag;
+    int ref_ID,checkLeader,leaderID,leaderIndex,flag;
+    int routeNum,routeIndex,OnTarget;
+    float errorX,errorY;
     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)
-    target.lon = config["routh1"]["x"].as<float>()*1e7;
-    target.lat = config["routh1"]["y"].as<float>()*1e7;
+    target.lon = config["route1"]["x"].as<float>()*1e7;
+    target.lat = config["route1"]["y"].as<float>()*1e7;
+    routeNum = config["routeNum"]["num"].as<int>();
+
+    std::string route[4]={"route1","route2","route3","route4"};
 
     sleep(5);
     self = request_object.get_self_GPS();
@@ -57,7 +62,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){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
@@ -66,24 +71,69 @@ int main(int argc, char **argv) {
     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){
-            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
+            target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
+            target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
+
+            checkLeader = check_leader(*pos,index).is_leader;
+            leaderID = check_leader(*pos,index).leader_ID;
+            leaderIndex = check_leader(*pos,index).leader_index;
+         
+            std::cout << leaderID << std::endl;
+            std::cout << target.lon << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
+
+        while(errorX > 100 || errorY > 100){
+
+            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["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 + 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
+            }
+            // OnTarget = leader_object.isOnTarget();
+            // std::cout <<"tar: "<< OnTarget << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
         }
 
+        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+            routeIndex++;
+            // OnTarget = 0;
+        }else if(routeIndex >= routeNum){
+            mode_object.set_Mode("LAND");
+        }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
 
-        
+        // mode_object.set_Mode("LAND");
     }
     ros::waitForShutdown();
 

class_model/src/bio_main5.cpp

@@ -22,12 +22,17 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,leaderID,flag;
+    int ref_ID,checkLeader,leaderID,leaderIndex,flag;
+    int routeNum,routeIndex,OnTarget;
+    float errorX,errorY;
     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)
-    target.lon = config["routh1"]["x"].as<float>()*1e7;
-    target.lat = config["routh1"]["y"].as<float>()*1e7;
+    target.lon = config["route1"]["x"].as<float>()*1e7;
+    target.lat = config["route1"]["y"].as<float>()*1e7;
+    routeNum = config["routeNum"]["num"].as<int>();
+
+    std::string route[4]={"route1","route2","route3","route4"};
 
     sleep(5);
     self = request_object.get_self_GPS();
@@ -66,24 +71,69 @@ int main(int argc, char **argv) {
     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){
-            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
+            target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
+            target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
+
+            checkLeader = check_leader(*pos,index).is_leader;
+            leaderID = check_leader(*pos,index).leader_ID;
+            leaderIndex = check_leader(*pos,index).leader_index;
+         
+            std::cout << leaderID << std::endl;
+            std::cout << target.lon << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
+
+        while(errorX > 100 || errorY > 100){
+
+            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["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 + 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
+            }
+            // OnTarget = leader_object.isOnTarget();
+            // std::cout <<"tar: "<< OnTarget << std::endl;
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            std::cout <<"x: "<< errorX << std::endl;
+            std::cout <<"y: "<< errorY << std::endl;
         }
 
+        if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
+            routeIndex++;
+            // OnTarget = 0;
+        }else if(routeIndex >= routeNum){
+            mode_object.set_Mode("LAND");
+        }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
         // }
 
-        
+        // mode_object.set_Mode("LAND");
     }
     ros::waitForShutdown();
 

class_model/src/command.cpp

@@ -109,7 +109,7 @@ void CommandClass::set_global_position(double lon,double lat,float alt,float yaw
                               mavros_msgs::GlobalPositionTarget::IGNORE_AFY |
                               mavros_msgs::GlobalPositionTarget::IGNORE_AFZ |
                               mavros_msgs::GlobalPositionTarget::FORCE |
-                              mavros_msgs::GlobalPositionTarget::IGNORE_YAW_RATE;
+                              mavros_msgs::GlobalPositionTarget::IGNORE_YAW_RATE ;
     goal_position.latitude = lat;
     goal_position.longitude = lon;
     goal_position.altitude = alt;

class_model/src/config.yaml

@@ -1,14 +1,25 @@
 DroneParam:
   ID: 1 
-routh1:
+route1:
   x: 120.6743161             #(24.1218859, 120.6743161)
-  y: 24.1219310
+  y: 24.1219860
   z: 0.0
-routh2:
-  x: 120.6743661             #(24.1218859, 120.6743161)
-  y: 24.1219310
+route2:
+  x: 120.6744161             #(24.1218859, 120.6743161)
+  y: 24.1219860
+  z: 0.0
+route3:
+  x: 120.6744161             #(24.1218859, 120.6743161)
+  y: 24.1218859
+  z: 0.0
+route4:
+  x: 120.6743161             #(24.1218859, 120.6743161)
+  y: 24.1218859
   z: 0.0
 formation:
   distance: 4.0
   angle: 45
+routeNum:
+  num: 4
+
   

class_model/src/follower.cpp

@@ -77,7 +77,7 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
     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;              
@@ -325,8 +325,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
     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 <<"\n"<< 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;   
 
 }
@@ -422,5 +422,6 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
     
     // ROS_INFO("drone%d,plane%d",self.ID,plane);
     
-    // command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1);
+    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);
 }

class_model/src/formation.cpp

@@ -560,7 +560,7 @@ void FormationClass::calculate_position(float k,float theta,int direction){
     #endif
 
     #ifndef DEBUG
-    command_object.set_velocity(error_lon ,error_lat ,0,0,0.1);
+    command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
     #endif
     #ifdef DEBUG
     command_object.set_velocity(error_lon ,error_lat ,0,0,0.1,get_data_t,cal_data_t);