add bio drones into simulation environment(need debugging)

class_model/.vscode/launch.json

@@ -9,7 +9,7 @@
             "name": "ROS: Launch",
             "type": "ros",
             "request": "launch",
-            "target": "/home/dodo/ardupilot_ws/src/class_model/launch/testLib.launch",
+            "target": "/home/dodo/ardupilot_ws/src/class_model/launch/test.launch",
             "launch": [
                 "rviz",
                 "gz",

class_model/.vscode/settings.json

@@ -76,6 +76,7 @@
         "valarray": "cpp",
         "any": "cpp",
         "codecvt": "cpp",
-        "forward_list": "cpp"
+        "forward_list": "cpp",
+        "shared_mutex": "cpp"
     }
 }

class_model/CMakeLists.txt

@@ -242,7 +242,7 @@ add_dependencies(receiver_lib main_generate_messages_cpp)
 
 add_library(requestData_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/requestData.cpp)
 target_include_directories(requestData_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
-target_link_libraries(requestData_lib ${catkin_LIBRARIES})
+target_link_libraries(requestData_lib ${catkin_LIBRARIES} /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
 add_dependencies(requestData_lib main_generate_messages_cpp)
 
 add_library(param_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/Param.cpp)
@@ -264,6 +264,7 @@ add_library(PubInfo_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/PubInfo.cpp)
 target_include_directories(PubInfo_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
 target_link_libraries(PubInfo_lib ${catkin_LIBRARIES})
 add_dependencies(PubInfo_lib main_generate_messages_cpp)
+
 ##################
 
 add_library(formation_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/formation.cpp)
@@ -281,11 +282,34 @@ target_include_directories(mission_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/includ
 target_link_libraries(mission_lib ${catkin_LIBRARIES} select_lib)
 add_dependencies(mission_lib main_generate_messages_cpp)
 
+add_library(Leader_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/Leader.cpp)
+target_include_directories(Leader_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_link_libraries(Leader_lib ${catkin_LIBRARIES} sensor_lib mode_lib control_lib command_lib requestData_lib PID_lib PubData_lib receiver_lib)
+add_dependencies(Leader_lib main_generate_messages_cpp)
+
+add_library(follower_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/follower.cpp)
+target_include_directories(follower_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_link_libraries(follower_lib ${catkin_LIBRARIES} command_lib control_lib mode_lib requestData_lib sensor_lib PID_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a) 
+add_dependencies(follower_lib main_generate_messages_cpp)
+
 ##################
 add_executable(main src/main.cpp)
 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)
+target_link_libraries(bio_main ${catkin_LIBRARIES} follower_lib Leader_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
+add_dependencies(bio_main class_model_generate_messages_cpp)
+
+add_executable(bio_main2 src/bio_main2.cpp)
+target_link_libraries(bio_main2 ${catkin_LIBRARIES} follower_lib Leader_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
+add_dependencies(bio_main2 class_model_generate_messages_cpp)
+
+add_executable(bio_main3 src/bio_main3.cpp)
+target_link_libraries(bio_main3 ${catkin_LIBRARIES} follower_lib Leader_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
+add_dependencies(bio_main3 class_model_generate_messages_cpp)
+
+
 ################json test
 add_executable(json src/json.cpp)
 target_link_libraries(json ${catkin_LIBRARIES} )

class_model/include/class_model/DroneStatus.h

@@ -1,10 +1,14 @@
+#ifndef DRONESTATUS_H
+#define DRONESTATUS_H
+
 #include <std_msgs/String.h>
 #include "class_model/gps_location.h"
-typedef struct DroneStatus
+struct DroneStatus{
-{
     /* data */
     global_location init_location;
     int plane,ID;
     std::string leader;
     std::string DroneName;
-} DroneStatus;
+};
+
+#endif // DRONESTATUS_H

class_model/include/class_model/Leader.h

@@ -0,0 +1,49 @@
+/*Follow the leader in a fixed formation */
+#ifndef Leader_H
+#define Leader_H
+
+#include <ros/ros.h>
+#include "class_model/sensor.h"
+#include "class_model/mode.h"
+#include "class_model/receiver.h"
+#include "class_model/control.h"
+#include "class_model/command.h"
+#include "class_model/requestData.h"
+#include "class_model/PID.h"
+#include "class_model/PubData.h"
+#include <std_msgs/String.h>
+
+class LeaderClass {
+public:
+    LeaderClass();
+    virtual ~LeaderClass();
+        //ClassObject
+    ThreadGPSClass GPS_object;
+    ModeClass mode_object;
+    ReceiverClass receiver_object;
+    CommandClass command_object;
+    RequestClass request_object;
+    PubDataClass PubData_object;
+    PIDClass PID_x;
+    PIDClass PID_y;
+    
+    void leader(global_location target);
+    void go2target(float x,float y);
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    global_location target_location;
+    global_location follower_location;
+    global_location leader_location;
+    global_location current_location;
+    velocity drone_speed;
+    int flag = 0,heading_status = 0,buf = 0;
+    float pre_alt,cur_alt;
+    float error_lon,error_lat;
+    float leader_pid[3] = {1 , 0.000000000001 ,0.5};
+    float ignore_small = 0.50;
+    void face2target(float target_heading);   
+};
+#endif // Leader_H

class_model/include/class_model/follower.h

@@ -7,8 +7,12 @@
 #include "class_model/mode.h"
 #include "class_model/control.h"
 #include "class_model/command.h"
+#include "class_model/receiver.h"
 #include "class_model/requestData.h"
 #include "class_model/DroneStatus.h"
+#include "class_model/PID.h"
+#include "class_model/convert_degree.h"
+#include "yaml-cpp/yaml.h"
 
 
 class FollowerClass {
@@ -19,15 +23,19 @@ public:
     ThreadGPSClass GPS_object;
     ModeClass mode_object;
     ControlClass control_object;
+    ReceiverClass receiver_object;
     CommandClass command_object;
     RequestClass request_object;
+    PIDClass PID_x;
+    PIDClass PID_y;
     DroneStatus drone1;
     DroneStatus drone2;    
     DroneStatus drone3;
     DroneStatus self;
-    DroneStatus samePlane[3];
+    DroneStatus samePlane[5];
     
-    void follower();
+    void follower(global_location data[], size_t index, int refID);
+    int find_ref_drone(global_location data[], size_t index, int leaderID);
 
     float lon[3],lat[3];
     
@@ -40,15 +48,22 @@ private:
     global_location follower_location;
     global_location leader_location;
     global_location leader_drone;
-    DroneStatus memberDrone[];
+    global_location refpos;
+    DroneStatus memberDrone[5];
+    global_location (RequestClass::*ref)();
 
-    void calculate_position(float k,float theta);
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
+    float distance = config["formation"]["distance"].as<float>();
+    float angle = config["formation"]["angle"].as<float>(); 
+    float error_lon,error_lat;
+    float follower_pid[3] = {1 , 0.00000000001 ,0.5};
+    float ignore_small = 0.50;
+
+    void calculate_position(float k,float theta,int plane);
     void plane(global_location target, global_location leader, global_location follower);
-    void find_ref_drone(global_location data[], size_t index, int leaderID);
     int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member);
     void choose_leader();
-    void follow();
+
-    void get_location();
 
 };
 

class_model/include/class_model/requestData.h

@@ -4,10 +4,14 @@
 
 #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;
@@ -30,28 +34,28 @@ private:
     ros::NodeHandle node_handle_;
 
     void Data_callback(const std_msgs::String::ConstPtr &gps);
-    void self_callback(const std_msgs::String::ConstPtr &gps);
+    void drone1_callback(const std_msgs::String::ConstPtr &gps);
-    void M1_callback(const std_msgs::String::ConstPtr &gps);
+    void drone2_callback(const std_msgs::String::ConstPtr &gps);
-    void M2_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(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 {1,2,3};
+    std::array <int,3> ID_array {1,2,3};
     // rapidjson::Document document;
-    json j_data;
+    json j_data,j_data2,j_data3;
-    int self_ID,drone_ID;
+    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;
 
   // SUBSCRIBE
     ros::Subscriber mqtt_data; 
     ros::Subscriber formation_data;
-    ros::Subscriber self_data;
+    ros::Subscriber drone1_data;
-    ros::Subscriber member1_data;
+    ros::Subscriber drone2_data;
-    ros::Subscriber member2_data;
+    ros::Subscriber drone3_data;
-
 };
 
 #endif // REQUESTDATA_H

class_model/src/bio_main.cpp

@@ -22,14 +22,12 @@ int main(int argc, char **argv) {
     // SelectionClass selection_object;
 
     global_location target,self,M1,M2;
-    float distance,angle;
     int ref_ID,checkLeader;
     
     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>();
     target.lat = config["routh1"]["y"].as<float>();
-    distance = config["formation"]["distance"].as<float>();
+
-    angle = config["formation"]["angle"].as<float>();
 
     self = request_object.get_self_GPS();
     M1 = request_object.get_M1_GPS();
@@ -40,9 +38,9 @@ int main(int argc, char **argv) {
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 
-    mode_object.set_Mode("GUIDED");
+    // mode_object.set_Mode("GUIDED");
-    control_object.arm();
+    // control_object.arm();
-    control_object.takeoff(2);
+    // control_object.takeoff(2);
 
     checkLeader = check_leader(pos,index).is_leader;
     if(checkLeader != 1){
@@ -51,12 +49,14 @@ int main(int argc, char **argv) {
 
     while(true){
         //
-        if(checkLeader == 1){
+        // if(checkLeader == 1){
-            leader_object.leader(target);
+        //     leader_object.leader(target);
-        }else{
+        // }else{
-               //follow reference drone position
+        //     follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
-        }
+        // }
     }
+    ros::waitForShutdown();
 
+    return 0;
 
 }

class_model/src/bio_main2.cpp

@@ -0,0 +1,62 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "drone2_node");
+ 
+    // reate Assync spiner
+    ros::AsyncSpinner spinner(0);
+	spinner.start();
+    // ros::Rate rate(5);   
+
+    ModeClass mode_object;
+    ControlClass control_object;
+    RequestClass request_object;
+    FollowerClass follower_object;
+    LeaderClass leader_object;
+    
+    // SelectionClass selection_object;
+
+    global_location target,self,M1,M2;
+    int ref_ID,checkLeader;
+    
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config2.yaml");  //reading yaml parameter(target point)
+    target.lon = config["routh1"]["x"].as<float>();
+    target.lat = config["routh1"]["y"].as<float>();
+
+
+    self = request_object.get_self_GPS();
+    M1 = request_object.get_M1_GPS();
+    M2 = request_object.get_M2_GPS();
+
+    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]);
+
+    // mode_object.set_Mode("GUIDED");
+    // control_object.arm();
+    // control_object.takeoff(2);
+
+    checkLeader = check_leader(pos,index).is_leader;
+    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); 
+    }
+
+    while(true){
+        //
+        // if(checkLeader == 1){
+        //     leader_object.leader(target);
+        // }else{
+        //     follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
+        // }
+    }
+    ros::waitForShutdown();
+
+    return 0;
+
+}

class_model/src/bio_main3.cpp

@@ -0,0 +1,62 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "drone3_node");
+ 
+    // reate Assync spiner
+    ros::AsyncSpinner spinner(0);
+	spinner.start();
+    // ros::Rate rate(5);   
+
+    ModeClass mode_object;
+    ControlClass control_object;
+    RequestClass request_object;
+    FollowerClass follower_object;
+    LeaderClass leader_object;
+    
+    // SelectionClass selection_object;
+
+    global_location target,self,M1,M2;
+    int ref_ID,checkLeader;
+    
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config3.yaml");  //reading yaml parameter(target point)
+    target.lon = config["routh1"]["x"].as<float>();
+    target.lat = config["routh1"]["y"].as<float>();
+
+
+    self = request_object.get_self_GPS();
+    M1 = request_object.get_M1_GPS();
+    M2 = request_object.get_M2_GPS();
+
+    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]);
+
+    // mode_object.set_Mode("GUIDED");
+    // control_object.arm();
+    // control_object.takeoff(2);
+
+    checkLeader = check_leader(pos,index).is_leader;
+    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); 
+    }
+
+    while(true){
+        //
+        // if(checkLeader == 1){
+        //     leader_object.leader(target);
+        // }else{
+        //     follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
+        // }
+    }
+    ros::waitForShutdown();
+
+    return 0;
+
+}

