gazebo test ok,but PID control weird.

class_model/include/class_model/FindLeader.h

@@ -7,7 +7,7 @@ typedef struct result{
     bool is_leader;
 }result;
 
-result check_leader(global_location data[], size_t index){
+result check_leader(global_location* data, size_t index){
     
     float d[index];
     int check_ID;

class_model/include/class_model/follower.h

@@ -34,7 +34,7 @@ public:
     DroneStatus self;
     DroneStatus samePlane[5];
     
-    void follower(global_location data[], size_t index, int refID);
+    void follower(global_location* member_data, size_t index, int refID);
     int find_ref_drone(global_location data[], size_t index, int leaderID);
 
     float lon[3],lat[3];

class_model/include/class_model/requestData.h

@@ -47,8 +47,7 @@ private:
     std::array <int,3> ID_array {1,2,3};
     // rapidjson::Document document;
     json j_data,j_data2,j_data3;
-    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
-    int self_ID= config["DroneParam"]["ID"].as<int>(),drone_ID;
+    int self_ID,drone_ID;
 
   // SUBSCRIBE
     ros::Subscriber mqtt_data; 

class_model/include/class_model/requestData2.h

@@ -0,0 +1,62 @@
+/*Subscribe Data Which MQTT Pubilsh */
+#ifndef REQUESTDATA_H
+#define REQUESTDATA_H
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <iostream>
+#include <string>
+#include <class_model/gps_location.h>
+#include <nlohmann/json.hpp>
+#include <algorithm>
+#include <ros/message_event.h>
+#include "yaml-cpp/yaml.h"
+
+// #include "rapidjson/document.h"
+
+using json = nlohmann::json;
+
+
+class RequestClass {
+public:
+    RequestClass();
+    virtual ~RequestClass();
+    global_location get_leader_GPS();
+    global_location get_self_GPS();
+    global_location get_M1_GPS();
+    global_location get_M2_GPS();
+    float get_leader_heading();
+    int get_formation_message();
+    json get_data();
+    
+private:
+  // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    void Data_callback(const std_msgs::String::ConstPtr &gps);
+    void drone1_callback(const std_msgs::String::ConstPtr &gps);
+    void drone2_callback(const std_msgs::String::ConstPtr &gps);
+    void drone3_callback(const std_msgs::String::ConstPtr &gps);
+    void Message_callback(const std_msgs::String::ConstPtr &message);
+    void jsonToString(std::string data);
+    void L_INFO(std::string data);
+    void Bio_INFO(json json_data,int drone_ID);
+    float heading;
+    global_location leader_position;
+    global_location self_position,M1_position,M2_position;
+    std::array <int,3> ID_array {1,2,3};
+    // rapidjson::Document document;
+    json j_data,j_data2,j_data3;
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config2.yaml");  //reading yaml parameter(target point)
+    int self_ID= config["DroneParam"]["ID"].as<int>(),drone_ID;
+
+  // SUBSCRIBE
+    ros::Subscriber mqtt_data; 
+    ros::Subscriber formation_data;
+    ros::Subscriber drone1_data;
+    ros::Subscriber drone2_data;
+    ros::Subscriber drone3_data;
+};
+
+#endif // REQUESTDATA_H
+

class_model/include/class_model/requestData3.h

@@ -0,0 +1,62 @@
+/*Subscribe Data Which MQTT Pubilsh */
+#ifndef REQUESTDATA_H
+#define REQUESTDATA_H
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <iostream>
+#include <string>
+#include <class_model/gps_location.h>
+#include <nlohmann/json.hpp>
+#include <algorithm>
+#include <ros/message_event.h>
+#include "yaml-cpp/yaml.h"
+
+// #include "rapidjson/document.h"
+
+using json = nlohmann::json;
+
+
+class RequestClass {
+public:
+    RequestClass();
+    virtual ~RequestClass();
+    global_location get_leader_GPS();
+    global_location get_self_GPS();
+    global_location get_M1_GPS();
+    global_location get_M2_GPS();
+    float get_leader_heading();
+    int get_formation_message();
+    json get_data();
+    
+private:
+  // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    void Data_callback(const std_msgs::String::ConstPtr &gps);
+    void drone1_callback(const std_msgs::String::ConstPtr &gps);
+    void drone2_callback(const std_msgs::String::ConstPtr &gps);
+    void drone3_callback(const std_msgs::String::ConstPtr &gps);
+    void Message_callback(const std_msgs::String::ConstPtr &message);
+    void jsonToString(std::string data);
+    void L_INFO(std::string data);
+    void Bio_INFO(json json_data,int drone_ID);
+    float heading;
+    global_location leader_position;
+    global_location self_position,M1_position,M2_position;
+    std::array <int,3> ID_array {1,2,3};
+    // rapidjson::Document document;
+    json j_data,j_data2,j_data3;
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config3.yaml");  //reading yaml parameter(target point)
+    int self_ID= config["DroneParam"]["ID"].as<int>(),drone_ID;
+
+  // SUBSCRIBE
+    ros::Subscriber mqtt_data; 
+    ros::Subscriber formation_data;
+    ros::Subscriber drone1_data;
+    ros::Subscriber drone2_data;
+    ros::Subscriber drone3_data;
+};
+
+#endif // REQUESTDATA_H
+