change leader when target change

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;
 
@@ -24,6 +24,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;
     

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"