class_model/src/config.yaml

@@ -1,8 +1,8 @@
 DroneParam:
   ID: 1 
 routh1:
-  x: 1.0
+  x: 120.6743661              #(24.1218859, 120.6743161)
-  y: 1.5
+  y: 24.1219359
   z: 0.0
 formation:
   distance: 4.0

class_model/src/config2.yaml

@@ -0,0 +1,10 @@
+DroneParam:
+  ID: 2 
+routh1:
+  x: 120.6743661              #(24.1218859, 120.6743161)
+  y: 24.1219359
+  z: 0.0
+formation:
+  distance: 4.0
+  angle: 45
+  

class_model/src/config3.yaml

@@ -0,0 +1,10 @@
+DroneParam:
+  ID: 3 
+routh1:
+  x: 120.6743661              #(24.1218859, 120.6743161)
+  y: 24.1219359
+  z: 0.0
+formation:
+  distance: 4.0
+  angle: 45
+  

class_model/src/follower.cpp

@@ -14,7 +14,6 @@ FollowerClass::~FollowerClass() { ros::shutdown(); }
 
 void FollowerClass::follower(global_location data[], size_t index, int refID){
 
-    global_location (RequestClass::*ref)(); 
     if(refID == data[0].ID){
         ref = &RequestClass::get_M1_GPS;
     }else if (refID == data[1].ID)
@@ -22,12 +21,18 @@ void FollowerClass::follower(global_location data[], size_t index, int refID){
         ref = &RequestClass::get_M2_GPS;
     }
 
-    while(true){
+    std::string command = "";
-
+    std::string pre_command = receiver_object.check_command();
-    refpos = (request_object.*ref)();
     
+    while(true){
 
+    calculate_position(distance,angle,self.plane);
+    command = receiver_object.check_command();
 
+    if(command != pre_command ){
+        ROS_INFO("change formation");
+        break;
+    }
     
     }
 }
@@ -150,28 +155,28 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
     }else{
         memberDrone[j] = drone1;
         j+=1;
-        std::cout << "drone1" <<std::endl;
+        // std::cout << "drone1" <<std::endl;
     }
     if(drone2.ID == data[1].ID){
         self = drone2;
     }else{
         memberDrone[j] = drone2;
         j+=1;
-        std::cout << "drone2" <<std::endl;
+        // std::cout << "drone2" <<std::endl;
     }
     if(drone3.ID == data[1].ID){
         self = drone3;
     }else{
         memberDrone[j] = drone3;
         j+=1;
-        std::cout << "drone3" <<std::endl;
+        // std::cout << "drone3" <<std::endl;
     }
 
     for(int i=0;i<index-2;i++){
         if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){    //load same plane member
             samePlane[s_index] = memberDrone[i];
-            std::cout << samePlane[s_index].init_location.lat <<std::endl;
+            // std::cout << samePlane[s_index].init_location.lat <<std::endl;
-            std::cout << memberDrone[i].ID <<std::endl;
+            // std::cout << memberDrone[i].ID <<std::endl;
             s_index++;
         }
 
@@ -181,14 +186,14 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
     // int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
     float d[s_index];
     int dir,check_index,ref_ID;
-    std::cout <<"s_index " <<s_index <<std::endl;
+    // std::cout <<"s_index " <<s_index <<std::endl;
     for(int i=0 ; i<s_index ; i++){                 //caculate each same plane mamber's distance
 
         dir = direct(data[0],leader_drone,self,samePlane[i]);
         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 << i<<"," <<d[i] <<std::endl;
-        std::cout << samePlane[i].ID <<std::endl;
+        // std::cout << samePlane[i].ID <<std::endl;
     }
 
     std::vector<float> _d(d,d+s_index); 
@@ -196,49 +201,97 @@ 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 <<"refID " <<  ref_ID << std::endl;
+    std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " <<  ref_ID << std::endl;
 
     return ref_ID;   
 
 }
 
 
-// void FollowerClass::calculate_position(float k,float theta){
 
-//     theta = theta*3.14/180;
+void FollowerClass::calculate_position(float k,float theta,int plane){
-//     float phi = request_object.get_leader_heading()/100;
+
-//     phi = phi*3.14/180;
+    refpos = (request_object.*ref)();    //get reference drone data
-//     leader_location=request_object.get_leader_GPS();
+    float deg_phi = refpos.heading/100; 
-//     follower_location=GPS_object.gps_position();
+    
+    float heading = GPS_object.get_heading();
+    
+    float phi = deg_phi*3.14/180;             //degree-->radian
+    theta = theta*3.14/180;
+
+    leader_location=request_object.get_leader_GPS();           //get Leader/Follower 's GPS         
+    refpos = (request_object.*ref)();
+
+
+    double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};       //transfer maxtrix  
+    float transfer[2][2]={
+                            cos(phi),-sin(phi),
+                            sin(phi),cos(phi)
+                        };
+    float Q[2]={k*sin(theta),k*cos(theta)};
+    float T[2]={1,-1};
+
+    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_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
     
-//     double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};
-//     float transfer[2][2]={
-//                             cos(phi),-sin(phi),
-//                             sin(phi),cos(phi)
-//                         };
-//     float Q[2]={k*sin(theta),k*cos(theta)};
-//     float T[2]={1,-1};
 
-//     Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
+    // float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] );       //caculate error
-//     Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
+    // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
 
-//     float error_x = Pf[0] - (follower_location.lon*1e+5);
+    // error_lon = Pf[0] - (follower_location.lon*1e+5);            //caculate error
-//     float error_y = Pf[1] - (follower_location.lat*1e+5);
+    // error_lat = Pf[1] - (follower_location.lat*1e+5);
 
-//     if (error_x < 0.3 & error_x > -0.3){
-//         error_x = 0;
-//     }
-//     if (error_y < 0.3 & error_y > -0.3){
-//         error_y = 0;
-//     }
     
-//     // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
+    float error_degree = deg_phi - heading + plane;
-//     ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
+    float error_yaw = 0.2;   //CCW
-//     ROS_INFO("%f,%f",Pf[0],Pf[1]);
+
-//     ROS_INFO("%f,%f",error_x,error_y);
+    if (error_degree >= 360){
-//     ROS_INFO("************************************");
+        error_degree -= 360;
+    }
+    if (error_degree <= -360){
+        error_degree += 360;
+    }
 
-//     command_object.fix_velocity(error_x,error_y,0,0,0.1);
+    error_yaw = check_direction(error_degree)*error_yaw;              //check yaw direction
-//     // sleep(0.5);
 
-// }
+    if (error_lon < ignore_small && error_lon > -ignore_small){                     //ignore small errors                  
+        error_lon = 0;
+    }
+    if (error_lat < ignore_small && error_lat > -ignore_small){
+        error_lat = 0;
+    }
+    if (error_degree < 5 && error_degree > -5 ){
+        error_yaw = 0;
+    }else if(error_degree >355 || error_degree < -355){
+        error_yaw = 0;
+    }
+
+    // if (error_lon < -1){                                      
+    //     error_lon = -1;
+    // }
+    // if (error_lon > 1){                                      
+    //     error_lon = 1;
+    // }
+    // if (error_lat < -1){
+    //     error_lat = -1;
+    // }    
+    // if (error_lat > 1){
+    //     error_lat = 1;
+    // }
+
+    // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
+    // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
+    // ROS_INFO("%f,%f",Pf[0],Pf[1]);
+    // // ROS_INFO("%f,%f",error_x,error_y);
+    // ROS_INFO("%f,%f",error_lon,error_lat);
+    // // ROS_INFO("deg:%f",deg_phi);
+    // // ROS_INFO("heading:%f",heading);
+    // // ROS_INFO("error_degree:%f",error_degree);
+    // // ROS_INFO("error_yaw:%f",error_yaw);
+    // ROS_INFO("************************************");
+
+    command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3);
+}

class_model/src/mqttPubMain2.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import paho.mqtt.client as mqtt
+import os
+import sys
+import time
+import utils
+import proto.flight_information_pb2 as flight_information_pb2
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import logging
+
+
+import rospy
+from std_msgs.msg import String
+from std_msgs.msg import Float64
+from std_msgs.msg import Header
+from mavros_msgs.srv import ParamGet
+from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import Imu
+from sensor_msgs.msg import Range
+from geometry_msgs.msg import Vector3
+
+
+
+
+def init_dataFormat(cfg:utils.Read_PUB_Config):
+    ros_namespace="/drone2"
+    if cfg.msg_format == "Proto":
+        utils.Proto_msg_from_ros.client = client
+        utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
+        utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
+        utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
+        utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
+        utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps)
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg)
+    elif cfg.msg_format == "Json":
+        utils.Json_msg_from_ros.client = client
+        utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
+        utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
+        utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
+    else:
+        logging.debug("msg_format not found")
+
+
+def on_connect(self, userdata, flags, rc):
+    logger.info("Connected with result code " + str(rc))
+
+def on_publish(self, userdata, mid):
+    logger.debug("pub success")
+
+def on_disconnect(client, userdata, rc):
+    logger.debug("disconnecting reason  "  +str(rc))
+    client.connected_flag=False
+    client.disconnect_flag=True
+
+if __name__ == '__main__':
+    # Read Config
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB2.yml")
+    cfg = utils.Read_PUB_Config(FilePath)
+    client = utils.MQTTClient(cfg.MQTTClientNamePub)
+
+    # set log
+    log_format = "%(asctime)s - %(levelname)s - %(message)s"
+    formatter = logging.Formatter(log_format)
+
+    stream_handler = logging.StreamHandler()
+    stream_handler.setFormatter(formatter)
+    stream_handler.setLevel(logging.DEBUG)
+
+    file_handler = logging.FileHandler(cfg.logFileName)
+    file_handler.setFormatter(formatter)
+    file_handler.setLevel(logging.INFO)
+
+    logger = logging.getLogger(__name__)
+    logger.setLevel(logging.INFO)
+    logger.addHandler(file_handler)
+    logger.addHandler(stream_handler)
+    logger.info(cfg)
+    
+    # Mqtt
+    client.on_connect = on_connect
+    client.on_publish = on_publish
+    client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
+    client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
+    client.loop_start()
+
+    # Ros
+    rosClient = cfg.ROSClientNamePub
+    rospy.init_node(rosClient)
+    
+    # init data format
+    init_dataFormat(cfg)
+
+    try:
+        rospy.spin()
+    except BaseException as error:
+        client.disconnect()
+        logger.info("End of program")
+        sys.exit()
+     

class_model/src/mqttPubMain3.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import paho.mqtt.client as mqtt
+import os
+import sys
+import time
+import utils
+import proto.flight_information_pb2 as flight_information_pb2
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import logging
+
+
+import rospy
+from std_msgs.msg import String
+from std_msgs.msg import Float64
+from std_msgs.msg import Header
+from mavros_msgs.srv import ParamGet
+from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import Imu
+from sensor_msgs.msg import Range
+from geometry_msgs.msg import Vector3
+
+
+
+
+def init_dataFormat(cfg:utils.Read_PUB_Config):
+    ros_namespace="/drone3"
+    if cfg.msg_format == "Proto":
+        utils.Proto_msg_from_ros.client = client
+        utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
+        utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
+        utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
+        utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
+        utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps)
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg)
+    elif cfg.msg_format == "Json":
+        utils.Json_msg_from_ros.client = client
+        utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
+        utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
+        utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
+    else:
+        logging.debug("msg_format not found")
+
+
+def on_connect(self, userdata, flags, rc):
+    logger.info("Connected with result code " + str(rc))
+
+def on_publish(self, userdata, mid):
+    logger.debug("pub success")
+
+def on_disconnect(client, userdata, rc):
+    logger.debug("disconnecting reason  "  +str(rc))
+    client.connected_flag=False
+    client.disconnect_flag=True
+
+if __name__ == '__main__':
+    # Read Config
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB3.yml")
+    cfg = utils.Read_PUB_Config(FilePath)
+    client = utils.MQTTClient(cfg.MQTTClientNamePub)
+
+    # set log
+    log_format = "%(asctime)s - %(levelname)s - %(message)s"
+    formatter = logging.Formatter(log_format)
+
+    stream_handler = logging.StreamHandler()
+    stream_handler.setFormatter(formatter)
+    stream_handler.setLevel(logging.DEBUG)
+
+    file_handler = logging.FileHandler(cfg.logFileName)
+    file_handler.setFormatter(formatter)
+    file_handler.setLevel(logging.INFO)
+
+    logger = logging.getLogger(__name__)
+    logger.setLevel(logging.INFO)
+    logger.addHandler(file_handler)
+    logger.addHandler(stream_handler)
+    logger.info(cfg)
+    
+    # Mqtt
+    client.on_connect = on_connect
+    client.on_publish = on_publish
+    client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
+    client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
+    client.loop_start()
+
+    # Ros
+    rosClient = cfg.ROSClientNamePub
+    rospy.init_node(rosClient)
+    
+    # init data format
+    init_dataFormat(cfg)
+
+    try:
+        rospy.spin()
+    except BaseException as error:
+        client.disconnect()
+        logger.info("End of program")
+        sys.exit()
+     

class_model/src/mqttSubMain2.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import paho.mqtt.client as mqtt
+import os
+import sys
+import time
+import utils
+import proto.flight_information_pb2 as flight_information_pb2
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import rospy
+from std_msgs.msg import String
+import logging
+
+
+def init_dataFormat(cfg:utils.Read_SUB_Config):
+    if cfg.msg_format == "Proto":
+        utils.Proto_msg_to_ros.rate = rospy.Rate(10)
+        utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
+        
+        utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
+        utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
+        
+        client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Proto_msg_to_ros.on_message_Flight_Information)
+        client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Proto_msg_to_ros.on_message_Fly_Formation)
+
+        utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
+        utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
+    elif cfg.msg_format == "Json":
+        # set ros publisher
+        utils.Json_msg_to_ros.rate = rospy.Rate(10)
+        utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
+        # set callback to each topic
+        client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
+        client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
+        client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
+        client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
+    else:
+        logger.debug("msg_format not found")
+
+
+def on_connect(self, userdata, flags, rc):
+    logger.info("Connected with result code " + str(rc))
+    client.subscribe(cfg.Fly_Formation_topicToMqtt)
+    client.subscribe(cfg.Drone550_Flight_Information_topicToMqtt)
+    client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
+    client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
+
+def on_disconnect(client, userdata, rc):
+    logger.info("disconnecting reason  "  +str(rc))
+    client.connected_flag=False
+    client.disconnect_flag=True
+
+def on_publish(self, userdata, mid):
+    pass
+
+
+if __name__ == '__main__':
+    # Read Config
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB2.yml")
+    cfg = utils.Read_SUB_Config(FilePath)
+    client = utils.MQTTClient(cfg.MQTTClientNameSub)
+    
+    # set log
+    log_format = "%(asctime)s - %(levelname)s - %(message)s"
+    formatter = logging.Formatter(log_format)
+
+    stream_handler = logging.StreamHandler()
+    stream_handler.setFormatter(formatter)
+    stream_handler.setLevel(logging.DEBUG)
+
+
+    file_handler = logging.FileHandler(cfg.logFileName)
+    file_handler.setFormatter(formatter)
+    file_handler.setLevel(logging.INFO)
+
+    logger = logging.getLogger(__name__)
+    logger.setLevel(logging.DEBUG)
+    logger.addHandler(file_handler)
+    logger.addHandler(stream_handler)
+
+    logger.info(cfg)
+      
+    # Mqtt
+    client = utils.MQTTClient(cfg.MQTTClientNameSub)
+    client.on_connect = on_connect
+    client.on_publish = on_publish
+    client.on_disconnect = on_disconnect
+    client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
+    
+
+    # Ros
+    rospy.init_node(cfg.ROSClientNameSub)
+    # initialize
+    init_dataFormat(cfg)
+
+    try:
+        client.loop_start()
+        rospy.spin()
+    except BaseException as error:
+        client.loop_stop()
+        client.disconnect()
+        logger.info("End of program")
+        sys.exit(0)

class_model/src/mqttSubMain3.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import paho.mqtt.client as mqtt
+import os
+import sys
+import time
+import utils
+import proto.flight_information_pb2 as flight_information_pb2
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import rospy
+from std_msgs.msg import String
+import logging
+
+
+def init_dataFormat(cfg:utils.Read_SUB_Config):
+    if cfg.msg_format == "Proto":
+        utils.Proto_msg_to_ros.rate = rospy.Rate(10)
+        utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
+        
+        utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
+        utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
+        
+        client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Proto_msg_to_ros.on_message_Flight_Information)
+        client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Proto_msg_to_ros.on_message_Fly_Formation)
+
+        utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
+        utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
+    elif cfg.msg_format == "Json":
+        # set ros publisher
+        utils.Json_msg_to_ros.rate = rospy.Rate(10)
+        utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
+        # set callback to each topic
+        client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
+        client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
+        client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
+        client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
+    else:
+        logger.debug("msg_format not found")
+
+
+def on_connect(self, userdata, flags, rc):
+    logger.info("Connected with result code " + str(rc))
+    client.subscribe(cfg.Fly_Formation_topicToMqtt)
+    client.subscribe(cfg.Drone550_Flight_Information_topicToMqtt)
+    client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
+    client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
+
+def on_disconnect(client, userdata, rc):
+    logger.info("disconnecting reason  "  +str(rc))
+    client.connected_flag=False
+    client.disconnect_flag=True
+
+def on_publish(self, userdata, mid):
+    pass
+
+
+if __name__ == '__main__':
+    # Read Config
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB3.yml")
+    cfg = utils.Read_SUB_Config(FilePath)
+    client = utils.MQTTClient(cfg.MQTTClientNameSub)
+    
+    # set log
+    log_format = "%(asctime)s - %(levelname)s - %(message)s"
+    formatter = logging.Formatter(log_format)
+
+    stream_handler = logging.StreamHandler()
+    stream_handler.setFormatter(formatter)
+    stream_handler.setLevel(logging.DEBUG)
+
+
+    file_handler = logging.FileHandler(cfg.logFileName)
+    file_handler.setFormatter(formatter)
+    file_handler.setLevel(logging.INFO)
+
+    logger = logging.getLogger(__name__)
+    logger.setLevel(logging.DEBUG)
+    logger.addHandler(file_handler)
+    logger.addHandler(stream_handler)
+
+    logger.info(cfg)
+      
+    # Mqtt
+    client = utils.MQTTClient(cfg.MQTTClientNameSub)
+    client.on_connect = on_connect
+    client.on_publish = on_publish
+    client.on_disconnect = on_disconnect
+    client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
+    
+
+    # Ros
+    rospy.init_node(cfg.ROSClientNameSub)
+    # initialize
+    init_dataFormat(cfg)
+
+    try:
+        client.loop_start()
+        rospy.spin()
+    except BaseException as error:
+        client.loop_stop()
+        client.disconnect()
+        logger.info("End of program")
+        sys.exit(0)

class_model/src/requestData.cpp

@@ -1,25 +1,25 @@
 #include"class_model/requestData.h"
 #include <cstdlib>
-#include <iostream>
+
 
 int command;
 // json j_data;   //Don't set json as global variable
 
-RequestClass::RequestClass() : node_handle_(""){
+RequestClass::RequestClass() : node_handle_("~"){
 
   // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
   //                                  &RequestClass::Data_callback, this);
   formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
                                    &RequestClass::Message_callback, this);
-  mqtt_data = node_handle_.subscribe("/uav_message", 10,
+  // mqtt_data = node_handle_.subscribe("/uav_message", 10,
-                                   &RequestClass::Data_callback, this);
+  //                                  &RequestClass::Data_callback, this);
-
+
-  self_data = node_handle_.subscribe("/self_data_message", 100,
+  drone1_data = node_handle_.subscribe("/Dron650_Flight_Information_reciver", 10,
-                                   &RequestClass::self_callback, this);       //test bionic
+                                   &RequestClass::drone1_callback, this);       //test bionic
-  member1_data = node_handle_.subscribe("/member1_data_message", 100,
+  drone2_data = node_handle_.subscribe("/Dron550_Flight_Information_reciver", 10,
-                                   &RequestClass::M1_callback, this);
+                                   &RequestClass::drone2_callback, this);
-  member2_data = node_handle_.subscribe("/member2_data_message", 100,
+  drone3_data = node_handle_.subscribe("/Dron380_Flight_Information_reciver", 10,
-                                   &RequestClass::M2_callback, this);
+                                   &RequestClass::drone3_callback, this);
 }
 
 RequestClass::~RequestClass() { ros::shutdown(); }
@@ -79,12 +79,12 @@ void RequestClass::L_INFO(std::string data){
   // alt = j_data["alt"];
   // degree = j_data["heading"];
   
-  // leader_position.lat=j_data["lat"];       //type error ,need number
+  leader_position.lat = std::stoi(j_data["lat"].get<std::string>());      
-	// leader_position.lon=j_data["lon"];
+	leader_position.lon = std::stoi(j_data["lon"].get<std::string>());
-  // leader_position.alt=j_data["alt"];
+  // leader_position.alt = std::stoi(j_data["alt"].get<std::string>()); 
-  // heading = j_data["heading"];
+  heading = std::stoi(j_data["heading"].get<std::string>()); 
   
-  std::cout <<"Json: " <<j_data << std::endl;
+  // std::cout <<"Json: " <<j_data << std::endl;
   // std::cout << leader_position.lat << std::endl;
   // std::cout << leader_position.lon << std::endl;
   // std::cout << leader_position.alt << std::endl;
@@ -92,38 +92,43 @@ void RequestClass::L_INFO(std::string data){
 }
 
 /*test biomic*/
-void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
+void RequestClass::drone1_callback(const std_msgs::String::ConstPtr &sensor) {
-  Bio_INFO(sensor->data);
+  drone_ID = 1;
+  j_data = json::parse(sensor->data);        //open source
+  // std::cout <<"Json: " <<j_data["lat"] << std::endl;
+  Bio_INFO(j_data,drone_ID);
 }
-void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
+void RequestClass::drone2_callback(const std_msgs::String::ConstPtr &sensor) {
-  Bio_INFO(sensor->data);
+  drone_ID = 2;
+  j_data2 = json::parse(sensor->data);        //open source
+  // std::cout <<"Json: " <<j_data2["lat"] << std::endl;
+  Bio_INFO(j_data2,drone_ID);
 }
-void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
+void RequestClass::drone3_callback(const std_msgs::String::ConstPtr &sensor) {
-  Bio_INFO(sensor->data);
+  drone_ID = 3;
+  j_data3 = json::parse(sensor->data); 
+  Bio_INFO(j_data3,drone_ID);
 }
 
-void RequestClass::Bio_INFO(std::string data){
+void RequestClass::Bio_INFO(json json_data,int drone_ID){
-
-  j_data = json::parse(data);        //open source
 
-  drone_ID = j_data["ID"];
+  std::remove(ID_array.begin(),ID_array.end(),self_ID);
-  std::remove(ID.begin(),ID.end(),self_ID);
 
   if (drone_ID == self_ID){
-    self_position.lat=j_data["lat"];   
+    self_position.lat = json_data["lat"]; 
-	  self_position.lon=j_data["lon"];
+	  self_position.lon = json_data["lon"]; 
-    self_position.alt=j_data["alt"];
+    // self_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
-    self_position.heading = j_data["heading"];
+    self_position.heading = json_data["heading"]; 
-  }else if (drone_ID == ID[0]){
+  }else if (drone_ID == ID_array[0]){
-    M1_position.lat=j_data["lat"];   
+    M1_position.lat = json_data["lat"];   
-	  M1_position.lon=j_data["lon"];
+	  M1_position.lon = json_data["lon"]; 
-    M1_position.alt=j_data["alt"];
+    // M1_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
-    M1_position.heading = j_data["heading"];
+    M1_position.heading = json_data["heading"];
-  }else if (drone_ID == ID[1]){
+  }else if (drone_ID == ID_array[1]){
-    M2_position.lat=j_data["lat"];   
+    M2_position.lat = json_data["lat"]; 
-	  M2_position.lon=j_data["lon"];
+	  M2_position.lon = json_data["lon"]; 
-    M2_position.alt=j_data["alt"];
+    // M2_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
-    M2_position.heading = j_data["heading"];
+    M2_position.heading = json_data["heading"];
   }
 
 }

class_model/src/test_lib.cpp

@@ -1,3 +1,6 @@
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <sensor_msgs/Imu.h>
 #include <iostream>
 #include "yaml-cpp/yaml.h"
 #include "class_model/gps_location.h"
@@ -220,19 +223,18 @@ void find_ref_drone(global_location data[], size_t index, int leaderID){
 }
 
 
-
 int main(int argc,char** argv){
 
-    // YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
 
-    // cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
+    cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
-    // cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
+    cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
 
-    // int ID[3] = {1,2,3};
+    int ID[3] = {1,2,3};
-    // int b = 1;
+    int b = 1;
-    // std::array <int,3> a {1,2,3};
+    std::array <int,3> a {1,2,3};
-    // remove(a.begin(),a.end(),b);
+    remove(a.begin(),a.end(),b);
-    // std::cout << a[0]<<a[1] <<std::endl;
+    std::cout << a[0]<<a[1] <<std::endl;
 
     global_location self;
     global_location M1;

class_model/src/utils/mqttConfig_PUB.yml

@@ -1,22 +1,22 @@
 MQTT:
   msg_format: Json
-  MQTTClientNamePub: Drone380Pub
+  MQTTClientNamePub: Drone650Pub
   host: 140.120.31.133
   port: 1883
   keepalive: 60
   willTopic: CheckDoneConnect
   willTopicQOS: 1
-  lwt: Dron550 Gone Offline
+  lwt: Dron650 Gone Offline
   willRetain: False
   # Mqtt topic
-  Flight_Information_topicToMqtt: Drone380/Flight_Information
+  Flight_Information_topicToMqtt: Drone650/Flight_Information
-  Fly_Formation_topicToMqtt: Drone380/Formation
+  Fly_Formation_topicToMqtt: Drone650/Formation
   # Change formate qos
   Fly_Formation_topicToMqtt_QOS: 2
   #ROS
 ROS:
-  ROSClientNamePub: Drone380Pub
+  ROSClientNamePub: Drone650Pub
-  ROStopicName_Flight_Information: Flight_Information_reciver
+  ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
   ROStopicName_Fly_Formation: Fly_Formation_reciver
 LOG:
   logFileName: pub.log

class_model/src/utils/mqttConfig_PUB2.yml

@@ -0,0 +1,22 @@
+MQTT:
+  msg_format: Json
+  MQTTClientNamePub: Drone550Pub
+  host: 140.120.31.133
+  port: 1883
+  keepalive: 60
+  willTopic: CheckDoneConnect
+  willTopicQOS: 1
+  lwt: Drone550 Gone Offline
+  willRetain: False
+  # Mqtt topic
+  Flight_Information_topicToMqtt: Drone550/Flight_Information
+  Fly_Formation_topicToMqtt: Drone550/Formation
+  # Change formate qos
+  Fly_Formation_topicToMqtt_QOS: 2
+  #ROS
+ROS:
+  ROSClientNamePub: Drone550Pub
+  ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
+  ROStopicName_Fly_Formation: Fly_Formation_reciver
+LOG:
+  logFileName: pub.log

class_model/src/utils/mqttConfig_PUB3.yml

@@ -0,0 +1,22 @@
+MQTT:
+  msg_format: Json
+  MQTTClientNamePub: Drone380Pub
+  host: 140.120.31.133
+  port: 1883
+  keepalive: 60
+  willTopic: CheckDoneConnect
+  willTopicQOS: 1
+  lwt: Dron380 Gone Offline
+  willRetain: False
+  # Mqtt topic
+  Flight_Information_topicToMqtt: Drone380/Flight_Information
+  Fly_Formation_topicToMqtt: Drone380/Formation
+  # Change formate qos
+  Fly_Formation_topicToMqtt_QOS: 2
+  #ROS
+ROS:
+  ROSClientNamePub: Drone380Pub
+  ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
+  ROStopicName_Fly_Formation: Fly_Formation_reciver
+LOG:
+  logFileName: pub.log

class_model/src/utils/mqttConfig_SUB2.yml

@@ -0,0 +1,30 @@
+MQTT:
+  msg_format: Json
+  MQTTClientNameSub: Drone650Sub
+  host: 140.120.31.133
+  port: 1883
+  keepalive: 60
+  willTopic: CheckDoneConnect
+  willTopicQOS: 1
+  lwt: Dron550 Gone Offline
+  willRetain: False
+  # Mqtt topic
+  Fly_Formation_topicToMqtt: Drone650/Formation
+  Drone550_Flight_Information_topicToMqtt: Drone550/Flight_Information
+  
+  Drone380_Flight_Information_topicToMqtt: Drone380/Flight_Information
+
+  Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
+
+  # Change formate qos
+  Fly_Formation_topicToMqtt_QOS: 2
+ROS:
+  ROSClientNameSub: Drone650Sub
+  ROStopicName_Fly_Formation: Fly_Formation_reciver
+  Dron550_ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
+
+  Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
+
+  Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
+LOG:
+  logFileName: sub.log

class_model/src/utils/mqttConfig_SUB3.yml

@@ -0,0 +1,30 @@
+MQTT:
+  msg_format: Json
+  MQTTClientNameSub: Drone380Sub
+  host: 140.120.31.133
+  port: 1883
+  keepalive: 60
+  willTopic: CheckDoneConnect
+  willTopicQOS: 1
+  lwt: Dron550 Gone Offline
+  willRetain: False
+  # Mqtt topic
+  Fly_Formation_topicToMqtt: Drone380/Formation
+  Drone550_Flight_Information_topicToMqtt: Drone550/Flight_Information
+  
+  Drone380_Flight_Information_topicToMqtt: Drone380/Flight_Information
+
+  Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
+
+  # Change formate qos
+  Fly_Formation_topicToMqtt_QOS: 2
+ROS:
+  ROSClientNameSub: Drone380Sub
+  ROStopicName_Fly_Formation: Fly_Formation_reciver
+  Dron550_ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
+
+  Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
+
+  Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
+LOG:
+  logFileName: sub.log