final update

.gitignore

@@ -1,3 +1,6 @@
 iq_gnc/
 ROS_controll/
-launch/
+__pycache__/
+*pyc
+*log
+.todo

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/test.launch",
+            "target": "/home/dodo/ardupilot_ws/src/class_model/launch/bio_drones.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

@@ -120,7 +120,7 @@ include_directories(
 include
   ${catkin_INCLUDE_DIRS}
 )
-link_directories(/home/dodo/ardupilot_ws/src/class_model/include/json_transport)
+# link_directories(/home/dodo/ardupilot_ws/src/class_model/include/json_transport)
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
 #   src/${PROJECT_NAME}/class_model.cpp
@@ -232,7 +232,7 @@ add_dependencies(control_lib main_generate_messages_cpp)
 
 add_library(command_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/command.cpp)
 target_include_directories(command_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
-target_link_libraries(command_lib ${catkin_LIBRARIES})
+target_link_libraries(command_lib ${catkin_LIBRARIES} param_lib) 
 add_dependencies(command_lib main_generate_messages_cpp)
 
 add_library(receiver_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/receiver.cpp)
@@ -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)
@@ -255,11 +255,21 @@ target_include_directories(PID_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
 target_link_libraries(PID_lib ${catkin_LIBRARIES})
 add_dependencies(PID_lib main_generate_messages_cpp)
 
+add_library(PubData_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/PubData.cpp)
+target_include_directories(PubData_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_link_libraries(PubData_lib ${catkin_LIBRARIES})
+add_dependencies(PubData_lib main_generate_messages_cpp)
+
+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)
 target_include_directories(formation_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
-target_link_libraries(formation_lib ${catkin_LIBRARIES} sensor_lib mode_lib control_lib command_lib requestData_lib PID_lib)
+target_link_libraries(formation_lib ${catkin_LIBRARIES} sensor_lib mode_lib control_lib command_lib requestData_lib PID_lib PubData_lib)
 add_dependencies(formation_lib main_generate_messages_cpp)
 
 add_library(select_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/select.cpp)
@@ -272,11 +282,41 @@ 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)
+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)
+
+add_executable(bio_main4 src/bio_main4.cpp)
+target_link_libraries(bio_main4 ${catkin_LIBRARIES} follower_lib Leader_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
+add_dependencies(bio_main4 class_model_generate_messages_cpp)
+
+add_executable(bio_main5 src/bio_main5.cpp)
+target_link_libraries(bio_main5 ${catkin_LIBRARIES} follower_lib Leader_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
+add_dependencies(bio_main5 class_model_generate_messages_cpp)
+
 ################json test
 add_executable(json src/json.cpp)
 target_link_libraries(json ${catkin_LIBRARIES} )
@@ -285,3 +325,8 @@ add_dependencies(json class_model_generate_messages_cpp)
 add_executable(json1 src/json1.cpp)
 target_link_libraries(json1 ${catkin_LIBRARIES} )
 add_dependencies(json1 class_model_generate_messages_cpp)
+
+##################test library
+add_executable(test_lib src/test_lib.cpp)
+target_link_libraries(test_lib ${catkin_LIBRARIES} /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
+add_dependencies(test_lib class_model_generate_messages_cpp)

class_model/include/class_model/DroneStatus.h

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

class_model/include/class_model/FindLeader.h

@@ -0,0 +1,44 @@
+#include "class_model/gps_location.h"
+#include "class_model/DroneStatus.h"
+#include <iterator> 
+
+typedef struct result{
+    int leader_ID,leader_index;
+    bool is_leader;
+}result;
+
+result check_leader(global_location* data, size_t index){
+    
+    float d[index];
+    int check_ID;
+    result _result;
+
+    for(int i=1 ; i<index ; i++){
+
+        d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
+
+        // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
+    }
+
+    std::vector<float> _d(d+1,d+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){
+        _result.is_leader = 1;
+        return _result;
+    }else{
+        _result.is_leader = 0;
+        return _result;
+    }
+    
+}

class_model/include/class_model/INFO.h

@@ -0,0 +1,14 @@
+#ifndef INFO_H
+#define INFO_H
+
+#include<std_msgs/String.h>
+
+typedef struct flight_INFO
+{
+    std::string type,              //type include drone's formation. ex: V , X ,| ...... 
+                mission;           //The mission is what we want the drone to do.
+}INFO;
+
+
+
+#endif //INFO_H

class_model/include/class_model/Leader.h

@@ -0,0 +1,52 @@
+/*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);
+    int isOnTarget();
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    global_location target_location;
+    global_location pre_target;
+    global_location follower_location;
+    global_location leader_location;
+    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] = {0.5 , 0.000001 ,0.001};
+    float ignore_small = 0.50 ,limit_speed = 10;
+    void face2target(float target_heading);   
+};
+#endif // Leader_H

class_model/include/class_model/PID.h

@@ -4,16 +4,21 @@
 
 #include <ros/ros.h>
 #include "ctime"
+#include <iostream>
+#include <fstream>
 
 class PIDClass {
 public:
-//   PIDClass();
-//   virtual ~PIDClass();
+  PIDClass();
+  virtual ~PIDClass();
   float update(float current ,float target ,float pidvals[3]);
   float update1(float current ,float target );
+  std::string droneID;
 private:
   // ROS NodeHandle
   ros::NodeHandle node_handle_;
+  std::ofstream logFile;
+  std::string ros_namespace;
 
   float pidval1[3] = {0.5 , 0.000000000001 , 0.5};
   float limit[2] = {2 , -2};
@@ -23,5 +28,5 @@ private:
 
 
 };
-
+//"/home/dodo/ardupilot_ws/src/class_model/src/log/PID"+droneID+".txt"
 #endif // PID_H

class_model/include/class_model/PubData.h

@@ -0,0 +1,39 @@
+/*Publish Compass & velocity data to topic "DroneStatus"*/
+#ifndef PUBDATA_H
+#define PUBDATA_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <std_msgs/Float64.h>
+#include <mavros_msgs/Waypoint.h>
+#include "class_model/velocity.h"
+
+class PubDataClass {
+public:
+    PubDataClass();
+    virtual ~PubDataClass();
+    void publishData(velocity v);
+
+
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    //SUBSCRIBE
+    ros::Subscriber Compass;
+
+    //PUBLISH
+    ros::Publisher PubData;
+
+    mavros_msgs::Waypoint msg;
+    float heading;
+    void Compass_callback(const std_msgs::Float64::ConstPtr &degree);    
+
+
+
+    
+
+};
+
+#endif // PUBDATA_H

class_model/include/class_model/PubInfo.h

@@ -0,0 +1,31 @@
+/*Publish Drone's status to topic "Flight_Information_reciver"*/
+#ifndef PUBINFO_H
+#define PUBINFO_H
+
+#include <ros/ros.h>
+#include <diagnostic_msgs/KeyValue.h>
+#include "class_model/INFO.h" 
+
+
+class PubInfoClass {
+public:
+    PubInfoClass();
+    virtual ~PubInfoClass();   
+    void pub_info(INFO msg);
+
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    //SUBSCRIBE
+
+    //PUBLISH
+    ros::Publisher PubInfo;
+    INFO msg;
+    diagnostic_msgs::KeyValue info;
+    
+
+};
+
+#endif // PUBINFO_H

class_model/include/class_model/command.h

@@ -1,29 +1,42 @@
 /*Drone Control Command*/
 #ifndef COMMAND_H
 #define COMMAND_H
-
+// #define DEBUG
 #include <ros/ros.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <mavros_msgs/GlobalPositionTarget.h>
 #include <class_model/sensor.h>
+#include <class_model/Param.h>
+#include <fstream>
+#include <iostream>
+#include <string>        
+
+// #include <geographic_msgs/GeoPoseStamped.h>
 
 class CommandClass {
 public:
     CommandClass();
     virtual ~CommandClass();
-    void set_global_position(float lat,float lon,float alt);
+    void set_global_position(double lon,double lat,float alt,float yaw);
+    #ifdef DEBUG
+    ParamClass param_object;
+    void set_velocity(float x,float y,float alt,float yaw,float second,uint64_t get_t,uint64_t cal_t);
+    #endif
+    #ifndef DEBUG
     void set_velocity(float x,float y,float alt,float yaw,float second);
+    #endif
     void fix_velocity(float x,float y,float alt,float yaw,float second);
     void velocity_init();
     void set_target_position(float x,float y);
     ThreadGPSClass gps_object;
 
+
 private:
     // ROS NodeHandle
     ros::NodeHandle node_handle_;
 
     geometry_msgs::TwistStamped msg;
-    mavros_msgs::GlobalPositionTarget goal_position;
+    // mavros_msgs::GlobalPositionTarget goal_position;
     global_location current_location;
     global_location target_location;
     global_location origin_location;

class_model/include/class_model/convert_degree.h

@@ -5,10 +5,10 @@ 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(y/x)*180/3.14) + 180;
+        degree = (atan(x/y)*180/3.14) + 180;
     }
     if(x==0 && y>=0){
         degree = 0;
@@ -29,11 +29,11 @@ float check_direction(float error_degree){
     int direction;
     if(error_degree <= 180 && error_degree >= 0){              //check yaw direction
         direction = -1;
-    }else if(error_degree < -180){
+    }else if(error_degree < -180 && error_degree < 0){
         direction = -1;
     }else if(error_degree >= -180 && error_degree < 0){
         direction = 1;
-    }else if(error_degree > 180){
+    }else if(error_degree > 180 && error_degree >= 0){
         direction = 1;
     }
 

class_model/include/class_model/follower.h

@@ -1,13 +1,20 @@
 /*Follow the leader in a fixed formation */
 #ifndef FOLLOWER_H
 #define FOLLOWER_H
-
+#define swarmNum 5       //The number of drones
+#define changeLeader
+// #define NoChangeLeader
 #include <ros/ros.h>
 #include "class_model/sensor.h"
 #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 {
@@ -18,9 +25,27 @@ public:
     ThreadGPSClass GPS_object;
     ModeClass mode_object;
     ControlClass control_object;
+    ReceiverClass receiver_object;
     CommandClass command_object;
     RequestClass request_object;
-    void follower();
+    PIDClass PID_x;
+    PIDClass PID_y;
+    DroneStatus drone1;
+    DroneStatus drone2;    
+    DroneStatus drone3;
+    DroneStatus drone4;
+    DroneStatus drone5;
+    DroneStatus self;
+    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);
+    void face2target(global_location* target, global_location* leader);
+
+    float lon[3],lat[3];
+    global_location vector_LT,vector_SM,pre_LT,vector_LR;
+    
+
 private:
     // ROS NodeHandle
     ros::NodeHandle node_handle_;
@@ -28,8 +53,30 @@ private:
     global_location target_location;
     global_location follower_location;
     global_location leader_location;
+    global_location leader_drone;
+    global_location refpos;
+    DroneStatus memberDrone[swarmNum];
+    global_location (RequestClass::*ref)();
+
+    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] = {0.5 , 0.000001 ,0.001};// D downsize
+    float ignore_small = 0.50 ,limit_speed = 10;
+    float m,n,k,cosTheta,pre_m,pre_k,u,LTcos;  // function direct()'s variable
+    global_location pre_tar,ref_point;  // function plane()'s variable
+    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);
+    int direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member);
+    void choose_leader();
 
-    void calculate_position(float k,float theta);
 
 };
 

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
 #include <ros/ros.h>
 #include "class_model/sensor.h"
 #include "class_model/mode.h"
@@ -10,6 +10,7 @@
 #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>
 
 
@@ -25,6 +26,7 @@ public:
     ControlClass control_object;
     CommandClass command_object;
     RequestClass request_object;
+    PubDataClass PubData_object;
     PIDClass PID_x;
     PIDClass PID_y;
     
@@ -35,6 +37,7 @@ public:
     void follower3(int type);
     void follower4(int type);
     void follower5(int type);
+    void follower6(int type);
     void sph_follower1(int type);
     void sph_follower2(int type);
     void sph_follower3(int type);
@@ -48,10 +51,13 @@ private:
     global_location follower_location;
     global_location leader_location;
     global_location current_location;
-    int flag = 0,heading_status = 0;
+    velocity drone_speed;
+    int flag = 0,heading_status = 0,buf = 0;
     float pre_alt,cur_alt;
-    float leader_pid[3] = {0.5 , 0.000000000001 ,0.5};
-    float follower_pid[3] = {1 , 0.00000000001 ,0.5};
+    float error_lon,error_lat;
+    float leader_pid[3] = {0.5 , 0.000001 ,0.001};
+    float follower_pid[3] = {0.5 , 0.000001 ,0.001};
+    float ignore_small = 0.50;
 
     void calculate_position(float k,float theta,int direction=0);
     void spherical_coordinate(float k,float theta,float psi);

class_model/include/class_model/gps_location.h

@@ -3,6 +3,8 @@
 
 typedef struct global_location{
 	double lat,lon,alt;
+	float heading;
+	int ID;
 }global_location;
 
 #endif // GOBAL_LOCATION_H

class_model/include/class_model/requestData.h

@@ -4,9 +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;
@@ -17,6 +22,11 @@ public:
     RequestClass();
     virtual ~RequestClass();
     global_location get_leader_GPS();
+    global_location get_self_GPS();
+    global_location get_M1_GPS();
+    global_location get_M2_GPS();
+    global_location get_M3_GPS();
+    global_location get_M4_GPS();
     float get_leader_heading();
     int get_formation_message();
     json get_data();
@@ -26,19 +36,31 @@ private:
     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 drone4_callback(const std_msgs::String::ConstPtr &gps);
+    void drone5_callback(const std_msgs::String::ConstPtr &gps);
     void Message_callback(const std_msgs::String::ConstPtr &message);
-    void jsonToInt(std::string data);
     void jsonToString(std::string data);
-    void StringToJson(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,M3_position,M4_position;
+    std::array <int,5> ID_array {1,2,3,4,5};
     // rapidjson::Document document;
-    json j_data;
+    json j_data,j_data2,j_data3,j_data4,j_data5;
+    int self_ID,drone_ID;
 
   // SUBSCRIBE
     ros::Subscriber mqtt_data; 
     ros::Subscriber formation_data;
-
+    ros::Subscriber drone1_data;
+    ros::Subscriber drone2_data;
+    ros::Subscriber drone3_data;
+    ros::Subscriber drone4_data;
+    ros::Subscriber drone5_data;
 };
 
 #endif // REQUESTDATA_H

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
+

class_model/include/class_model/select.h

@@ -22,6 +22,7 @@ public:
     void goose_formation(float x=0,float y=0);
     void protect_formation(float x=0,float y=0);
     void Hex_formation(float x=0,float y=0);
+    void house_formation(float x=0,float y=0);
     void start_formation();
     void stop();
     

class_model/include/class_model/velocity.h

@@ -0,0 +1,8 @@
+#ifndef VELOCITY_H
+#define VELOCITY_H
+
+typedef struct velocity{
+	float x,y;
+}velocity;
+
+#endif // VELOCITY_H

class_model/include/yaml-cpp/anchor.h

@@ -0,0 +1,17 @@
+#ifndef ANCHOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define ANCHOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <cstddef>
+
+namespace YAML {
+using anchor_t = std::size_t;
+const anchor_t NullAnchor = 0;
+}
+
+#endif  // ANCHOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/binary.h

@@ -0,0 +1,71 @@
+#ifndef BASE64_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define BASE64_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <string>
+#include <vector>
+
+#include "yaml-cpp/dll.h"
+
+namespace YAML {
+YAML_CPP_API std::string EncodeBase64(const unsigned char *data,
+                                      std::size_t size);
+YAML_CPP_API std::vector<unsigned char> DecodeBase64(const std::string &input);
+
+class YAML_CPP_API Binary {
+ public:
+  Binary(const unsigned char *data_, std::size_t size_)
+      : m_data{}, m_unownedData(data_), m_unownedSize(size_) {}
+  Binary() : Binary(nullptr, 0) {}
+  Binary(const Binary &) = default;
+  Binary(Binary &&) = default;
+  Binary &operator=(const Binary &) = default;
+  Binary &operator=(Binary &&) = default;
+
+  bool owned() const { return !m_unownedData; }
+  std::size_t size() const { return owned() ? m_data.size() : m_unownedSize; }
+  const unsigned char *data() const {
+    return owned() ? &m_data[0] : m_unownedData;
+  }
+
+  void swap(std::vector<unsigned char> &rhs) {
+    if (m_unownedData) {
+      m_data.swap(rhs);
+      rhs.clear();
+      rhs.resize(m_unownedSize);
+      std::copy(m_unownedData, m_unownedData + m_unownedSize, rhs.begin());
+      m_unownedData = nullptr;
+      m_unownedSize = 0;
+    } else {
+      m_data.swap(rhs);
+    }
+  }
+
+  bool operator==(const Binary &rhs) const {
+    const std::size_t s = size();
+    if (s != rhs.size())
+      return false;
+    const unsigned char *d1 = data();
+    const unsigned char *d2 = rhs.data();
+    for (std::size_t i = 0; i < s; i++) {
+      if (*d1++ != *d2++)
+        return false;
+    }
+    return true;
+  }
+
+  bool operator!=(const Binary &rhs) const { return !(*this == rhs); }
+
+ private:
+  std::vector<unsigned char> m_data;
+  const unsigned char *m_unownedData;
+  std::size_t m_unownedSize;
+};
+}  // namespace YAML
+
+#endif  // BASE64_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/contrib/anchordict.h

@@ -0,0 +1,40 @@
+#ifndef ANCHORDICT_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define ANCHORDICT_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <vector>
+
+#include "../anchor.h"
+
+namespace YAML {
+/**
+ * An object that stores and retrieves values correlating to {@link anchor_t}
+ * values.
+ *
+ * <p>Efficient implementation that can make assumptions about how
+ * {@code anchor_t} values are assigned by the {@link Parser} class.
+ */
+template <class T>
+class AnchorDict {
+ public:
+  AnchorDict() : m_data{} {}
+  void Register(anchor_t anchor, T value) {
+    if (anchor > m_data.size()) {
+      m_data.resize(anchor);
+    }
+    m_data[anchor - 1] = value;
+  }
+
+  T Get(anchor_t anchor) const { return m_data[anchor - 1]; }
+
+ private:
+  std::vector<T> m_data;
+};
+}  // namespace YAML
+
+#endif  // ANCHORDICT_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/contrib/graphbuilder.h

@@ -0,0 +1,149 @@
+#ifndef GRAPHBUILDER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define GRAPHBUILDER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/mark.h"
+#include <string>
+
+namespace YAML {
+class Parser;
+
+// GraphBuilderInterface
+// . Abstraction of node creation
+// . pParentNode is always nullptr or the return value of one of the NewXXX()
+//   functions.
+class GraphBuilderInterface {
+ public:
+  virtual ~GraphBuilderInterface() = 0;
+
+  // Create and return a new node with a null value.
+  virtual void *NewNull(const Mark &mark, void *pParentNode) = 0;
+
+  // Create and return a new node with the given tag and value.
+  virtual void *NewScalar(const Mark &mark, const std::string &tag,
+                          void *pParentNode, const std::string &value) = 0;
+
+  // Create and return a new sequence node
+  virtual void *NewSequence(const Mark &mark, const std::string &tag,
+                            void *pParentNode) = 0;
+
+  // Add pNode to pSequence.  pNode was created with one of the NewXxx()
+  // functions and pSequence with NewSequence().
+  virtual void AppendToSequence(void *pSequence, void *pNode) = 0;
+
+  // Note that no moew entries will be added to pSequence
+  virtual void SequenceComplete(void *pSequence) { (void)pSequence; }
+
+  // Create and return a new map node
+  virtual void *NewMap(const Mark &mark, const std::string &tag,
+                       void *pParentNode) = 0;
+
+  // Add the pKeyNode => pValueNode mapping to pMap.  pKeyNode and pValueNode
+  // were created with one of the NewXxx() methods and pMap with NewMap().
+  virtual void AssignInMap(void *pMap, void *pKeyNode, void *pValueNode) = 0;
+
+  // Note that no more assignments will be made in pMap
+  virtual void MapComplete(void *pMap) { (void)pMap; }
+
+  // Return the node that should be used in place of an alias referencing
+  // pNode (pNode by default)
+  virtual void *AnchorReference(const Mark &mark, void *pNode) {
+    (void)mark;
+    return pNode;
+  }
+};
+
+// Typesafe wrapper for GraphBuilderInterface.  Assumes that Impl defines
+// Node, Sequence, and Map types.  Sequence and Map must derive from Node
+// (unless Node is defined as void).  Impl must also implement function with
+// all of the same names as the virtual functions in GraphBuilderInterface
+// -- including the ones with default implementations -- but with the
+// prototypes changed to accept an explicit Node*, Sequence*, or Map* where
+// appropriate.
+template <class Impl>
+class GraphBuilder : public GraphBuilderInterface {
+ public:
+  typedef typename Impl::Node Node;
+  typedef typename Impl::Sequence Sequence;
+  typedef typename Impl::Map Map;
+
+  GraphBuilder(Impl &impl) : m_impl(impl) {
+    Map *pMap = nullptr;
+    Sequence *pSeq = nullptr;
+    Node *pNode = nullptr;
+
+    // Type consistency checks
+    pNode = pMap;
+    pNode = pSeq;
+  }
+
+  GraphBuilderInterface &AsBuilderInterface() { return *this; }
+
+  virtual void *NewNull(const Mark &mark, void *pParentNode) {
+    return CheckType<Node>(m_impl.NewNull(mark, AsNode(pParentNode)));
+  }
+
+  virtual void *NewScalar(const Mark &mark, const std::string &tag,
+                          void *pParentNode, const std::string &value) {
+    return CheckType<Node>(
+        m_impl.NewScalar(mark, tag, AsNode(pParentNode), value));
+  }
+
+  virtual void *NewSequence(const Mark &mark, const std::string &tag,
+                            void *pParentNode) {
+    return CheckType<Sequence>(
+        m_impl.NewSequence(mark, tag, AsNode(pParentNode)));
+  }
+  virtual void AppendToSequence(void *pSequence, void *pNode) {
+    m_impl.AppendToSequence(AsSequence(pSequence), AsNode(pNode));
+  }
+  virtual void SequenceComplete(void *pSequence) {
+    m_impl.SequenceComplete(AsSequence(pSequence));
+  }
+
+  virtual void *NewMap(const Mark &mark, const std::string &tag,
+                       void *pParentNode) {
+    return CheckType<Map>(m_impl.NewMap(mark, tag, AsNode(pParentNode)));
+  }
+  virtual void AssignInMap(void *pMap, void *pKeyNode, void *pValueNode) {
+    m_impl.AssignInMap(AsMap(pMap), AsNode(pKeyNode), AsNode(pValueNode));
+  }
+  virtual void MapComplete(void *pMap) { m_impl.MapComplete(AsMap(pMap)); }
+
+  virtual void *AnchorReference(const Mark &mark, void *pNode) {
+    return CheckType<Node>(m_impl.AnchorReference(mark, AsNode(pNode)));
+  }
+
+ private:
+  Impl &m_impl;
+
+  // Static check for pointer to T
+  template <class T, class U>
+  static T *CheckType(U *p) {
+    return p;
+  }
+
+  static Node *AsNode(void *pNode) { return static_cast<Node *>(pNode); }
+  static Sequence *AsSequence(void *pSeq) {
+    return static_cast<Sequence *>(pSeq);
+  }
+  static Map *AsMap(void *pMap) { return static_cast<Map *>(pMap); }
+};
+
+void *BuildGraphOfNextDocument(Parser &parser,
+                               GraphBuilderInterface &graphBuilder);
+
+template <class Impl>
+typename Impl::Node *BuildGraphOfNextDocument(Parser &parser, Impl &impl) {
+  GraphBuilder<Impl> graphBuilder(impl);
+  return static_cast<typename Impl::Node *>(
+      BuildGraphOfNextDocument(parser, graphBuilder));
+}
+}
+
+#endif  // GRAPHBUILDER_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/depthguard.h

@@ -0,0 +1,77 @@
+#ifndef DEPTH_GUARD_H_00000000000000000000000000000000000000000000000000000000
+#define DEPTH_GUARD_H_00000000000000000000000000000000000000000000000000000000
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "exceptions.h"
+
+namespace YAML {
+
+/**
+ * @brief The DeepRecursion class
+ *  An exception class which is thrown by DepthGuard. Ideally it should be
+ * a member of DepthGuard. However, DepthGuard is a templated class which means
+ * that any catch points would then need to know the template parameters. It is
+ * simpler for clients to not have to know at the catch point what was the
+ * maximum depth.
+ */
+class DeepRecursion : public ParserException {
+public:
+  virtual ~DeepRecursion() = default;
+
+  DeepRecursion(int depth, const Mark& mark_, const std::string& msg_);
+
+  // Returns the recursion depth when the exception was thrown
+  int depth() const {
+    return m_depth;
+  }
+
+private:
+  int m_depth = 0;
+};
+
+/**
+ * @brief The DepthGuard class
+ *  DepthGuard takes a reference to an integer. It increments the integer upon
+ * construction of DepthGuard and decrements the integer upon destruction.
+ *
+ * If the integer would be incremented past max_depth, then an exception is
+ * thrown. This is ideally geared toward guarding against deep recursion.
+ *
+ * @param max_depth
+ *  compile-time configurable maximum depth.
+ */
+template <int max_depth = 2000>
+class DepthGuard final {
+public:
+  DepthGuard(int & depth_, const Mark& mark_, const std::string& msg_) : m_depth(depth_) {
+    ++m_depth;
+    if ( max_depth <= m_depth ) {
+        throw DeepRecursion{m_depth, mark_, msg_};
+    }
+  }
+
+  DepthGuard(const DepthGuard & copy_ctor) = delete;
+  DepthGuard(DepthGuard && move_ctor) = delete;
+  DepthGuard & operator=(const DepthGuard & copy_assign) = delete;
+  DepthGuard & operator=(DepthGuard && move_assign) = delete;
+
+  ~DepthGuard() {
+    --m_depth;
+  }
+
+  int current_depth() const {
+    return m_depth;
+  }
+
+private:
+    int & m_depth;
+};
+
+} // namespace YAML
+
+#endif // DEPTH_GUARD_H_00000000000000000000000000000000000000000000000000000000

class_model/include/yaml-cpp/dll.h

@@ -0,0 +1,61 @@
+#ifndef DLL_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define DLL_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+// Definition YAML_CPP_STATIC_DEFINE using to building YAML-CPP as static
+// library (definition created by CMake or defined manually)
+
+// Definition yaml_cpp_EXPORTS using to building YAML-CPP as dll/so library
+// (definition created by CMake or defined manually)
+
+#ifdef YAML_CPP_STATIC_DEFINE
+#  define YAML_CPP_API
+#  define YAML_CPP_NO_EXPORT
+#else
+#  if defined(_MSC_VER) || defined(__MINGW32__) || defined(__MINGW64__)
+#    ifndef YAML_CPP_API
+#      ifdef yaml_cpp_EXPORTS
+         /* We are building this library */
+#        pragma message( "Defining YAML_CPP_API for DLL export" )
+#        define YAML_CPP_API __declspec(dllexport)
+#      else
+         /* We are using this library */
+#        pragma message( "Defining YAML_CPP_API for DLL import" )
+#        define YAML_CPP_API __declspec(dllimport)
+#      endif
+#    endif
+#    ifndef YAML_CPP_NO_EXPORT
+#      define YAML_CPP_NO_EXPORT
+#    endif
+#  else /* No _MSC_VER */
+#    ifndef YAML_CPP_API
+#      ifdef yaml_cpp_EXPORTS
+         /* We are building this library */
+#        define YAML_CPP_API __attribute__((visibility("default")))
+#      else
+         /* We are using this library */
+#        define YAML_CPP_API __attribute__((visibility("default")))
+#      endif
+#    endif
+#    ifndef YAML_CPP_NO_EXPORT
+#      define YAML_CPP_NO_EXPORT __attribute__((visibility("hidden")))
+#    endif
+#  endif /* _MSC_VER */
+#endif   /* YAML_CPP_STATIC_DEFINE */
+
+#ifndef YAML_CPP_DEPRECATED
+#  ifdef _MSC_VER
+#    define YAML_CPP_DEPRECATED __declspec(deprecated)
+#  else
+#    define YAML_CPP_DEPRECATED __attribute__ ((__deprecated__))
+#  endif
+#endif
+
+#ifndef YAML_CPP_DEPRECATED_EXPORT
+#  define YAML_CPP_DEPRECATED_EXPORT YAML_CPP_API YAML_CPP_DEPRECATED
+#endif
+
+#ifndef YAML_CPP_DEPRECATED_NO_EXPORT
+#  define YAML_CPP_DEPRECATED_NO_EXPORT YAML_CPP_NO_EXPORT YAML_CPP_DEPRECATED
+#endif
+
+#endif /* DLL_H_62B23520_7C8E_11DE_8A39_0800200C9A66 */

class_model/include/yaml-cpp/emitfromevents.h

@@ -0,0 +1,57 @@
+#ifndef EMITFROMEVENTS_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define EMITFROMEVENTS_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <stack>
+
+#include "yaml-cpp/anchor.h"
+#include "yaml-cpp/emitterstyle.h"
+#include "yaml-cpp/eventhandler.h"
+
+namespace YAML {
+struct Mark;
+}  // namespace YAML
+
+namespace YAML {
+class Emitter;
+
+class EmitFromEvents : public EventHandler {
+ public:
+  EmitFromEvents(Emitter& emitter);
+
+  void OnDocumentStart(const Mark& mark) override;
+  void OnDocumentEnd() override;
+
+  void OnNull(const Mark& mark, anchor_t anchor) override;
+  void OnAlias(const Mark& mark, anchor_t anchor) override;
+  void OnScalar(const Mark& mark, const std::string& tag,
+                        anchor_t anchor, const std::string& value) override;
+
+  void OnSequenceStart(const Mark& mark, const std::string& tag,
+                               anchor_t anchor, EmitterStyle::value style) override;
+  void OnSequenceEnd() override;
+
+  void OnMapStart(const Mark& mark, const std::string& tag,
+                          anchor_t anchor, EmitterStyle::value style) override;
+  void OnMapEnd() override;
+
+ private:
+  void BeginNode();
+  void EmitProps(const std::string& tag, anchor_t anchor);
+
+ private:
+  Emitter& m_emitter;
+
+  struct State {
+    enum value { WaitingForSequenceEntry, WaitingForKey, WaitingForValue };
+  };
+  std::stack<State::value> m_stateStack;
+};
+}
+
+#endif  // EMITFROMEVENTS_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/emitter.h

@@ -0,0 +1,281 @@
+#ifndef EMITTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define EMITTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <type_traits>
+
+#include "yaml-cpp/binary.h"
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/emitterdef.h"
+#include "yaml-cpp/emittermanip.h"
+#include "yaml-cpp/null.h"
+#include "yaml-cpp/ostream_wrapper.h"
+
+namespace YAML {
+class Binary;
+struct _Null;
+}  // namespace YAML
+
+namespace YAML {
+class EmitterState;
+
+class YAML_CPP_API Emitter {
+ public:
+  Emitter();
+  explicit Emitter(std::ostream& stream);
+  Emitter(const Emitter&) = delete;
+  Emitter& operator=(const Emitter&) = delete;
+  ~Emitter();
+
+  // output
+  const char* c_str() const;
+  std::size_t size() const;
+
+  // state checking
+  bool good() const;
+  const std::string GetLastError() const;
+
+  // global setters
+  bool SetOutputCharset(EMITTER_MANIP value);
+  bool SetStringFormat(EMITTER_MANIP value);
+  bool SetBoolFormat(EMITTER_MANIP value);
+  bool SetNullFormat(EMITTER_MANIP value);
+  bool SetIntBase(EMITTER_MANIP value);
+  bool SetSeqFormat(EMITTER_MANIP value);
+  bool SetMapFormat(EMITTER_MANIP value);
+  bool SetIndent(std::size_t n);
+  bool SetPreCommentIndent(std::size_t n);
+  bool SetPostCommentIndent(std::size_t n);
+  bool SetFloatPrecision(std::size_t n);
+  bool SetDoublePrecision(std::size_t n);
+  void RestoreGlobalModifiedSettings();
+
+  // local setters
+  Emitter& SetLocalValue(EMITTER_MANIP value);
+  Emitter& SetLocalIndent(const _Indent& indent);
+  Emitter& SetLocalPrecision(const _Precision& precision);
+
+  // overloads of write
+  Emitter& Write(const std::string& str);
+  Emitter& Write(bool b);
+  Emitter& Write(char ch);
+  Emitter& Write(const _Alias& alias);
+  Emitter& Write(const _Anchor& anchor);
+  Emitter& Write(const _Tag& tag);
+  Emitter& Write(const _Comment& comment);
+  Emitter& Write(const _Null& n);
+  Emitter& Write(const Binary& binary);
+
+  template <typename T>
+  Emitter& WriteIntegralType(T value);
+
+  template <typename T>
+  Emitter& WriteStreamable(T value);
+
+ private:
+  template <typename T>
+  void SetStreamablePrecision(std::stringstream&) {}
+  std::size_t GetFloatPrecision() const;
+  std::size_t GetDoublePrecision() const;
+
+  void PrepareIntegralStream(std::stringstream& stream) const;
+  void StartedScalar();
+
+ private:
+  void EmitBeginDoc();
+  void EmitEndDoc();
+  void EmitBeginSeq();
+  void EmitEndSeq();
+  void EmitBeginMap();
+  void EmitEndMap();
+  void EmitNewline();
+  void EmitKindTag();
+  void EmitTag(bool verbatim, const _Tag& tag);
+
+  void PrepareNode(EmitterNodeType::value child);
+  void PrepareTopNode(EmitterNodeType::value child);
+  void FlowSeqPrepareNode(EmitterNodeType::value child);
+  void BlockSeqPrepareNode(EmitterNodeType::value child);
+
+  void FlowMapPrepareNode(EmitterNodeType::value child);
+
+  void FlowMapPrepareLongKey(EmitterNodeType::value child);
+  void FlowMapPrepareLongKeyValue(EmitterNodeType::value child);
+  void FlowMapPrepareSimpleKey(EmitterNodeType::value child);
+  void FlowMapPrepareSimpleKeyValue(EmitterNodeType::value child);
+
+  void BlockMapPrepareNode(EmitterNodeType::value child);
+
+  void BlockMapPrepareLongKey(EmitterNodeType::value child);
+  void BlockMapPrepareLongKeyValue(EmitterNodeType::value child);
+  void BlockMapPrepareSimpleKey(EmitterNodeType::value child);
+  void BlockMapPrepareSimpleKeyValue(EmitterNodeType::value child);
+
+  void SpaceOrIndentTo(bool requireSpace, std::size_t indent);
+
+  const char* ComputeFullBoolName(bool b) const;
+  const char* ComputeNullName() const;
+  bool CanEmitNewline() const;
+
+ private:
+  std::unique_ptr<EmitterState> m_pState;
+  ostream_wrapper m_stream;
+};
+
+template <typename T>
+inline Emitter& Emitter::WriteIntegralType(T value) {
+  if (!good())
+    return *this;
+
+  PrepareNode(EmitterNodeType::Scalar);
+
+  std::stringstream stream;
+  PrepareIntegralStream(stream);
+  stream << value;
+  m_stream << stream.str();
+
+  StartedScalar();
+
+  return *this;
+}
+
+template <typename T>
+inline Emitter& Emitter::WriteStreamable(T value) {
+  if (!good())
+    return *this;
+
+  PrepareNode(EmitterNodeType::Scalar);
+
+  std::stringstream stream;
+  SetStreamablePrecision<T>(stream);
+
+  bool special = false;
+  if (std::is_floating_point<T>::value) {
+    if ((std::numeric_limits<T>::has_quiet_NaN ||
+         std::numeric_limits<T>::has_signaling_NaN) &&
+        std::isnan(value)) {
+      special = true;
+      stream << ".nan";
+    } else if (std::numeric_limits<T>::has_infinity && std::isinf(value)) {
+      special = true;
+      if (std::signbit(value)) {
+        stream << "-.inf";
+      } else {
+        stream << ".inf";
+      }
+    }
+  }
+
+  if (!special) {
+    stream << value;
+  }
+  m_stream << stream.str();
+
+  StartedScalar();
+
+  return *this;
+}
+
+template <>
+inline void Emitter::SetStreamablePrecision<float>(std::stringstream& stream) {
+  stream.precision(static_cast<std::streamsize>(GetFloatPrecision()));
+}
+
+template <>
+inline void Emitter::SetStreamablePrecision<double>(std::stringstream& stream) {
+  stream.precision(static_cast<std::streamsize>(GetDoublePrecision()));
+}
+
+// overloads of insertion
+inline Emitter& operator<<(Emitter& emitter, const std::string& v) {
+  return emitter.Write(v);
+}
+inline Emitter& operator<<(Emitter& emitter, bool v) {
+  return emitter.Write(v);
+}
+inline Emitter& operator<<(Emitter& emitter, char v) {
+  return emitter.Write(v);
+}
+inline Emitter& operator<<(Emitter& emitter, unsigned char v) {
+  return emitter.Write(static_cast<char>(v));
+}
+inline Emitter& operator<<(Emitter& emitter, const _Alias& v) {
+  return emitter.Write(v);
+}
+inline Emitter& operator<<(Emitter& emitter, const _Anchor& v) {
+  return emitter.Write(v);
+}
+inline Emitter& operator<<(Emitter& emitter, const _Tag& v) {
+  return emitter.Write(v);
+}
+inline Emitter& operator<<(Emitter& emitter, const _Comment& v) {
+  return emitter.Write(v);
+}
+inline Emitter& operator<<(Emitter& emitter, const _Null& v) {
+  return emitter.Write(v);
+}
+inline Emitter& operator<<(Emitter& emitter, const Binary& b) {
+  return emitter.Write(b);
+}
+
+inline Emitter& operator<<(Emitter& emitter, const char* v) {
+  return emitter.Write(std::string(v));
+}
+
+inline Emitter& operator<<(Emitter& emitter, int v) {
+  return emitter.WriteIntegralType(v);
+}
+inline Emitter& operator<<(Emitter& emitter, unsigned int v) {
+  return emitter.WriteIntegralType(v);
+}
+inline Emitter& operator<<(Emitter& emitter, short v) {
+  return emitter.WriteIntegralType(v);
+}
+inline Emitter& operator<<(Emitter& emitter, unsigned short v) {
+  return emitter.WriteIntegralType(v);
+}
+inline Emitter& operator<<(Emitter& emitter, long v) {
+  return emitter.WriteIntegralType(v);
+}
+inline Emitter& operator<<(Emitter& emitter, unsigned long v) {
+  return emitter.WriteIntegralType(v);
+}
+inline Emitter& operator<<(Emitter& emitter, long long v) {
+  return emitter.WriteIntegralType(v);
+}
+inline Emitter& operator<<(Emitter& emitter, unsigned long long v) {
+  return emitter.WriteIntegralType(v);
+}
+
+inline Emitter& operator<<(Emitter& emitter, float v) {
+  return emitter.WriteStreamable(v);
+}
+inline Emitter& operator<<(Emitter& emitter, double v) {
+  return emitter.WriteStreamable(v);
+}
+
+inline Emitter& operator<<(Emitter& emitter, EMITTER_MANIP value) {
+  return emitter.SetLocalValue(value);
+}
+
+inline Emitter& operator<<(Emitter& emitter, _Indent indent) {
+  return emitter.SetLocalIndent(indent);
+}
+
+inline Emitter& operator<<(Emitter& emitter, _Precision precision) {
+  return emitter.SetLocalPrecision(precision);
+}
+}  // namespace YAML
+
+#endif  // EMITTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/emitterdef.h

@@ -0,0 +1,16 @@
+#ifndef EMITTERDEF_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define EMITTERDEF_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+namespace YAML {
+struct EmitterNodeType {
+  enum value { NoType, Property, Scalar, FlowSeq, BlockSeq, FlowMap, BlockMap };
+};
+}
+
+#endif  // EMITTERDEF_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/emittermanip.h

@@ -0,0 +1,144 @@
+#ifndef EMITTERMANIP_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define EMITTERMANIP_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <string>
+
+namespace YAML {
+enum EMITTER_MANIP {
+  // general manipulators
+  Auto,
+  TagByKind,
+  Newline,
+
+  // output character set
+  EmitNonAscii,
+  EscapeNonAscii,
+  EscapeAsJson,
+
+  // string manipulators
+  // Auto, // duplicate
+  SingleQuoted,
+  DoubleQuoted,
+  Literal,
+
+  // null manipulators
+  LowerNull,
+  UpperNull,
+  CamelNull,
+  TildeNull,
+
+  // bool manipulators
+  YesNoBool,      // yes, no
+  TrueFalseBool,  // true, false
+  OnOffBool,      // on, off
+  UpperCase,      // TRUE, N
+  LowerCase,      // f, yes
+  CamelCase,      // No, Off
+  LongBool,       // yes, On
+  ShortBool,      // y, t
+
+  // int manipulators
+  Dec,
+  Hex,
+  Oct,
+
+  // document manipulators
+  BeginDoc,
+  EndDoc,
+
+  // sequence manipulators
+  BeginSeq,
+  EndSeq,
+  Flow,
+  Block,
+
+  // map manipulators
+  BeginMap,
+  EndMap,
+  Key,
+  Value,
+  // Flow, // duplicate
+  // Block, // duplicate
+  // Auto, // duplicate
+  LongKey
+};
+
+struct _Indent {
+  _Indent(int value_) : value(value_) {}
+  int value;
+};
+
+inline _Indent Indent(int value) { return _Indent(value); }
+
+struct _Alias {
+  _Alias(const std::string& content_) : content(content_) {}
+  std::string content;
+};
+
+inline _Alias Alias(const std::string& content) { return _Alias(content); }
+
+struct _Anchor {
+  _Anchor(const std::string& content_) : content(content_) {}
+  std::string content;
+};
+
+inline _Anchor Anchor(const std::string& content) { return _Anchor(content); }
+
+struct _Tag {
+  struct Type {
+    enum value { Verbatim, PrimaryHandle, NamedHandle };
+  };
+
+  explicit _Tag(const std::string& prefix_, const std::string& content_,
+                Type::value type_)
+      : prefix(prefix_), content(content_), type(type_) {}
+  std::string prefix;
+  std::string content;
+  Type::value type;
+};
+
+inline _Tag VerbatimTag(const std::string& content) {
+  return _Tag("", content, _Tag::Type::Verbatim);
+}
+
+inline _Tag LocalTag(const std::string& content) {
+  return _Tag("", content, _Tag::Type::PrimaryHandle);
+}
+
+inline _Tag LocalTag(const std::string& prefix, const std::string content) {
+  return _Tag(prefix, content, _Tag::Type::NamedHandle);
+}
+
+inline _Tag SecondaryTag(const std::string& content) {
+  return _Tag("", content, _Tag::Type::NamedHandle);
+}
+
+struct _Comment {
+  _Comment(const std::string& content_) : content(content_) {}
+  std::string content;
+};
+
+inline _Comment Comment(const std::string& content) { return _Comment(content); }
+
+struct _Precision {
+  _Precision(int floatPrecision_, int doublePrecision_)
+      : floatPrecision(floatPrecision_), doublePrecision(doublePrecision_) {}
+
+  int floatPrecision;
+  int doublePrecision;
+};
+
+inline _Precision FloatPrecision(int n) { return _Precision(n, -1); }
+
+inline _Precision DoublePrecision(int n) { return _Precision(-1, n); }
+
+inline _Precision Precision(int n) { return _Precision(n, n); }
+}
+
+#endif  // EMITTERMANIP_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/emitterstyle.h

@@ -0,0 +1,16 @@
+#ifndef EMITTERSTYLE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define EMITTERSTYLE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+namespace YAML {
+struct EmitterStyle {
+  enum value { Default, Block, Flow };
+};
+}
+
+#endif  // EMITTERSTYLE_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/eventhandler.h

@@ -0,0 +1,45 @@
+#ifndef EVENTHANDLER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define EVENTHANDLER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <string>
+
+#include "yaml-cpp/anchor.h"
+#include "yaml-cpp/emitterstyle.h"
+
+namespace YAML {
+struct Mark;
+
+class EventHandler {
+ public:
+  virtual ~EventHandler() = default;
+
+  virtual void OnDocumentStart(const Mark& mark) = 0;
+  virtual void OnDocumentEnd() = 0;
+
+  virtual void OnNull(const Mark& mark, anchor_t anchor) = 0;
+  virtual void OnAlias(const Mark& mark, anchor_t anchor) = 0;
+  virtual void OnScalar(const Mark& mark, const std::string& tag,
+                        anchor_t anchor, const std::string& value) = 0;
+
+  virtual void OnSequenceStart(const Mark& mark, const std::string& tag,
+                               anchor_t anchor, EmitterStyle::value style) = 0;
+  virtual void OnSequenceEnd() = 0;
+
+  virtual void OnMapStart(const Mark& mark, const std::string& tag,
+                          anchor_t anchor, EmitterStyle::value style) = 0;
+  virtual void OnMapEnd() = 0;
+
+  virtual void OnAnchor(const Mark& /*mark*/,
+                        const std::string& /*anchor_name*/) {
+    // empty default implementation for compatibility
+  }
+};
+}  // namespace YAML
+
+#endif  // EVENTHANDLER_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/exceptions.h

@@ -0,0 +1,303 @@
+#ifndef EXCEPTIONS_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define EXCEPTIONS_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/mark.h"
+#include "yaml-cpp/noexcept.h"
+#include "yaml-cpp/traits.h"
+#include <sstream>
+#include <stdexcept>
+#include <string>
+
+namespace YAML {
+// error messages
+namespace ErrorMsg {
+const char* const YAML_DIRECTIVE_ARGS =
+    "YAML directives must have exactly one argument";
+const char* const YAML_VERSION = "bad YAML version: ";
+const char* const YAML_MAJOR_VERSION = "YAML major version too large";
+const char* const REPEATED_YAML_DIRECTIVE = "repeated YAML directive";
+const char* const TAG_DIRECTIVE_ARGS =
+    "TAG directives must have exactly two arguments";
+const char* const REPEATED_TAG_DIRECTIVE = "repeated TAG directive";
+const char* const CHAR_IN_TAG_HANDLE =
+    "illegal character found while scanning tag handle";
+const char* const TAG_WITH_NO_SUFFIX = "tag handle with no suffix";
+const char* const END_OF_VERBATIM_TAG = "end of verbatim tag not found";
+const char* const END_OF_MAP = "end of map not found";
+const char* const END_OF_MAP_FLOW = "end of map flow not found";
+const char* const END_OF_SEQ = "end of sequence not found";
+const char* const END_OF_SEQ_FLOW = "end of sequence flow not found";
+const char* const MULTIPLE_TAGS =
+    "cannot assign multiple tags to the same node";
+const char* const MULTIPLE_ANCHORS =
+    "cannot assign multiple anchors to the same node";
+const char* const MULTIPLE_ALIASES =
+    "cannot assign multiple aliases to the same node";
+const char* const ALIAS_CONTENT =
+    "aliases can't have any content, *including* tags";
+const char* const INVALID_HEX = "bad character found while scanning hex number";
+const char* const INVALID_UNICODE = "invalid unicode: ";
+const char* const INVALID_ESCAPE = "unknown escape character: ";
+const char* const UNKNOWN_TOKEN = "unknown token";
+const char* const DOC_IN_SCALAR = "illegal document indicator in scalar";
+const char* const EOF_IN_SCALAR = "illegal EOF in scalar";
+const char* const CHAR_IN_SCALAR = "illegal character in scalar";
+const char* const TAB_IN_INDENTATION =
+    "illegal tab when looking for indentation";
+const char* const FLOW_END = "illegal flow end";
+const char* const BLOCK_ENTRY = "illegal block entry";
+const char* const MAP_KEY = "illegal map key";
+const char* const MAP_VALUE = "illegal map value";
+const char* const ALIAS_NOT_FOUND = "alias not found after *";
+const char* const ANCHOR_NOT_FOUND = "anchor not found after &";
+const char* const CHAR_IN_ALIAS =
+    "illegal character found while scanning alias";
+const char* const CHAR_IN_ANCHOR =
+    "illegal character found while scanning anchor";
+const char* const ZERO_INDENT_IN_BLOCK =
+    "cannot set zero indentation for a block scalar";
+const char* const CHAR_IN_BLOCK = "unexpected character in block scalar";
+const char* const AMBIGUOUS_ANCHOR =
+    "cannot assign the same alias to multiple nodes";
+const char* const UNKNOWN_ANCHOR = "the referenced anchor is not defined: ";
+
+const char* const INVALID_NODE =
+    "invalid node; this may result from using a map iterator as a sequence "
+    "iterator, or vice-versa";
+const char* const INVALID_SCALAR = "invalid scalar";
+const char* const KEY_NOT_FOUND = "key not found";
+const char* const BAD_CONVERSION = "bad conversion";
+const char* const BAD_DEREFERENCE = "bad dereference";
+const char* const BAD_SUBSCRIPT = "operator[] call on a scalar";
+const char* const BAD_PUSHBACK = "appending to a non-sequence";
+const char* const BAD_INSERT = "inserting in a non-convertible-to-map";
+
+const char* const UNMATCHED_GROUP_TAG = "unmatched group tag";
+const char* const UNEXPECTED_END_SEQ = "unexpected end sequence token";
+const char* const UNEXPECTED_END_MAP = "unexpected end map token";
+const char* const SINGLE_QUOTED_CHAR =
+    "invalid character in single-quoted string";
+const char* const INVALID_ANCHOR = "invalid anchor";
+const char* const INVALID_ALIAS = "invalid alias";
+const char* const INVALID_TAG = "invalid tag";
+const char* const BAD_FILE = "bad file";
+
+template <typename T>
+inline const std::string KEY_NOT_FOUND_WITH_KEY(
+    const T&, typename disable_if<is_numeric<T>>::type* = 0) {
+  return KEY_NOT_FOUND;
+}
+
+inline const std::string KEY_NOT_FOUND_WITH_KEY(const std::string& key) {
+  std::stringstream stream;
+  stream << KEY_NOT_FOUND << ": " << key;
+  return stream.str();
+}
+
+inline const std::string KEY_NOT_FOUND_WITH_KEY(const char* key) {
+  std::stringstream stream;
+  stream << KEY_NOT_FOUND << ": " << key;
+  return stream.str();
+}
+
+template <typename T>
+inline const std::string KEY_NOT_FOUND_WITH_KEY(
+    const T& key, typename enable_if<is_numeric<T>>::type* = 0) {
+  std::stringstream stream;
+  stream << KEY_NOT_FOUND << ": " << key;
+  return stream.str();
+}
+
+template <typename T>
+inline const std::string BAD_SUBSCRIPT_WITH_KEY(
+    const T&, typename disable_if<is_numeric<T>>::type* = nullptr) {
+  return BAD_SUBSCRIPT;
+}
+
+inline const std::string BAD_SUBSCRIPT_WITH_KEY(const std::string& key) {
+  std::stringstream stream;
+  stream << BAD_SUBSCRIPT << " (key: \"" << key << "\")";
+  return stream.str();
+}
+
+inline const std::string BAD_SUBSCRIPT_WITH_KEY(const char* key) {
+  std::stringstream stream;
+  stream << BAD_SUBSCRIPT << " (key: \"" << key << "\")";
+  return stream.str();
+}
+
+template <typename T>
+inline const std::string BAD_SUBSCRIPT_WITH_KEY(
+    const T& key, typename enable_if<is_numeric<T>>::type* = nullptr) {
+  std::stringstream stream;
+  stream << BAD_SUBSCRIPT << " (key: \"" << key << "\")";
+  return stream.str();
+}
+
+inline const std::string INVALID_NODE_WITH_KEY(const std::string& key) {
+  std::stringstream stream;
+  if (key.empty()) {
+    return INVALID_NODE;
+  }
+  stream << "invalid node; first invalid key: \"" << key << "\"";
+  return stream.str();
+}
+}  // namespace ErrorMsg
+
+class YAML_CPP_API Exception : public std::runtime_error {
+ public:
+  Exception(const Mark& mark_, const std::string& msg_)
+      : std::runtime_error(build_what(mark_, msg_)), mark(mark_), msg(msg_) {}
+  ~Exception() YAML_CPP_NOEXCEPT override;
+
+  Exception(const Exception&) = default;
+
+  Mark mark;
+  std::string msg;
+
+ private:
+  static const std::string build_what(const Mark& mark,
+                                      const std::string& msg) {
+    if (mark.is_null()) {
+      return msg;
+    }
+
+    std::stringstream output;
+    output << "yaml-cpp: error at line " << mark.line + 1 << ", column "
+           << mark.column + 1 << ": " << msg;
+    return output.str();
+  }
+};
+
+class YAML_CPP_API ParserException : public Exception {
+ public:
+  ParserException(const Mark& mark_, const std::string& msg_)
+      : Exception(mark_, msg_) {}
+  ParserException(const ParserException&) = default;
+  ~ParserException() YAML_CPP_NOEXCEPT override;
+};
+
+class YAML_CPP_API RepresentationException : public Exception {
+ public:
+  RepresentationException(const Mark& mark_, const std::string& msg_)
+      : Exception(mark_, msg_) {}
+  RepresentationException(const RepresentationException&) = default;
+  ~RepresentationException() YAML_CPP_NOEXCEPT override;
+};
+
+// representation exceptions
+class YAML_CPP_API InvalidScalar : public RepresentationException {
+ public:
+  InvalidScalar(const Mark& mark_)
+      : RepresentationException(mark_, ErrorMsg::INVALID_SCALAR) {}
+  InvalidScalar(const InvalidScalar&) = default;
+  ~InvalidScalar() YAML_CPP_NOEXCEPT override;
+};
+
+class YAML_CPP_API KeyNotFound : public RepresentationException {
+ public:
+  template <typename T>
+  KeyNotFound(const Mark& mark_, const T& key_)
+      : RepresentationException(mark_, ErrorMsg::KEY_NOT_FOUND_WITH_KEY(key_)) {
+  }
+  KeyNotFound(const KeyNotFound&) = default;
+  ~KeyNotFound() YAML_CPP_NOEXCEPT override;
+};
+
+template <typename T>
+class YAML_CPP_API TypedKeyNotFound : public KeyNotFound {
+ public:
+  TypedKeyNotFound(const Mark& mark_, const T& key_)
+      : KeyNotFound(mark_, key_), key(key_) {}
+  ~TypedKeyNotFound() YAML_CPP_NOEXCEPT override = default;
+
+  T key;
+};
+
+template <typename T>
+inline TypedKeyNotFound<T> MakeTypedKeyNotFound(const Mark& mark,
+                                                const T& key) {
+  return TypedKeyNotFound<T>(mark, key);
+}
+
+class YAML_CPP_API InvalidNode : public RepresentationException {
+ public:
+  InvalidNode(const std::string& key)
+      : RepresentationException(Mark::null_mark(),
+                                ErrorMsg::INVALID_NODE_WITH_KEY(key)) {}
+  InvalidNode(const InvalidNode&) = default;
+  ~InvalidNode() YAML_CPP_NOEXCEPT override;
+};
+
+class YAML_CPP_API BadConversion : public RepresentationException {
+ public:
+  explicit BadConversion(const Mark& mark_)
+      : RepresentationException(mark_, ErrorMsg::BAD_CONVERSION) {}
+  BadConversion(const BadConversion&) = default;
+  ~BadConversion() YAML_CPP_NOEXCEPT override;
+};
+
+template <typename T>
+class TypedBadConversion : public BadConversion {
+ public:
+  explicit TypedBadConversion(const Mark& mark_) : BadConversion(mark_) {}
+};
+
+class YAML_CPP_API BadDereference : public RepresentationException {
+ public:
+  BadDereference()
+      : RepresentationException(Mark::null_mark(), ErrorMsg::BAD_DEREFERENCE) {}
+  BadDereference(const BadDereference&) = default;
+  ~BadDereference() YAML_CPP_NOEXCEPT override;
+};
+
+class YAML_CPP_API BadSubscript : public RepresentationException {
+ public:
+  template <typename Key>
+  BadSubscript(const Mark& mark_, const Key& key)
+      : RepresentationException(mark_, ErrorMsg::BAD_SUBSCRIPT_WITH_KEY(key)) {}
+  BadSubscript(const BadSubscript&) = default;
+  ~BadSubscript() YAML_CPP_NOEXCEPT override;
+};
+
+class YAML_CPP_API BadPushback : public RepresentationException {
+ public:
+  BadPushback()
+      : RepresentationException(Mark::null_mark(), ErrorMsg::BAD_PUSHBACK) {}
+  BadPushback(const BadPushback&) = default;
+  ~BadPushback() YAML_CPP_NOEXCEPT override;
+};
+
+class YAML_CPP_API BadInsert : public RepresentationException {
+ public:
+  BadInsert()
+      : RepresentationException(Mark::null_mark(), ErrorMsg::BAD_INSERT) {}
+  BadInsert(const BadInsert&) = default;
+  ~BadInsert() YAML_CPP_NOEXCEPT override;
+};
+
+class YAML_CPP_API EmitterException : public Exception {
+ public:
+  EmitterException(const std::string& msg_)
+      : Exception(Mark::null_mark(), msg_) {}
+  EmitterException(const EmitterException&) = default;
+  ~EmitterException() YAML_CPP_NOEXCEPT override;
+};
+
+class YAML_CPP_API BadFile : public Exception {
+ public:
+  explicit BadFile(const std::string& filename)
+      : Exception(Mark::null_mark(),
+                  std::string(ErrorMsg::BAD_FILE) + ": " + filename) {}
+  BadFile(const BadFile&) = default;
+  ~BadFile() YAML_CPP_NOEXCEPT override;
+};
+}  // namespace YAML
+
+#endif  // EXCEPTIONS_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/mark.h

@@ -0,0 +1,29 @@
+#ifndef MARK_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define MARK_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/dll.h"
+
+namespace YAML {
+struct YAML_CPP_API Mark {
+  Mark() : pos(0), line(0), column(0) {}
+
+  static const Mark null_mark() { return Mark(-1, -1, -1); }
+
+  bool is_null() const { return pos == -1 && line == -1 && column == -1; }
+
+  int pos;
+  int line, column;
+
+ private:
+  Mark(int pos_, int line_, int column_)
+      : pos(pos_), line(line_), column(column_) {}
+};
+}
+
+#endif  // MARK_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/convert.h

@@ -0,0 +1,451 @@
+#ifndef NODE_CONVERT_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define NODE_CONVERT_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <array>
+#include <cmath>
+#include <limits>
+#include <list>
+#include <map>
+#include <unordered_map>
+#include <sstream>
+#include <type_traits>
+#include <valarray>
+#include <vector>
+
+#include "yaml-cpp/binary.h"
+#include "yaml-cpp/node/impl.h"
+#include "yaml-cpp/node/iterator.h"
+#include "yaml-cpp/node/node.h"
+#include "yaml-cpp/node/type.h"
+#include "yaml-cpp/null.h"
+
+
+namespace YAML {
+class Binary;
+struct _Null;
+template <typename T>
+struct convert;
+}  // namespace YAML
+
+namespace YAML {
+namespace conversion {
+inline bool IsInfinity(const std::string& input) {
+  return input == ".inf" || input == ".Inf" || input == ".INF" ||
+         input == "+.inf" || input == "+.Inf" || input == "+.INF";
+}
+
+inline bool IsNegativeInfinity(const std::string& input) {
+  return input == "-.inf" || input == "-.Inf" || input == "-.INF";
+}
+
+inline bool IsNaN(const std::string& input) {
+  return input == ".nan" || input == ".NaN" || input == ".NAN";
+}
+}
+
+// Node
+template <>
+struct convert<Node> {
+  static Node encode(const Node& rhs) { return rhs; }
+
+  static bool decode(const Node& node, Node& rhs) {
+    rhs.reset(node);
+    return true;
+  }
+};
+
+// std::string
+template <>
+struct convert<std::string> {
+  static Node encode(const std::string& rhs) { return Node(rhs); }
+
+  static bool decode(const Node& node, std::string& rhs) {
+    if (!node.IsScalar())
+      return false;
+    rhs = node.Scalar();
+    return true;
+  }
+};
+
+// C-strings can only be encoded
+template <>
+struct convert<const char*> {
+  static Node encode(const char* rhs) { return Node(rhs); }
+};
+
+template <>
+struct convert<char*> {
+  static Node encode(const char* rhs) { return Node(rhs); }
+};
+
+template <std::size_t N>
+struct convert<char[N]> {
+  static Node encode(const char* rhs) { return Node(rhs); }
+};
+
+template <>
+struct convert<_Null> {
+  static Node encode(const _Null& /* rhs */) { return Node(); }
+
+  static bool decode(const Node& node, _Null& /* rhs */) {
+    return node.IsNull();
+  }
+};
+
+namespace conversion {
+template <typename T>
+typename std::enable_if< std::is_floating_point<T>::value, void>::type
+inner_encode(const T& rhs, std::stringstream& stream){
+  if (std::isnan(rhs)) {
+    stream << ".nan";
+  } else if (std::isinf(rhs)) {
+    if (std::signbit(rhs)) {
+      stream << "-.inf";
+    } else {
+      stream << ".inf";
+    }
+  } else {
+    stream << rhs;
+  }
+}
+
+template <typename T>
+typename std::enable_if<!std::is_floating_point<T>::value, void>::type
+inner_encode(const T& rhs, std::stringstream& stream){
+  stream << rhs;
+}
+
+template <typename T>
+typename std::enable_if<(std::is_same<T, unsigned char>::value ||
+                         std::is_same<T, signed char>::value), bool>::type
+ConvertStreamTo(std::stringstream& stream, T& rhs) {
+  int num;
+  if ((stream >> std::noskipws >> num) && (stream >> std::ws).eof()) {
+    if (num >= (std::numeric_limits<T>::min)() &&
+        num <= (std::numeric_limits<T>::max)()) {
+      rhs = static_cast<T>(num);
+      return true;
+    }
+  }
+  return false;
+}
+
+template <typename T>
+typename std::enable_if<!(std::is_same<T, unsigned char>::value ||
+                          std::is_same<T, signed char>::value), bool>::type
+ConvertStreamTo(std::stringstream& stream, T& rhs) {
+  if ((stream >> std::noskipws >> rhs) && (stream >> std::ws).eof()) {
+    return true;
+  }
+  return false;
+}
+}
+
+#define YAML_DEFINE_CONVERT_STREAMABLE(type, negative_op)                  \
+  template <>                                                              \
+  struct convert<type> {                                                   \
+                                                                           \
+    static Node encode(const type& rhs) {                                  \
+      std::stringstream stream;                                            \
+      stream.precision(std::numeric_limits<type>::max_digits10);           \
+      conversion::inner_encode(rhs, stream);                               \
+      return Node(stream.str());                                           \
+    }                                                                      \
+                                                                           \
+    static bool decode(const Node& node, type& rhs) {                      \
+      if (node.Type() != NodeType::Scalar) {                               \
+        return false;                                                      \
+      }                                                                    \
+      const std::string& input = node.Scalar();                            \
+      std::stringstream stream(input);                                     \
+      stream.unsetf(std::ios::dec);                                        \
+      if ((stream.peek() == '-') && std::is_unsigned<type>::value) {       \
+        return false;                                                      \
+      }                                                                    \
+      if (conversion::ConvertStreamTo(stream, rhs)) {                      \
+        return true;                                                       \
+      }                                                                    \
+      if (std::numeric_limits<type>::has_infinity) {                       \
+        if (conversion::IsInfinity(input)) {                               \
+          rhs = std::numeric_limits<type>::infinity();                     \
+          return true;                                                     \
+        } else if (conversion::IsNegativeInfinity(input)) {                \
+          rhs = negative_op std::numeric_limits<type>::infinity();         \
+          return true;                                                     \
+        }                                                                  \
+      }                                                                    \
+                                                                           \
+      if (std::numeric_limits<type>::has_quiet_NaN) {                      \
+        if (conversion::IsNaN(input)) {                                    \
+          rhs = std::numeric_limits<type>::quiet_NaN();                    \
+          return true;                                                     \
+        }                                                                  \
+      }                                                                    \
+                                                                           \
+      return false;                                                        \
+    }                                                                      \
+  }
+
+#define YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(type) \
+  YAML_DEFINE_CONVERT_STREAMABLE(type, -)
+
+#define YAML_DEFINE_CONVERT_STREAMABLE_UNSIGNED(type) \
+  YAML_DEFINE_CONVERT_STREAMABLE(type, +)
+
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(int);
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(short);
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(long);
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(long long);
+YAML_DEFINE_CONVERT_STREAMABLE_UNSIGNED(unsigned);
+YAML_DEFINE_CONVERT_STREAMABLE_UNSIGNED(unsigned short);
+YAML_DEFINE_CONVERT_STREAMABLE_UNSIGNED(unsigned long);
+YAML_DEFINE_CONVERT_STREAMABLE_UNSIGNED(unsigned long long);
+
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(char);
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(signed char);
+YAML_DEFINE_CONVERT_STREAMABLE_UNSIGNED(unsigned char);
+
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(float);
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(double);
+YAML_DEFINE_CONVERT_STREAMABLE_SIGNED(long double);
+
+#undef YAML_DEFINE_CONVERT_STREAMABLE_SIGNED
+#undef YAML_DEFINE_CONVERT_STREAMABLE_UNSIGNED
+#undef YAML_DEFINE_CONVERT_STREAMABLE
+
+// bool
+template <>
+struct convert<bool> {
+  static Node encode(bool rhs) { return rhs ? Node("true") : Node("false"); }
+
+  YAML_CPP_API static bool decode(const Node& node, bool& rhs);
+};
+
+// std::map
+template <typename K, typename V, typename C, typename A>
+struct convert<std::map<K, V, C, A>> {
+  static Node encode(const std::map<K, V, C, A>& rhs) {
+    Node node(NodeType::Map);
+    for (const auto& element : rhs)
+      node.force_insert(element.first, element.second);
+    return node;
+  }
+
+  static bool decode(const Node& node, std::map<K, V, C, A>& rhs) {
+    if (!node.IsMap())
+      return false;
+
+    rhs.clear();
+    for (const auto& element : node)
+#if defined(__GNUC__) && __GNUC__ < 4
+      // workaround for GCC 3:
+      rhs[element.first.template as<K>()] = element.second.template as<V>();
+#else
+      rhs[element.first.as<K>()] = element.second.as<V>();
+#endif
+    return true;
+  }
+};
+
+// std::unordered_map
+template <typename K, typename V, typename H, typename P, typename A>
+struct convert<std::unordered_map<K, V, H, P, A>> {
+  static Node encode(const std::unordered_map<K, V, H, P, A>& rhs) {
+    Node node(NodeType::Map);
+    for (const auto& element : rhs)
+      node.force_insert(element.first, element.second);
+    return node;
+  }
+
+  static bool decode(const Node& node, std::unordered_map<K, V, H, P, A>& rhs) {
+    if (!node.IsMap())
+      return false;
+
+    rhs.clear();
+    for (const auto& element : node)
+#if defined(__GNUC__) && __GNUC__ < 4
+      // workaround for GCC 3:
+      rhs[element.first.template as<K>()] = element.second.template as<V>();
+#else
+      rhs[element.first.as<K>()] = element.second.as<V>();
+#endif
+    return true;
+  }
+};
+
+// std::vector
+template <typename T, typename A>
+struct convert<std::vector<T, A>> {
+  static Node encode(const std::vector<T, A>& rhs) {
+    Node node(NodeType::Sequence);
+    for (const auto& element : rhs)
+      node.push_back(element);
+    return node;
+  }
+
+  static bool decode(const Node& node, std::vector<T, A>& rhs) {
+    if (!node.IsSequence())
+      return false;
+
+    rhs.clear();
+    for (const auto& element : node)
+#if defined(__GNUC__) && __GNUC__ < 4
+      // workaround for GCC 3:
+      rhs.push_back(element.template as<T>());
+#else
+      rhs.push_back(element.as<T>());
+#endif
+    return true;
+  }
+};
+
+// std::list
+template <typename T, typename A>
+struct convert<std::list<T,A>> {
+  static Node encode(const std::list<T,A>& rhs) {
+    Node node(NodeType::Sequence);
+    for (const auto& element : rhs)
+      node.push_back(element);
+    return node;
+  }
+
+  static bool decode(const Node& node, std::list<T,A>& rhs) {
+    if (!node.IsSequence())
+      return false;
+
+    rhs.clear();
+    for (const auto& element : node)
+#if defined(__GNUC__) && __GNUC__ < 4
+      // workaround for GCC 3:
+      rhs.push_back(element.template as<T>());
+#else
+      rhs.push_back(element.as<T>());
+#endif
+    return true;
+  }
+};
+
+// std::array
+template <typename T, std::size_t N>
+struct convert<std::array<T, N>> {
+  static Node encode(const std::array<T, N>& rhs) {
+    Node node(NodeType::Sequence);
+    for (const auto& element : rhs) {
+      node.push_back(element);
+    }
+    return node;
+  }
+
+  static bool decode(const Node& node, std::array<T, N>& rhs) {
+    if (!isNodeValid(node)) {
+      return false;
+    }
+
+    for (auto i = 0u; i < node.size(); ++i) {
+#if defined(__GNUC__) && __GNUC__ < 4
+      // workaround for GCC 3:
+      rhs[i] = node[i].template as<T>();
+#else
+      rhs[i] = node[i].as<T>();
+#endif
+    }
+    return true;
+  }
+
+ private:
+  static bool isNodeValid(const Node& node) {
+    return node.IsSequence() && node.size() == N;
+  }
+};
+
+
+// std::valarray
+template <typename T>
+struct convert<std::valarray<T>> {
+  static Node encode(const std::valarray<T>& rhs) {
+    Node node(NodeType::Sequence);
+    for (const auto& element : rhs) {
+      node.push_back(element);
+    }
+    return node;
+  }
+
+  static bool decode(const Node& node, std::valarray<T>& rhs) {
+    if (!node.IsSequence()) {
+      return false;
+    }
+
+    rhs.resize(node.size());
+    for (auto i = 0u; i < node.size(); ++i) {
+#if defined(__GNUC__) && __GNUC__ < 4
+      // workaround for GCC 3:
+      rhs[i] = node[i].template as<T>();
+#else
+      rhs[i] = node[i].as<T>();
+#endif
+    }
+    return true;
+  }
+};
+
+
+// std::pair
+template <typename T, typename U>
+struct convert<std::pair<T, U>> {
+  static Node encode(const std::pair<T, U>& rhs) {
+    Node node(NodeType::Sequence);
+    node.push_back(rhs.first);
+    node.push_back(rhs.second);
+    return node;
+  }
+
+  static bool decode(const Node& node, std::pair<T, U>& rhs) {
+    if (!node.IsSequence())
+      return false;
+    if (node.size() != 2)
+      return false;
+
+#if defined(__GNUC__) && __GNUC__ < 4
+    // workaround for GCC 3:
+    rhs.first = node[0].template as<T>();
+#else
+    rhs.first = node[0].as<T>();
+#endif
+#if defined(__GNUC__) && __GNUC__ < 4
+    // workaround for GCC 3:
+    rhs.second = node[1].template as<U>();
+#else
+    rhs.second = node[1].as<U>();
+#endif
+    return true;
+  }
+};
+
+// binary
+template <>
+struct convert<Binary> {
+  static Node encode(const Binary& rhs) {
+    return Node(EncodeBase64(rhs.data(), rhs.size()));
+  }
+
+  static bool decode(const Node& node, Binary& rhs) {
+    if (!node.IsScalar())
+      return false;
+
+    std::vector<unsigned char> data = DecodeBase64(node.Scalar());
+    if (data.empty() && !node.Scalar().empty())
+      return false;
+
+    rhs.swap(data);
+    return true;
+  }
+};
+}
+
+#endif  // NODE_CONVERT_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/detail/impl.h

@@ -0,0 +1,235 @@
+#ifndef NODE_DETAIL_IMPL_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define NODE_DETAIL_IMPL_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/node/detail/node.h"
+#include "yaml-cpp/node/detail/node_data.h"
+
+#include <algorithm>
+#include <type_traits>
+
+namespace YAML {
+namespace detail {
+template <typename Key, typename Enable = void>
+struct get_idx {
+  static node* get(const std::vector<node*>& /* sequence */,
+                   const Key& /* key */, shared_memory_holder /* pMemory */) {
+    return nullptr;
+  }
+};
+
+template <typename Key>
+struct get_idx<Key,
+               typename std::enable_if<std::is_unsigned<Key>::value &&
+                                       !std::is_same<Key, bool>::value>::type> {
+  static node* get(const std::vector<node*>& sequence, const Key& key,
+                   shared_memory_holder /* pMemory */) {
+    return key < sequence.size() ? sequence[key] : nullptr;
+  }
+
+  static node* get(std::vector<node*>& sequence, const Key& key,
+                   shared_memory_holder pMemory) {
+    if (key > sequence.size() || (key > 0 && !sequence[key - 1]->is_defined()))
+      return nullptr;
+    if (key == sequence.size())
+      sequence.push_back(&pMemory->create_node());
+    return sequence[key];
+  }
+};
+
+template <typename Key>
+struct get_idx<Key, typename std::enable_if<std::is_signed<Key>::value>::type> {
+  static node* get(const std::vector<node*>& sequence, const Key& key,
+                   shared_memory_holder pMemory) {
+    return key >= 0 ? get_idx<std::size_t>::get(
+                          sequence, static_cast<std::size_t>(key), pMemory)
+                    : nullptr;
+  }
+  static node* get(std::vector<node*>& sequence, const Key& key,
+                   shared_memory_holder pMemory) {
+    return key >= 0 ? get_idx<std::size_t>::get(
+                          sequence, static_cast<std::size_t>(key), pMemory)
+                    : nullptr;
+  }
+};
+
+template <typename Key, typename Enable = void>
+struct remove_idx {
+  static bool remove(std::vector<node*>&, const Key&, std::size_t&) {
+    return false;
+  }
+};
+
+template <typename Key>
+struct remove_idx<
+    Key, typename std::enable_if<std::is_unsigned<Key>::value &&
+                                 !std::is_same<Key, bool>::value>::type> {
+
+  static bool remove(std::vector<node*>& sequence, const Key& key,
+                     std::size_t& seqSize) {
+    if (key >= sequence.size()) {
+      return false;
+    } else {
+      sequence.erase(sequence.begin() + key);
+      if (seqSize > key) {
+          --seqSize;
+      }
+      return true;
+    }
+  }
+};
+
+template <typename Key>
+struct remove_idx<Key,
+                  typename std::enable_if<std::is_signed<Key>::value>::type> {
+
+  static bool remove(std::vector<node*>& sequence, const Key& key,
+                     std::size_t& seqSize) {
+    return key >= 0 ? remove_idx<std::size_t>::remove(
+                          sequence, static_cast<std::size_t>(key), seqSize)
+                    : false;
+  }
+};
+
+template <typename T>
+inline bool node::equals(const T& rhs, shared_memory_holder pMemory) {
+  T lhs;
+  if (convert<T>::decode(Node(*this, pMemory), lhs)) {
+    return lhs == rhs;
+  }
+  return false;
+}
+
+inline bool node::equals(const char* rhs, shared_memory_holder pMemory) {
+  std::string lhs;
+  if (convert<std::string>::decode(Node(*this, std::move(pMemory)), lhs)) {
+    return lhs == rhs;
+  }
+  return false;
+}
+
+// indexing
+template <typename Key>
+inline node* node_data::get(const Key& key,
+                            shared_memory_holder pMemory) const {
+  switch (m_type) {
+    case NodeType::Map:
+      break;
+    case NodeType::Undefined:
+    case NodeType::Null:
+      return nullptr;
+    case NodeType::Sequence:
+      if (node* pNode = get_idx<Key>::get(m_sequence, key, pMemory))
+        return pNode;
+      return nullptr;
+    case NodeType::Scalar:
+      throw BadSubscript(m_mark, key);
+  }
+
+  auto it = std::find_if(m_map.begin(), m_map.end(), [&](const kv_pair m) {
+    return m.first->equals(key, pMemory);
+  });
+
+  return it != m_map.end() ? it->second : nullptr;
+}
+
+template <typename Key>
+inline node& node_data::get(const Key& key, shared_memory_holder pMemory) {
+  switch (m_type) {
+    case NodeType::Map:
+      break;
+    case NodeType::Undefined:
+    case NodeType::Null:
+    case NodeType::Sequence:
+      if (node* pNode = get_idx<Key>::get(m_sequence, key, pMemory)) {
+        m_type = NodeType::Sequence;
+        return *pNode;
+      }
+
+      convert_to_map(pMemory);
+      break;
+    case NodeType::Scalar:
+      throw BadSubscript(m_mark, key);
+  }
+
+  auto it = std::find_if(m_map.begin(), m_map.end(), [&](const kv_pair m) {
+    return m.first->equals(key, pMemory);
+  });
+
+  if (it != m_map.end()) {
+    return *it->second;
+  }
+
+  node& k = convert_to_node(key, pMemory);
+  node& v = pMemory->create_node();
+  insert_map_pair(k, v);
+  return v;
+}
+
+template <typename Key>
+inline bool node_data::remove(const Key& key, shared_memory_holder pMemory) {
+  if (m_type == NodeType::Sequence) {
+    return remove_idx<Key>::remove(m_sequence, key, m_seqSize);
+  }
+
+  if (m_type == NodeType::Map) {
+    kv_pairs::iterator it = m_undefinedPairs.begin();
+    while (it != m_undefinedPairs.end()) {
+      kv_pairs::iterator jt = std::next(it);
+      if (it->first->equals(key, pMemory)) {
+        m_undefinedPairs.erase(it);
+      }
+      it = jt;
+    }
+
+    auto iter = std::find_if(m_map.begin(), m_map.end(), [&](const kv_pair m) {
+      return m.first->equals(key, pMemory);
+    });
+
+    if (iter != m_map.end()) {
+      m_map.erase(iter);
+      return true;
+    }
+  }
+
+  return false;
+}
+
+// map
+template <typename Key, typename Value>
+inline void node_data::force_insert(const Key& key, const Value& value,
+                                    shared_memory_holder pMemory) {
+  switch (m_type) {
+    case NodeType::Map:
+      break;
+    case NodeType::Undefined:
+    case NodeType::Null:
+    case NodeType::Sequence:
+      convert_to_map(pMemory);
+      break;
+    case NodeType::Scalar:
+      throw BadInsert();
+  }
+
+  node& k = convert_to_node(key, pMemory);
+  node& v = convert_to_node(value, pMemory);
+  insert_map_pair(k, v);
+}
+
+template <typename T>
+inline node& node_data::convert_to_node(const T& rhs,
+                                        shared_memory_holder pMemory) {
+  Node value = convert<T>::encode(rhs);
+  value.EnsureNodeExists();
+  pMemory->merge(*value.m_pMemory);
+  return *value.m_pNode;
+}
+}
+}
+
+#endif  // NODE_DETAIL_IMPL_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/detail/iterator.h

@@ -0,0 +1,96 @@
+#ifndef VALUE_DETAIL_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_DETAIL_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/node/detail/node_iterator.h"
+#include "yaml-cpp/node/node.h"
+#include "yaml-cpp/node/ptr.h"
+#include <cstddef>
+#include <iterator>
+
+
+namespace YAML {
+namespace detail {
+struct iterator_value;
+
+template <typename V>
+class iterator_base {
+
+ private:
+  template <typename>
+  friend class iterator_base;
+  struct enabler {};
+  using base_type = node_iterator;
+
+  struct proxy {
+    explicit proxy(const V& x) : m_ref(x) {}
+    V* operator->() { return std::addressof(m_ref); }
+    operator V*() { return std::addressof(m_ref); }
+
+    V m_ref;
+  };
+
+ public:
+  using iterator_category = std::forward_iterator_tag;
+  using value_type = V;
+  using difference_type = std::ptrdiff_t;
+  using pointer = V*;
+  using reference = V;
+
+ public:
+  iterator_base() : m_iterator(), m_pMemory() {}
+  explicit iterator_base(base_type rhs, shared_memory_holder pMemory)
+      : m_iterator(rhs), m_pMemory(pMemory) {}
+
+  template <class W>
+  iterator_base(const iterator_base<W>& rhs,
+                typename std::enable_if<std::is_convertible<W*, V*>::value,
+                                        enabler>::type = enabler())
+      : m_iterator(rhs.m_iterator), m_pMemory(rhs.m_pMemory) {}
+
+  iterator_base<V>& operator++() {
+    ++m_iterator;
+    return *this;
+  }
+
+  iterator_base<V> operator++(int) {
+    iterator_base<V> iterator_pre(*this);
+    ++(*this);
+    return iterator_pre;
+  }
+
+  template <typename W>
+  bool operator==(const iterator_base<W>& rhs) const {
+    return m_iterator == rhs.m_iterator;
+  }
+
+  template <typename W>
+  bool operator!=(const iterator_base<W>& rhs) const {
+    return m_iterator != rhs.m_iterator;
+  }
+
+  value_type operator*() const {
+    const typename base_type::value_type& v = *m_iterator;
+    if (v.pNode)
+      return value_type(Node(*v, m_pMemory));
+    if (v.first && v.second)
+      return value_type(Node(*v.first, m_pMemory), Node(*v.second, m_pMemory));
+    return value_type();
+  }
+
+  proxy operator->() const { return proxy(**this); }
+
+ private:
+  base_type m_iterator;
+  shared_memory_holder m_pMemory;
+};
+}  // namespace detail
+}  // namespace YAML
+
+#endif  // VALUE_DETAIL_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/detail/iterator_fwd.h

@@ -0,0 +1,27 @@
+#ifndef VALUE_DETAIL_ITERATOR_FWD_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_DETAIL_ITERATOR_FWD_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/dll.h"
+#include <list>
+#include <utility>
+#include <vector>
+
+namespace YAML {
+
+namespace detail {
+struct iterator_value;
+template <typename V>
+class iterator_base;
+}
+
+using iterator = detail::iterator_base<detail::iterator_value>;
+using const_iterator = detail::iterator_base<const detail::iterator_value>;
+}
+
+#endif  // VALUE_DETAIL_ITERATOR_FWD_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/detail/memory.h

@@ -0,0 +1,47 @@
+#ifndef VALUE_DETAIL_MEMORY_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_DETAIL_MEMORY_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <set>
+
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/node/ptr.h"
+
+namespace YAML {
+namespace detail {
+class node;
+}  // namespace detail
+}  // namespace YAML
+
+namespace YAML {
+namespace detail {
+class YAML_CPP_API memory {
+ public:
+  memory() : m_nodes{} {}
+  node& create_node();
+  void merge(const memory& rhs);
+
+ private:
+  using Nodes = std::set<shared_node>;
+  Nodes m_nodes;
+};
+
+class YAML_CPP_API memory_holder {
+ public:
+  memory_holder() : m_pMemory(new memory) {}
+
+  node& create_node() { return m_pMemory->create_node(); }
+  void merge(memory_holder& rhs);
+
+ private:
+  shared_memory m_pMemory;
+};
+}  // namespace detail
+}  // namespace YAML
+
+#endif  // VALUE_DETAIL_MEMORY_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/detail/node.h

@@ -0,0 +1,177 @@
+#ifndef NODE_DETAIL_NODE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define NODE_DETAIL_NODE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/emitterstyle.h"
+#include "yaml-cpp/node/detail/node_ref.h"
+#include "yaml-cpp/node/ptr.h"
+#include "yaml-cpp/node/type.h"
+#include <set>
+#include <atomic>
+
+namespace YAML {
+namespace detail {
+class node {
+ private:
+  struct less {
+    bool operator ()(const node* l, const node* r) const {return l->m_index < r->m_index;}
+  };
+
+ public:
+  node() : m_pRef(new node_ref), m_dependencies{}, m_index{} {}
+  node(const node&) = delete;
+  node& operator=(const node&) = delete;
+
+  bool is(const node& rhs) const { return m_pRef == rhs.m_pRef; }
+  const node_ref* ref() const { return m_pRef.get(); }
+
+  bool is_defined() const { return m_pRef->is_defined(); }
+  const Mark& mark() const { return m_pRef->mark(); }
+  NodeType::value type() const { return m_pRef->type(); }
+
+  const std::string& scalar() const { return m_pRef->scalar(); }
+  const std::string& tag() const { return m_pRef->tag(); }
+  EmitterStyle::value style() const { return m_pRef->style(); }
+
+  template <typename T>
+  bool equals(const T& rhs, shared_memory_holder pMemory);
+  bool equals(const char* rhs, shared_memory_holder pMemory);
+
+  void mark_defined() {
+    if (is_defined())
+      return;
+
+    m_pRef->mark_defined();
+    for (node* dependency : m_dependencies)
+      dependency->mark_defined();
+    m_dependencies.clear();
+  }
+
+  void add_dependency(node& rhs) {
+    if (is_defined())
+      rhs.mark_defined();
+    else
+      m_dependencies.insert(&rhs);
+  }
+
+  void set_ref(const node& rhs) {
+    if (rhs.is_defined())
+      mark_defined();
+    m_pRef = rhs.m_pRef;
+  }
+  void set_data(const node& rhs) {
+    if (rhs.is_defined())
+      mark_defined();
+    m_pRef->set_data(*rhs.m_pRef);
+  }
+
+  void set_mark(const Mark& mark) { m_pRef->set_mark(mark); }
+
+  void set_type(NodeType::value type) {
+    if (type != NodeType::Undefined)
+      mark_defined();
+    m_pRef->set_type(type);
+  }
+  void set_null() {
+    mark_defined();
+    m_pRef->set_null();
+  }
+  void set_scalar(const std::string& scalar) {
+    mark_defined();
+    m_pRef->set_scalar(scalar);
+  }
+  void set_tag(const std::string& tag) {
+    mark_defined();
+    m_pRef->set_tag(tag);
+  }
+
+  // style
+  void set_style(EmitterStyle::value style) {
+    mark_defined();
+    m_pRef->set_style(style);
+  }
+
+  // size/iterator
+  std::size_t size() const { return m_pRef->size(); }
+
+  const_node_iterator begin() const {
+    return static_cast<const node_ref&>(*m_pRef).begin();
+  }
+  node_iterator begin() { return m_pRef->begin(); }
+
+  const_node_iterator end() const {
+    return static_cast<const node_ref&>(*m_pRef).end();
+  }
+  node_iterator end() { return m_pRef->end(); }
+
+  // sequence
+  void push_back(node& input, shared_memory_holder pMemory) {
+    m_pRef->push_back(input, pMemory);
+    input.add_dependency(*this);
+    m_index = m_amount.fetch_add(1);
+  }
+  void insert(node& key, node& value, shared_memory_holder pMemory) {
+    m_pRef->insert(key, value, pMemory);
+    key.add_dependency(*this);
+    value.add_dependency(*this);
+  }
+
+  // indexing
+  template <typename Key>
+  node* get(const Key& key, shared_memory_holder pMemory) const {
+    // NOTE: this returns a non-const node so that the top-level Node can wrap
+    // it, and returns a pointer so that it can be nullptr (if there is no such
+    // key).
+    return static_cast<const node_ref&>(*m_pRef).get(key, pMemory);
+  }
+  template <typename Key>
+  node& get(const Key& key, shared_memory_holder pMemory) {
+    node& value = m_pRef->get(key, pMemory);
+    value.add_dependency(*this);
+    return value;
+  }
+  template <typename Key>
+  bool remove(const Key& key, shared_memory_holder pMemory) {
+    return m_pRef->remove(key, pMemory);
+  }
+
+  node* get(node& key, shared_memory_holder pMemory) const {
+    // NOTE: this returns a non-const node so that the top-level Node can wrap
+    // it, and returns a pointer so that it can be nullptr (if there is no such
+    // key).
+    return static_cast<const node_ref&>(*m_pRef).get(key, pMemory);
+  }
+  node& get(node& key, shared_memory_holder pMemory) {
+    node& value = m_pRef->get(key, pMemory);
+    key.add_dependency(*this);
+    value.add_dependency(*this);
+    return value;
+  }
+  bool remove(node& key, shared_memory_holder pMemory) {
+    return m_pRef->remove(key, pMemory);
+  }
+
+  // map
+  template <typename Key, typename Value>
+  void force_insert(const Key& key, const Value& value,
+                    shared_memory_holder pMemory) {
+    m_pRef->force_insert(key, value, pMemory);
+  }
+
+ private:
+  shared_node_ref m_pRef;
+  using nodes = std::set<node*, less>;
+  nodes m_dependencies;
+  size_t m_index;
+  static YAML_CPP_API std::atomic<size_t> m_amount;
+};
+}  // namespace detail
+}  // namespace YAML
+
+#endif  // NODE_DETAIL_NODE_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/detail/node_data.h

@@ -0,0 +1,127 @@
+#ifndef VALUE_DETAIL_NODE_DATA_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_DETAIL_NODE_DATA_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <list>
+#include <map>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/node/detail/node_iterator.h"
+#include "yaml-cpp/node/iterator.h"
+#include "yaml-cpp/node/ptr.h"
+#include "yaml-cpp/node/type.h"
+
+namespace YAML {
+namespace detail {
+class node;
+}  // namespace detail
+}  // namespace YAML
+
+namespace YAML {
+namespace detail {
+class YAML_CPP_API node_data {
+ public:
+  node_data();
+  node_data(const node_data&) = delete;
+  node_data& operator=(const node_data&) = delete;
+
+  void mark_defined();
+  void set_mark(const Mark& mark);
+  void set_type(NodeType::value type);
+  void set_tag(const std::string& tag);
+  void set_null();
+  void set_scalar(const std::string& scalar);
+  void set_style(EmitterStyle::value style);
+
+  bool is_defined() const { return m_isDefined; }
+  const Mark& mark() const { return m_mark; }
+  NodeType::value type() const {
+    return m_isDefined ? m_type : NodeType::Undefined;
+  }
+  const std::string& scalar() const { return m_scalar; }
+  const std::string& tag() const { return m_tag; }
+  EmitterStyle::value style() const { return m_style; }
+
+  // size/iterator
+  std::size_t size() const;
+
+  const_node_iterator begin() const;
+  node_iterator begin();
+
+  const_node_iterator end() const;
+  node_iterator end();
+
+  // sequence
+  void push_back(node& node, const shared_memory_holder& pMemory);
+  void insert(node& key, node& value, const shared_memory_holder& pMemory);
+
+  // indexing
+  template <typename Key>
+  node* get(const Key& key, shared_memory_holder pMemory) const;
+  template <typename Key>
+  node& get(const Key& key, shared_memory_holder pMemory);
+  template <typename Key>
+  bool remove(const Key& key, shared_memory_holder pMemory);
+
+  node* get(node& key, const shared_memory_holder& pMemory) const;
+  node& get(node& key, const shared_memory_holder& pMemory);
+  bool remove(node& key, const shared_memory_holder& pMemory);
+
+  // map
+  template <typename Key, typename Value>
+  void force_insert(const Key& key, const Value& value,
+                    shared_memory_holder pMemory);
+
+ public:
+  static const std::string& empty_scalar();
+
+ private:
+  void compute_seq_size() const;
+  void compute_map_size() const;
+
+  void reset_sequence();
+  void reset_map();
+
+  void insert_map_pair(node& key, node& value);
+  void convert_to_map(const shared_memory_holder& pMemory);
+  void convert_sequence_to_map(const shared_memory_holder& pMemory);
+
+  template <typename T>
+  static node& convert_to_node(const T& rhs, shared_memory_holder pMemory);
+
+ private:
+  bool m_isDefined;
+  Mark m_mark;
+  NodeType::value m_type;
+  std::string m_tag;
+  EmitterStyle::value m_style;
+
+  // scalar
+  std::string m_scalar;
+
+  // sequence
+  using node_seq = std::vector<node *>;
+  node_seq m_sequence;
+
+  mutable std::size_t m_seqSize;
+
+  // map
+  using node_map = std::vector<std::pair<node*, node*>>;
+  node_map m_map;
+
+  using kv_pair = std::pair<node*, node*>;
+  using kv_pairs = std::list<kv_pair>;
+  mutable kv_pairs m_undefinedPairs;
+};
+}
+}
+
+#endif  // VALUE_DETAIL_NODE_DATA_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/detail/node_iterator.h

@@ -0,0 +1,181 @@
+#ifndef VALUE_DETAIL_NODE_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_DETAIL_NODE_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/node/ptr.h"
+#include <cstddef>
+#include <iterator>
+#include <memory>
+#include <map>
+#include <utility>
+#include <vector>
+
+namespace YAML {
+namespace detail {
+struct iterator_type {
+  enum value { NoneType, Sequence, Map };
+};
+
+template <typename V>
+struct node_iterator_value : public std::pair<V*, V*> {
+  using kv = std::pair<V*, V*>;
+
+  node_iterator_value() : kv(), pNode(nullptr) {}
+  explicit node_iterator_value(V& rhs) : kv(), pNode(&rhs) {}
+  explicit node_iterator_value(V& key, V& value) : kv(&key, &value), pNode(nullptr) {}
+
+  V& operator*() const { return *pNode; }
+  V& operator->() const { return *pNode; }
+
+  V* pNode;
+};
+
+using node_seq = std::vector<node *>;
+using node_map = std::vector<std::pair<node*, node*>>;
+
+template <typename V>
+struct node_iterator_type {
+  using seq = node_seq::iterator;
+  using map = node_map::iterator;
+};
+
+template <typename V>
+struct node_iterator_type<const V> {
+  using seq = node_seq::const_iterator;
+  using map = node_map::const_iterator;
+};
+
+template <typename V>
+class node_iterator_base {
+ private:
+  struct enabler {};
+
+  struct proxy {
+    explicit proxy(const node_iterator_value<V>& x) : m_ref(x) {}
+    node_iterator_value<V>* operator->() { return std::addressof(m_ref); }
+    operator node_iterator_value<V>*() { return std::addressof(m_ref); }
+
+    node_iterator_value<V> m_ref;
+  };
+
+ public:
+  using iterator_category = std::forward_iterator_tag;
+  using value_type = node_iterator_value<V>;
+  using difference_type = std::ptrdiff_t;
+  using pointer = node_iterator_value<V>*;
+  using reference = node_iterator_value<V>;
+  using SeqIter = typename node_iterator_type<V>::seq;
+  using MapIter = typename node_iterator_type<V>::map;
+
+  node_iterator_base()
+      : m_type(iterator_type::NoneType), m_seqIt(), m_mapIt(), m_mapEnd() {}
+  explicit node_iterator_base(SeqIter seqIt)
+      : m_type(iterator_type::Sequence),
+        m_seqIt(seqIt),
+        m_mapIt(),
+        m_mapEnd() {}
+  explicit node_iterator_base(MapIter mapIt, MapIter mapEnd)
+      : m_type(iterator_type::Map),
+        m_seqIt(),
+        m_mapIt(mapIt),
+        m_mapEnd(mapEnd) {
+    m_mapIt = increment_until_defined(m_mapIt);
+  }
+
+  template <typename W>
+  node_iterator_base(const node_iterator_base<W>& rhs,
+                     typename std::enable_if<std::is_convertible<W*, V*>::value,
+                                             enabler>::type = enabler())
+      : m_type(rhs.m_type),
+        m_seqIt(rhs.m_seqIt),
+        m_mapIt(rhs.m_mapIt),
+        m_mapEnd(rhs.m_mapEnd) {}
+
+  template <typename>
+  friend class node_iterator_base;
+
+  template <typename W>
+  bool operator==(const node_iterator_base<W>& rhs) const {
+    if (m_type != rhs.m_type)
+      return false;
+
+    switch (m_type) {
+      case iterator_type::NoneType:
+        return true;
+      case iterator_type::Sequence:
+        return m_seqIt == rhs.m_seqIt;
+      case iterator_type::Map:
+        return m_mapIt == rhs.m_mapIt;
+    }
+    return true;
+  }
+
+  template <typename W>
+  bool operator!=(const node_iterator_base<W>& rhs) const {
+    return !(*this == rhs);
+  }
+
+  node_iterator_base<V>& operator++() {
+    switch (m_type) {
+      case iterator_type::NoneType:
+        break;
+      case iterator_type::Sequence:
+        ++m_seqIt;
+        break;
+      case iterator_type::Map:
+        ++m_mapIt;
+        m_mapIt = increment_until_defined(m_mapIt);
+        break;
+    }
+    return *this;
+  }
+
+  node_iterator_base<V> operator++(int) {
+    node_iterator_base<V> iterator_pre(*this);
+    ++(*this);
+    return iterator_pre;
+  }
+
+  value_type operator*() const {
+    switch (m_type) {
+      case iterator_type::NoneType:
+        return value_type();
+      case iterator_type::Sequence:
+        return value_type(**m_seqIt);
+      case iterator_type::Map:
+        return value_type(*m_mapIt->first, *m_mapIt->second);
+    }
+    return value_type();
+  }
+
+  proxy operator->() const { return proxy(**this); }
+
+  MapIter increment_until_defined(MapIter it) {
+    while (it != m_mapEnd && !is_defined(it))
+      ++it;
+    return it;
+  }
+
+  bool is_defined(MapIter it) const {
+    return it->first->is_defined() && it->second->is_defined();
+  }
+
+ private:
+  typename iterator_type::value m_type;
+
+  SeqIter m_seqIt;
+  MapIter m_mapIt, m_mapEnd;
+};
+
+using node_iterator = node_iterator_base<node>;
+using const_node_iterator = node_iterator_base<const node>;
+}
+}
+
+#endif  // VALUE_DETAIL_NODE_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/detail/node_ref.h

@@ -0,0 +1,98 @@
+#ifndef VALUE_DETAIL_NODE_REF_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_DETAIL_NODE_REF_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/node/type.h"
+#include "yaml-cpp/node/ptr.h"
+#include "yaml-cpp/node/detail/node_data.h"
+
+namespace YAML {
+namespace detail {
+class node_ref {
+ public:
+  node_ref() : m_pData(new node_data) {}
+  node_ref(const node_ref&) = delete;
+  node_ref& operator=(const node_ref&) = delete;
+
+  bool is_defined() const { return m_pData->is_defined(); }
+  const Mark& mark() const { return m_pData->mark(); }
+  NodeType::value type() const { return m_pData->type(); }
+  const std::string& scalar() const { return m_pData->scalar(); }
+  const std::string& tag() const { return m_pData->tag(); }
+  EmitterStyle::value style() const { return m_pData->style(); }
+
+  void mark_defined() { m_pData->mark_defined(); }
+  void set_data(const node_ref& rhs) { m_pData = rhs.m_pData; }
+
+  void set_mark(const Mark& mark) { m_pData->set_mark(mark); }
+  void set_type(NodeType::value type) { m_pData->set_type(type); }
+  void set_tag(const std::string& tag) { m_pData->set_tag(tag); }
+  void set_null() { m_pData->set_null(); }
+  void set_scalar(const std::string& scalar) { m_pData->set_scalar(scalar); }
+  void set_style(EmitterStyle::value style) { m_pData->set_style(style); }
+
+  // size/iterator
+  std::size_t size() const { return m_pData->size(); }
+
+  const_node_iterator begin() const {
+    return static_cast<const node_data&>(*m_pData).begin();
+  }
+  node_iterator begin() { return m_pData->begin(); }
+
+  const_node_iterator end() const {
+    return static_cast<const node_data&>(*m_pData).end();
+  }
+  node_iterator end() { return m_pData->end(); }
+
+  // sequence
+  void push_back(node& node, shared_memory_holder pMemory) {
+    m_pData->push_back(node, pMemory);
+  }
+  void insert(node& key, node& value, shared_memory_holder pMemory) {
+    m_pData->insert(key, value, pMemory);
+  }
+
+  // indexing
+  template <typename Key>
+  node* get(const Key& key, shared_memory_holder pMemory) const {
+    return static_cast<const node_data&>(*m_pData).get(key, pMemory);
+  }
+  template <typename Key>
+  node& get(const Key& key, shared_memory_holder pMemory) {
+    return m_pData->get(key, pMemory);
+  }
+  template <typename Key>
+  bool remove(const Key& key, shared_memory_holder pMemory) {
+    return m_pData->remove(key, pMemory);
+  }
+
+  node* get(node& key, shared_memory_holder pMemory) const {
+    return static_cast<const node_data&>(*m_pData).get(key, pMemory);
+  }
+  node& get(node& key, shared_memory_holder pMemory) {
+    return m_pData->get(key, pMemory);
+  }
+  bool remove(node& key, shared_memory_holder pMemory) {
+    return m_pData->remove(key, pMemory);
+  }
+
+  // map
+  template <typename Key, typename Value>
+  void force_insert(const Key& key, const Value& value,
+                    shared_memory_holder pMemory) {
+    m_pData->force_insert(key, value, pMemory);
+  }
+
+ private:
+  shared_node_data m_pData;
+};
+}
+}
+
+#endif  // VALUE_DETAIL_NODE_REF_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/emit.h

@@ -0,0 +1,32 @@
+#ifndef NODE_EMIT_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define NODE_EMIT_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <string>
+#include <iosfwd>
+
+#include "yaml-cpp/dll.h"
+
+namespace YAML {
+class Emitter;
+class Node;
+
+/**
+ * Emits the node to the given {@link Emitter}. If there is an error in writing,
+ * {@link Emitter#good} will return false.
+ */
+YAML_CPP_API Emitter& operator<<(Emitter& out, const Node& node);
+
+/** Emits the node to the given output stream. */
+YAML_CPP_API std::ostream& operator<<(std::ostream& out, const Node& node);
+
+/** Converts the node to a YAML string. */
+YAML_CPP_API std::string Dump(const Node& node);
+}  // namespace YAML
+
+#endif  // NODE_EMIT_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/impl.h

@@ -0,0 +1,385 @@
+#ifndef NODE_IMPL_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define NODE_IMPL_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/exceptions.h"
+#include "yaml-cpp/node/detail/memory.h"
+#include "yaml-cpp/node/detail/node.h"
+#include "yaml-cpp/node/iterator.h"
+#include "yaml-cpp/node/node.h"
+#include <sstream>
+#include <string>
+
+namespace YAML {
+inline Node::Node()
+    : m_isValid(true), m_invalidKey{}, m_pMemory(nullptr), m_pNode(nullptr) {}
+
+inline Node::Node(NodeType::value type)
+    : m_isValid(true),
+      m_invalidKey{},
+      m_pMemory(new detail::memory_holder),
+      m_pNode(&m_pMemory->create_node()) {
+  m_pNode->set_type(type);
+}
+
+template <typename T>
+inline Node::Node(const T& rhs)
+    : m_isValid(true),
+      m_invalidKey{},
+      m_pMemory(new detail::memory_holder),
+      m_pNode(&m_pMemory->create_node()) {
+  Assign(rhs);
+}
+
+inline Node::Node(const detail::iterator_value& rhs)
+    : m_isValid(rhs.m_isValid),
+      m_invalidKey(rhs.m_invalidKey),
+      m_pMemory(rhs.m_pMemory),
+      m_pNode(rhs.m_pNode) {}
+
+inline Node::Node(const Node&) = default;
+
+inline Node::Node(Zombie)
+    : m_isValid(false), m_invalidKey{}, m_pMemory{}, m_pNode(nullptr) {}
+
+inline Node::Node(Zombie, const std::string& key)
+    : m_isValid(false), m_invalidKey(key), m_pMemory{}, m_pNode(nullptr) {}
+
+inline Node::Node(detail::node& node, detail::shared_memory_holder pMemory)
+    : m_isValid(true), m_invalidKey{}, m_pMemory(pMemory), m_pNode(&node) {}
+
+inline Node::~Node() = default;
+
+inline void Node::EnsureNodeExists() const {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  if (!m_pNode) {
+    m_pMemory.reset(new detail::memory_holder);
+    m_pNode = &m_pMemory->create_node();
+    m_pNode->set_null();
+  }
+}
+
+inline bool Node::IsDefined() const {
+  if (!m_isValid) {
+    return false;
+  }
+  return m_pNode ? m_pNode->is_defined() : true;
+}
+
+inline Mark Node::Mark() const {
+  if (!m_isValid) {
+    throw InvalidNode(m_invalidKey);
+  }
+  return m_pNode ? m_pNode->mark() : Mark::null_mark();
+}
+
+inline NodeType::value Node::Type() const {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  return m_pNode ? m_pNode->type() : NodeType::Null;
+}
+
+// access
+
+// template helpers
+template <typename T, typename S>
+struct as_if {
+  explicit as_if(const Node& node_) : node(node_) {}
+  const Node& node;
+
+  T operator()(const S& fallback) const {
+    if (!node.m_pNode)
+      return fallback;
+
+    T t;
+    if (convert<T>::decode(node, t))
+      return t;
+    return fallback;
+  }
+};
+
+template <typename S>
+struct as_if<std::string, S> {
+  explicit as_if(const Node& node_) : node(node_) {}
+  const Node& node;
+
+  std::string operator()(const S& fallback) const {
+    if (node.Type() == NodeType::Null)
+      return "null";
+    if (node.Type() != NodeType::Scalar)
+      return fallback;
+    return node.Scalar();
+  }
+};
+
+template <typename T>
+struct as_if<T, void> {
+  explicit as_if(const Node& node_) : node(node_) {}
+  const Node& node;
+
+  T operator()() const {
+    if (!node.m_pNode)
+      throw TypedBadConversion<T>(node.Mark());
+
+    T t;
+    if (convert<T>::decode(node, t))
+      return t;
+    throw TypedBadConversion<T>(node.Mark());
+  }
+};
+
+template <>
+struct as_if<std::string, void> {
+  explicit as_if(const Node& node_) : node(node_) {}
+  const Node& node;
+
+  std::string operator()() const {
+    if (node.Type() == NodeType::Null)
+      return "null";
+    if (node.Type() != NodeType::Scalar)
+      throw TypedBadConversion<std::string>(node.Mark());
+    return node.Scalar();
+  }
+};
+
+// access functions
+template <typename T>
+inline T Node::as() const {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  return as_if<T, void>(*this)();
+}
+
+template <typename T, typename S>
+inline T Node::as(const S& fallback) const {
+  if (!m_isValid)
+    return fallback;
+  return as_if<T, S>(*this)(fallback);
+}
+
+inline const std::string& Node::Scalar() const {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  return m_pNode ? m_pNode->scalar() : detail::node_data::empty_scalar();
+}
+
+inline const std::string& Node::Tag() const {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  return m_pNode ? m_pNode->tag() : detail::node_data::empty_scalar();
+}
+
+inline void Node::SetTag(const std::string& tag) {
+  EnsureNodeExists();
+  m_pNode->set_tag(tag);
+}
+
+inline EmitterStyle::value Node::Style() const {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  return m_pNode ? m_pNode->style() : EmitterStyle::Default;
+}
+
+inline void Node::SetStyle(EmitterStyle::value style) {
+  EnsureNodeExists();
+  m_pNode->set_style(style);
+}
+
+// assignment
+inline bool Node::is(const Node& rhs) const {
+  if (!m_isValid || !rhs.m_isValid)
+    throw InvalidNode(m_invalidKey);
+  if (!m_pNode || !rhs.m_pNode)
+    return false;
+  return m_pNode->is(*rhs.m_pNode);
+}
+
+template <typename T>
+inline Node& Node::operator=(const T& rhs) {
+  Assign(rhs);
+  return *this;
+}
+
+inline Node& Node::operator=(const Node& rhs) {
+  if (is(rhs))
+    return *this;
+  AssignNode(rhs);
+  return *this;
+}
+
+inline void Node::reset(const YAML::Node& rhs) {
+  if (!m_isValid || !rhs.m_isValid)
+    throw InvalidNode(m_invalidKey);
+  m_pMemory = rhs.m_pMemory;
+  m_pNode = rhs.m_pNode;
+}
+
+template <typename T>
+inline void Node::Assign(const T& rhs) {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  AssignData(convert<T>::encode(rhs));
+}
+
+template <>
+inline void Node::Assign(const std::string& rhs) {
+  EnsureNodeExists();
+  m_pNode->set_scalar(rhs);
+}
+
+inline void Node::Assign(const char* rhs) {
+  EnsureNodeExists();
+  m_pNode->set_scalar(rhs);
+}
+
+inline void Node::Assign(char* rhs) {
+  EnsureNodeExists();
+  m_pNode->set_scalar(rhs);
+}
+
+inline void Node::AssignData(const Node& rhs) {
+  EnsureNodeExists();
+  rhs.EnsureNodeExists();
+
+  m_pNode->set_data(*rhs.m_pNode);
+  m_pMemory->merge(*rhs.m_pMemory);
+}
+
+inline void Node::AssignNode(const Node& rhs) {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  rhs.EnsureNodeExists();
+
+  if (!m_pNode) {
+    m_pNode = rhs.m_pNode;
+    m_pMemory = rhs.m_pMemory;
+    return;
+  }
+
+  m_pNode->set_ref(*rhs.m_pNode);
+  m_pMemory->merge(*rhs.m_pMemory);
+  m_pNode = rhs.m_pNode;
+}
+
+// size/iterator
+inline std::size_t Node::size() const {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  return m_pNode ? m_pNode->size() : 0;
+}
+
+inline const_iterator Node::begin() const {
+  if (!m_isValid)
+    return const_iterator();
+  return m_pNode ? const_iterator(m_pNode->begin(), m_pMemory)
+                 : const_iterator();
+}
+
+inline iterator Node::begin() {
+  if (!m_isValid)
+    return iterator();
+  return m_pNode ? iterator(m_pNode->begin(), m_pMemory) : iterator();
+}
+
+inline const_iterator Node::end() const {
+  if (!m_isValid)
+    return const_iterator();
+  return m_pNode ? const_iterator(m_pNode->end(), m_pMemory) : const_iterator();
+}
+
+inline iterator Node::end() {
+  if (!m_isValid)
+    return iterator();
+  return m_pNode ? iterator(m_pNode->end(), m_pMemory) : iterator();
+}
+
+// sequence
+template <typename T>
+inline void Node::push_back(const T& rhs) {
+  if (!m_isValid)
+    throw InvalidNode(m_invalidKey);
+  push_back(Node(rhs));
+}
+
+inline void Node::push_back(const Node& rhs) {
+  EnsureNodeExists();
+  rhs.EnsureNodeExists();
+
+  m_pNode->push_back(*rhs.m_pNode, m_pMemory);
+  m_pMemory->merge(*rhs.m_pMemory);
+}
+
+template<typename Key>
+std::string key_to_string(const Key& key) {
+  return streamable_to_string<Key, is_streamable<std::stringstream, Key>::value>().impl(key);
+}
+
+// indexing
+template <typename Key>
+inline const Node Node::operator[](const Key& key) const {
+  EnsureNodeExists();
+  detail::node* value =
+      static_cast<const detail::node&>(*m_pNode).get(key, m_pMemory);
+  if (!value) {
+    return Node(ZombieNode, key_to_string(key));
+  }
+  return Node(*value, m_pMemory);
+}
+
+template <typename Key>
+inline Node Node::operator[](const Key& key) {
+  EnsureNodeExists();
+  detail::node& value = m_pNode->get(key, m_pMemory);
+  return Node(value, m_pMemory);
+}
+
+template <typename Key>
+inline bool Node::remove(const Key& key) {
+  EnsureNodeExists();
+  return m_pNode->remove(key, m_pMemory);
+}
+
+inline const Node Node::operator[](const Node& key) const {
+  EnsureNodeExists();
+  key.EnsureNodeExists();
+  m_pMemory->merge(*key.m_pMemory);
+  detail::node* value =
+      static_cast<const detail::node&>(*m_pNode).get(*key.m_pNode, m_pMemory);
+  if (!value) {
+    return Node(ZombieNode, key_to_string(key));
+  }
+  return Node(*value, m_pMemory);
+}
+
+inline Node Node::operator[](const Node& key) {
+  EnsureNodeExists();
+  key.EnsureNodeExists();
+  m_pMemory->merge(*key.m_pMemory);
+  detail::node& value = m_pNode->get(*key.m_pNode, m_pMemory);
+  return Node(value, m_pMemory);
+}
+
+inline bool Node::remove(const Node& key) {
+  EnsureNodeExists();
+  key.EnsureNodeExists();
+  return m_pNode->remove(*key.m_pNode, m_pMemory);
+}
+
+// map
+template <typename Key, typename Value>
+inline void Node::force_insert(const Key& key, const Value& value) {
+  EnsureNodeExists();
+  m_pNode->force_insert(key, value, m_pMemory);
+}
+
+// free functions
+inline bool operator==(const Node& lhs, const Node& rhs) { return lhs.is(rhs); }
+}  // namespace YAML
+
+#endif  // NODE_IMPL_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/iterator.h

@@ -0,0 +1,34 @@
+#ifndef VALUE_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/node/node.h"
+#include "yaml-cpp/node/detail/iterator_fwd.h"
+#include "yaml-cpp/node/detail/iterator.h"
+#include <list>
+#include <utility>
+#include <vector>
+
+// Assert in place so gcc + libc++ combination properly builds
+static_assert(std::is_constructible<YAML::Node, const YAML::Node&>::value, "Node must be copy constructable");
+
+namespace YAML {
+namespace detail {
+struct iterator_value : public Node, std::pair<Node, Node> {
+  iterator_value() = default;
+  explicit iterator_value(const Node& rhs)
+      : Node(rhs),
+        std::pair<Node, Node>(Node(Node::ZombieNode), Node(Node::ZombieNode)) {}
+  explicit iterator_value(const Node& key, const Node& value)
+      : Node(Node::ZombieNode), std::pair<Node, Node>(key, value) {}
+};
+}
+}
+
+#endif  // VALUE_ITERATOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/node.h

@@ -0,0 +1,148 @@
+#ifndef NODE_NODE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define NODE_NODE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <stdexcept>
+#include <string>
+
+#include "yaml-cpp/dll.h"
+#include "yaml-cpp/emitterstyle.h"
+#include "yaml-cpp/mark.h"
+#include "yaml-cpp/node/detail/iterator_fwd.h"
+#include "yaml-cpp/node/ptr.h"
+#include "yaml-cpp/node/type.h"
+
+namespace YAML {
+namespace detail {
+class node;
+class node_data;
+struct iterator_value;
+}  // namespace detail
+}  // namespace YAML
+
+namespace YAML {
+class YAML_CPP_API Node {
+ public:
+  friend class NodeBuilder;
+  friend class NodeEvents;
+  friend struct detail::iterator_value;
+  friend class detail::node;
+  friend class detail::node_data;
+  template <typename>
+  friend class detail::iterator_base;
+  template <typename T, typename S>
+  friend struct as_if;
+
+  using iterator = YAML::iterator;
+  using const_iterator = YAML::const_iterator;
+
+  Node();
+  explicit Node(NodeType::value type);
+  template <typename T>
+  explicit Node(const T& rhs);
+  explicit Node(const detail::iterator_value& rhs);
+  Node(const Node& rhs);
+  ~Node();
+
+  YAML::Mark Mark() const;
+  NodeType::value Type() const;
+  bool IsDefined() const;
+  bool IsNull() const { return Type() == NodeType::Null; }
+  bool IsScalar() const { return Type() == NodeType::Scalar; }
+  bool IsSequence() const { return Type() == NodeType::Sequence; }
+  bool IsMap() const { return Type() == NodeType::Map; }
+
+  // bool conversions
+  explicit operator bool() const { return IsDefined(); }
+  bool operator!() const { return !IsDefined(); }
+
+  // access
+  template <typename T>
+  T as() const;
+  template <typename T, typename S>
+  T as(const S& fallback) const;
+  const std::string& Scalar() const;
+
+  const std::string& Tag() const;
+  void SetTag(const std::string& tag);
+
+  // style
+  // WARNING: This API might change in future releases.
+  EmitterStyle::value Style() const;
+  void SetStyle(EmitterStyle::value style);
+
+  // assignment
+  bool is(const Node& rhs) const;
+  template <typename T>
+  Node& operator=(const T& rhs);
+  Node& operator=(const Node& rhs);
+  void reset(const Node& rhs = Node());
+
+  // size/iterator
+  std::size_t size() const;
+
+  const_iterator begin() const;
+  iterator begin();
+
+  const_iterator end() const;
+  iterator end();
+
+  // sequence
+  template <typename T>
+  void push_back(const T& rhs);
+  void push_back(const Node& rhs);
+
+  // indexing
+  template <typename Key>
+  const Node operator[](const Key& key) const;
+  template <typename Key>
+  Node operator[](const Key& key);
+  template <typename Key>
+  bool remove(const Key& key);
+
+  const Node operator[](const Node& key) const;
+  Node operator[](const Node& key);
+  bool remove(const Node& key);
+
+  // map
+  template <typename Key, typename Value>
+  void force_insert(const Key& key, const Value& value);
+
+ private:
+  enum Zombie { ZombieNode };
+  explicit Node(Zombie);
+  explicit Node(Zombie, const std::string&);
+  explicit Node(detail::node& node, detail::shared_memory_holder pMemory);
+
+  void EnsureNodeExists() const;
+
+  template <typename T>
+  void Assign(const T& rhs);
+  void Assign(const char* rhs);
+  void Assign(char* rhs);
+
+  void AssignData(const Node& rhs);
+  void AssignNode(const Node& rhs);
+
+ private:
+  bool m_isValid;
+  // String representation of invalid key, if the node is invalid.
+  std::string m_invalidKey;
+  mutable detail::shared_memory_holder m_pMemory;
+  mutable detail::node* m_pNode;
+};
+
+YAML_CPP_API bool operator==(const Node& lhs, const Node& rhs);
+
+YAML_CPP_API Node Clone(const Node& node);
+
+template <typename T>
+struct convert;
+}
+
+#endif  // NODE_NODE_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/parse.h

@@ -0,0 +1,78 @@
+#ifndef VALUE_PARSE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_PARSE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <iosfwd>
+#include <string>
+#include <vector>
+
+#include "yaml-cpp/dll.h"
+
+namespace YAML {
+class Node;
+
+/**
+ * Loads the input string as a single YAML document.
+ *
+ * @throws {@link ParserException} if it is malformed.
+ */
+YAML_CPP_API Node Load(const std::string& input);
+
+/**
+ * Loads the input string as a single YAML document.
+ *
+ * @throws {@link ParserException} if it is malformed.
+ */
+YAML_CPP_API Node Load(const char* input);
+
+/**
+ * Loads the input stream as a single YAML document.
+ *
+ * @throws {@link ParserException} if it is malformed.
+ */
+YAML_CPP_API Node Load(std::istream& input);
+
+/**
+ * Loads the input file as a single YAML document.
+ *
+ * @throws {@link ParserException} if it is malformed.
+ * @throws {@link BadFile} if the file cannot be loaded.
+ */
+YAML_CPP_API Node LoadFile(const std::string& filename);
+
+/**
+ * Loads the input string as a list of YAML documents.
+ *
+ * @throws {@link ParserException} if it is malformed.
+ */
+YAML_CPP_API std::vector<Node> LoadAll(const std::string& input);
+
+/**
+ * Loads the input string as a list of YAML documents.
+ *
+ * @throws {@link ParserException} if it is malformed.
+ */
+YAML_CPP_API std::vector<Node> LoadAll(const char* input);
+
+/**
+ * Loads the input stream as a list of YAML documents.
+ *
+ * @throws {@link ParserException} if it is malformed.
+ */
+YAML_CPP_API std::vector<Node> LoadAll(std::istream& input);
+
+/**
+ * Loads the input file as a list of YAML documents.
+ *
+ * @throws {@link ParserException} if it is malformed.
+ * @throws {@link BadFile} if the file cannot be loaded.
+ */
+YAML_CPP_API std::vector<Node> LoadAllFromFile(const std::string& filename);
+}  // namespace YAML
+
+#endif  // VALUE_PARSE_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/ptr.h

@@ -0,0 +1,28 @@
+#ifndef VALUE_PTR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_PTR_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <memory>
+
+namespace YAML {
+namespace detail {
+class node;
+class node_ref;
+class node_data;
+class memory;
+class memory_holder;
+
+using shared_node = std::shared_ptr<node>;
+using shared_node_ref = std::shared_ptr<node_ref>;
+using shared_node_data = std::shared_ptr<node_data>;
+using shared_memory_holder = std::shared_ptr<memory_holder>;
+using shared_memory = std::shared_ptr<memory>;
+}
+}
+
+#endif  // VALUE_PTR_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/node/type.h

@@ -0,0 +1,16 @@
+#ifndef VALUE_TYPE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define VALUE_TYPE_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+namespace YAML {
+struct NodeType {
+  enum value { Undefined, Null, Scalar, Sequence, Map };
+};
+}
+
+#endif  // VALUE_TYPE_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/noexcept.h

@@ -0,0 +1,18 @@
+#ifndef NOEXCEPT_H_768872DA_476C_11EA_88B8_90B11C0C0FF8
+#define NOEXCEPT_H_768872DA_476C_11EA_88B8_90B11C0C0FF8
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+// This is here for compatibility with older versions of Visual Studio
+// which don't support noexcept.
+#if defined(_MSC_VER) && _MSC_VER < 1900
+    #define YAML_CPP_NOEXCEPT _NOEXCEPT
+#else
+    #define YAML_CPP_NOEXCEPT noexcept
+#endif
+
+#endif

class_model/include/yaml-cpp/null.h

@@ -0,0 +1,26 @@
+#ifndef NULL_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define NULL_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/dll.h"
+#include <string>
+
+namespace YAML {
+class Node;
+
+struct YAML_CPP_API _Null {};
+inline bool operator==(const _Null&, const _Null&) { return true; }
+inline bool operator!=(const _Null&, const _Null&) { return false; }
+
+YAML_CPP_API bool IsNull(const Node& node);  // old API only
+YAML_CPP_API bool IsNullString(const std::string& str);
+
+extern YAML_CPP_API _Null Null;
+}
+
+#endif  // NULL_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/ostream_wrapper.h

@@ -0,0 +1,76 @@
+#ifndef OSTREAM_WRAPPER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define OSTREAM_WRAPPER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <string>
+#include <vector>
+
+#include "yaml-cpp/dll.h"
+
+namespace YAML {
+class YAML_CPP_API ostream_wrapper {
+ public:
+  ostream_wrapper();
+  explicit ostream_wrapper(std::ostream& stream);
+  ostream_wrapper(const ostream_wrapper&) = delete;
+  ostream_wrapper(ostream_wrapper&&) = delete;
+  ostream_wrapper& operator=(const ostream_wrapper&) = delete;
+  ostream_wrapper& operator=(ostream_wrapper&&) = delete;
+  ~ostream_wrapper();
+
+  void write(const std::string& str);
+  void write(const char* str, std::size_t size);
+
+  void set_comment() { m_comment = true; }
+
+  const char* str() const {
+    if (m_pStream) {
+      return nullptr;
+    } else {
+      m_buffer[m_pos] = '\0';
+      return &m_buffer[0];
+    }
+  }
+
+  std::size_t row() const { return m_row; }
+  std::size_t col() const { return m_col; }
+  std::size_t pos() const { return m_pos; }
+  bool comment() const { return m_comment; }
+
+ private:
+  void update_pos(char ch);
+
+ private:
+  mutable std::vector<char> m_buffer;
+  std::ostream* const m_pStream;
+
+  std::size_t m_pos;
+  std::size_t m_row, m_col;
+  bool m_comment;
+};
+
+template <std::size_t N>
+inline ostream_wrapper& operator<<(ostream_wrapper& stream,
+                                   const char (&str)[N]) {
+  stream.write(str, N - 1);
+  return stream;
+}
+
+inline ostream_wrapper& operator<<(ostream_wrapper& stream,
+                                   const std::string& str) {
+  stream.write(str);
+  return stream;
+}
+
+inline ostream_wrapper& operator<<(ostream_wrapper& stream, char ch) {
+  stream.write(&ch, 1);
+  return stream;
+}
+}  // namespace YAML
+
+#endif  // OSTREAM_WRAPPER_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/parser.h

@@ -0,0 +1,90 @@
+#ifndef PARSER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define PARSER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <ios>
+#include <memory>
+
+#include "yaml-cpp/dll.h"
+
+namespace YAML {
+class EventHandler;
+class Node;
+class Scanner;
+struct Directives;
+struct Token;
+
+/**
+ * A parser turns a stream of bytes into one stream of "events" per YAML
+ * document in the input stream.
+ */
+class YAML_CPP_API Parser {
+ public:
+  /** Constructs an empty parser (with no input. */
+  Parser();
+
+  Parser(const Parser&) = delete;
+  Parser(Parser&&) = delete;
+  Parser& operator=(const Parser&) = delete;
+  Parser& operator=(Parser&&) = delete;
+
+  /**
+   * Constructs a parser from the given input stream. The input stream must
+   * live as long as the parser.
+   */
+  explicit Parser(std::istream& in);
+
+  ~Parser();
+
+  /** Evaluates to true if the parser has some valid input to be read. */
+  explicit operator bool() const;
+
+  /**
+   * Resets the parser with the given input stream. Any existing state is
+   * erased.
+   */
+  void Load(std::istream& in);
+
+  /**
+   * Handles the next document by calling events on the {@code eventHandler}.
+   *
+   * @throw a ParserException on error.
+   * @return false if there are no more documents
+   */
+  bool HandleNextDocument(EventHandler& eventHandler);
+
+  void PrintTokens(std::ostream& out);
+
+ private:
+  /**
+   * Reads any directives that are next in the queue, setting the internal
+   * {@code m_pDirectives} state.
+   */
+  void ParseDirectives();
+
+  void HandleDirective(const Token& token);
+
+  /**
+   * Handles a "YAML" directive, which should be of the form 'major.minor' (like
+   * a version number).
+   */
+  void HandleYamlDirective(const Token& token);
+
+  /**
+   * Handles a "TAG" directive, which should be of the form 'handle prefix',
+   * where 'handle' is converted to 'prefix' in the file.
+   */
+  void HandleTagDirective(const Token& token);
+
+ private:
+  std::unique_ptr<Scanner> m_pScanner;
+  std::unique_ptr<Directives> m_pDirectives;
+};
+}  // namespace YAML
+
+#endif  // PARSER_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/stlemitter.h

@@ -0,0 +1,50 @@
+#ifndef STLEMITTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define STLEMITTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <vector>
+#include <list>
+#include <set>
+#include <map>
+
+namespace YAML {
+template <typename Seq>
+inline Emitter& EmitSeq(Emitter& emitter, const Seq& seq) {
+  emitter << BeginSeq;
+  for (const auto& v : seq)
+    emitter << v;
+  emitter << EndSeq;
+  return emitter;
+}
+
+template <typename T>
+inline Emitter& operator<<(Emitter& emitter, const std::vector<T>& v) {
+  return EmitSeq(emitter, v);
+}
+
+template <typename T>
+inline Emitter& operator<<(Emitter& emitter, const std::list<T>& v) {
+  return EmitSeq(emitter, v);
+}
+
+template <typename T>
+inline Emitter& operator<<(Emitter& emitter, const std::set<T>& v) {
+  return EmitSeq(emitter, v);
+}
+
+template <typename K, typename V>
+inline Emitter& operator<<(Emitter& emitter, const std::map<K, V>& m) {
+  emitter << BeginMap;
+  for (const auto& v : m)
+    emitter << Key << v.first << Value << v.second;
+  emitter << EndMap;
+  return emitter;
+}
+}
+
+#endif  // STLEMITTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/traits.h

@@ -0,0 +1,135 @@
+#ifndef TRAITS_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define TRAITS_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <type_traits>
+#include <utility>
+#include <string>
+#include <sstream>
+
+namespace YAML {
+template <typename>
+struct is_numeric {
+  enum { value = false };
+};
+
+template <>
+struct is_numeric<char> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<unsigned char> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<int> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<unsigned int> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<long int> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<unsigned long int> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<short int> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<unsigned short int> {
+  enum { value = true };
+};
+#if defined(_MSC_VER) && (_MSC_VER < 1310)
+template <>
+struct is_numeric<__int64> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<unsigned __int64> {
+  enum { value = true };
+};
+#else
+template <>
+struct is_numeric<long long> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<unsigned long long> {
+  enum { value = true };
+};
+#endif
+template <>
+struct is_numeric<float> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<double> {
+  enum { value = true };
+};
+template <>
+struct is_numeric<long double> {
+  enum { value = true };
+};
+
+template <bool, class T = void>
+struct enable_if_c {
+  using type = T;
+};
+
+template <class T>
+struct enable_if_c<false, T> {};
+
+template <class Cond, class T = void>
+struct enable_if : public enable_if_c<Cond::value, T> {};
+
+template <bool, class T = void>
+struct disable_if_c {
+  using type = T;
+};
+
+template <class T>
+struct disable_if_c<true, T> {};
+
+template <class Cond, class T = void>
+struct disable_if : public disable_if_c<Cond::value, T> {};
+}
+
+template <typename S, typename T>
+struct is_streamable {
+  template <typename StreamT, typename ValueT>
+  static auto test(int)
+      -> decltype(std::declval<StreamT&>() << std::declval<ValueT>(), std::true_type());
+
+  template <typename, typename>
+  static auto test(...) -> std::false_type;
+
+  static const bool value = decltype(test<S, T>(0))::value;
+};
+
+template<typename Key, bool Streamable>
+struct streamable_to_string {
+  static std::string impl(const Key& key) {
+    std::stringstream ss;
+    ss << key;
+    return ss.str();
+  }
+};
+
+template<typename Key>
+struct streamable_to_string<Key, false> {
+  static std::string impl(const Key&) {
+    return "";
+  }
+};
+#endif  // TRAITS_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/include/yaml-cpp/yaml.h

@@ -0,0 +1,24 @@
+#ifndef YAML_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define YAML_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include "yaml-cpp/parser.h"
+#include "yaml-cpp/emitter.h"
+#include "yaml-cpp/emitterstyle.h"
+#include "yaml-cpp/stlemitter.h"
+#include "yaml-cpp/exceptions.h"
+
+#include "yaml-cpp/node/node.h"
+#include "yaml-cpp/node/impl.h"
+#include "yaml-cpp/node/convert.h"
+#include "yaml-cpp/node/iterator.h"
+#include "yaml-cpp/node/detail/impl.h"
+#include "yaml-cpp/node/parse.h"
+#include "yaml-cpp/node/emit.h"
+
+#endif  // YAML_H_62B23520_7C8E_11DE_8A39_0800200C9A66

class_model/launch/bio_drones.launch

@@ -0,0 +1,45 @@
+<launch>
+
+  <group>
+    <node pkg="class_model" type="bio_main" name="drone1" output="screen" ns="/drone1">
+      <param name="namespace" value="/drone1"/>
+      <param name="droneID" value="1"/>
+      <param name="use_sim_time"  value="true" />
+    </node>
+  </group>
+
+  <group>
+    <node pkg="class_model" type="bio_main2" name="drone2" output="screen" ns="/drone2">
+      <param name="namespace" value="/drone2"/>
+      <param name="droneID" value="2"/>
+      <param name="use_sim_time"  value="true" />
+    </node>
+  </group>  
+
+  <group>
+    <node pkg="class_model" type="bio_main3" name="drone3" output="screen" ns="/drone3">
+      <param name="namespace" value="/drone3"/>
+      <param name="droneID" value="3"/>
+      <param name="use_sim_time"  value="true" />
+    </node>
+  </group>  
+
+  <group>
+    <node pkg="class_model" type="bio_main4" name="drone4" output="screen" ns="/drone4">
+      <param name="namespace" value="/drone4"/>
+      <param name="droneID" value="4"/>
+      <param name="use_sim_time"  value="true" />
+    </node>
+  </group> 
+
+  <group>
+    <node pkg="class_model" type="bio_main5" name="drone5" output="screen" ns="/drone5">
+      <param name="namespace" value="/drone5"/>
+      <param name="droneID" value="5"/>
+      <param name="use_sim_time"  value="true" />
+    </node>
+  </group>  
+
+ 
+</launch>
+

class_model/launch/drone1_mqtt.launch

@@ -0,0 +1,13 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="mqttPubMain.py" name="drone1mqttPub" output="screen">
+		</node>
+	</group>
+
+	<group>
+		
+		<node pkg="class_model" type="mqttSubMain.py" name="drone1mqttSub" output="screen">
+		</node>
+	</group>
+
+</launch>

class_model/launch/drone2_mqtt.launch

@@ -0,0 +1,13 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="mqttPubMain2.py" name="drone2mqttPub" output="screen">
+		</node>
+	</group>
+
+	<group>
+		
+		<node pkg="class_model" type="mqttSubMain2.py" name="drone2mqttSub" output="screen">
+		</node>
+	</group>
+
+</launch>

class_model/launch/drone3_mqtt.launch

@@ -0,0 +1,13 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="mqttPubMain3.py" name="drone3mqttPub" output="screen">
+		</node>
+	</group>
+
+	<group>
+		
+		<node pkg="class_model" type="mqttSubMain3.py" name="drone3mqttSub" output="screen">
+		</node>
+	</group>
+
+</launch>

class_model/launch/drone4_mqtt.launch

@@ -0,0 +1,13 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="mqttPubMain4.py" name="drone4mqttPub" output="screen">
+		</node>
+	</group>
+
+	<group>
+		
+		<node pkg="class_model" type="mqttSubMain4.py" name="drone4mqttSub" output="screen">
+		</node>
+	</group>
+
+</launch>

class_model/launch/drone5_mqtt.launch

@@ -0,0 +1,13 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="mqttPubMain5.py" name="drone5mqttPub" output="screen">
+		</node>
+	</group>
+
+	<group>
+		
+		<node pkg="class_model" type="mqttSubMain5.py" name="drone5mqttSub" output="screen">
+		</node>
+	</group>
+
+</launch>

class_model/launch/drones_mqtt.launch

@@ -0,0 +1,7 @@
+<launch>
+	<include file="$(find class_model)/launch/drone1_mqtt.launch"/>
+	<include file="$(find class_model)/launch/drone2_mqtt.launch"/>
+	<include file="$(find class_model)/launch/drone3_mqtt.launch"/>
+	<include file="$(find class_model)/launch/drone4_mqtt.launch"/>
+	<include file="$(find class_model)/launch/drone5_mqtt.launch"/>
+</launch>

class_model/launch/mavlink.launch

@@ -0,0 +1,12 @@
+<launch>
+	<!-- vim: set ft=xml noet : -->
+	<!-- base node launch file-->
+
+	<arg name="fcu_url" default="/dev/ttyACM0:115200" />
+	<arg name="tgt_system" default="2"/>	
+
+	<node pkg="mavros" type="mavros_node" name="mavros" >
+		<param name="fcu_url" value="$(arg fcu_url)" />
+		<param name="target_system_id" value="$(arg tgt_system)" />
+	</node>
+</launch>

class_model/launch/mqtt.launch

@@ -0,0 +1,18 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="mqttPubMain.py" name="mqttPubMain" output="screen">
+		</node>
+	</group>
+
+	<group>
+		
+		<node pkg="class_model" type="mqttSubMain.py" name="mqttSubMain" output="screen">
+		</node>
+	</group>
+	<group>
+		
+		<node pkg="class_model" type="command.py" name="command" output="screen">
+		</node>
+	</group>
+	
+</launch>

class_model/launch/mqtt_old.launch

@@ -0,0 +1,18 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="local_mqtt_pub_data_to_ros.py" name="mqttPubMain" output="screen">
+		</node>
+	</group>
+
+	<group>
+		
+		<node pkg="class_model" type="local_mqtt_sub_data_from_ros.py" name="mqttSubMain" output="screen">
+		</node>
+	</group>
+	<group>
+		
+		<node pkg="class_model" type="command.py" name="command" output="screen">
+		</node>
+	</group>
+	
+</launch>

class_model/launch/one.launch

@@ -0,0 +1,11 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="main" name="leader" output="screen">
+			<!-- <param name="namespace" value="/drone1"/> -->
+			<param name="droneID" value="1"/>
+			<param name="use_sim_time"  value="true" />
+		</node>
+	</group>
+
+	
+</launch>

class_model/launch/testLib.launch

@@ -0,0 +1,9 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="test_lib" name="test" output="screen">
+
+		</node>
+	</group>
+
+	
+</launch>

class_model/launch/three.launch

@@ -0,0 +1,29 @@
+<launch>
+	<group>
+		<node pkg="class_model" type="main" name="leader" output="screen" ns="/drone1">
+			<param name="namespace" value="/drone1"/>
+			<param name="droneID" value="1"/>
+			<param name="use_sim_time"  value="true" />
+		</node>
+	</group>
+
+	<group>
+		
+		<node pkg="class_model" type="main" name="follower1" output="screen" ns="/drone2">
+			<param name="namespace" value="/drone2"/>
+			<param name="droneID" value="2"/>
+			<param name="use_sim_time"  value="true" />
+		</node>
+	</group>
+	
+	<group>
+		
+		<node pkg="class_model" type="main" name="follower2" output="screen" ns="/drone3">
+			<param name="namespace" value="/drone3"/>
+			<param name="droneID" value="3"/>
+			<param name="use_sim_time"  value="true" />
+		</node>
+	</group>
+
+
+</launch>

class_model/launch/three_bio.launch

@@ -0,0 +1,29 @@
+<launch>
+
+  <group>
+    <node pkg="class_model" type="bio_main" name="drone1" output="screen" ns="/drone1">
+      <param name="namespace" value="/drone1"/>
+      <param name="droneID" value="1"/>
+      <param name="use_sim_time"  value="true" />
+    </node>
+  </group>
+
+  <group>
+    <node pkg="class_model" type="bio_main2" name="drone2" output="screen" ns="/drone2">
+      <param name="namespace" value="/drone2"/>
+      <param name="droneID" value="2"/>
+      <param name="use_sim_time"  value="true" />
+    </node>
+  </group>  
+
+  <group>
+    <node pkg="class_model" type="bio_main3" name="drone3" output="screen" ns="/drone3">
+      <param name="namespace" value="/drone3"/>
+      <param name="droneID" value="3"/>
+      <param name="use_sim_time"  value="true" />
+    </node>
+  </group> 
+
+ 
+</launch>
+

class_model/src/.vscode/c_cpp_properties.json

@@ -0,0 +1,24 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/dodo/ardupilot_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/dodo/ardupilot_ws/src/ROS_controll/include/**",
+        "/home/dodo/ardupilot_ws/src/class_model/include/**",
+        "/home/dodo/ardupilot_ws/src/iq_gnc/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

class_model/src/.vscode/settings.json

@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/dodo/ardupilot_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/dodo/ardupilot_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

class_model/src/Leader.cpp

@@ -0,0 +1,140 @@
+#include "class_model/Leader.h"
+#include "class_model/convert_degree.h"
+
+LeaderClass::LeaderClass() : node_handle_(""){
+  	
+}
+
+LeaderClass::~LeaderClass() { ros::shutdown(); }
+
+void LeaderClass::leader(global_location target){
+
+    std::string command = "",pre_command = receiver_object.check_command();;
+    std_msgs::String message;
+
+    flag = 0;
+    if(pre_target.lon != target.lon || pre_target.lat != target.lat){
+        heading_status = 0;
+        ROS_INFO("heading_flag");
+
+    }
+    // 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;
+            heading_status = 0;
+            ROS_INFO("break");
+            break;
+        }
+
+        command = receiver_object.check_command();
+        while(command =="stop" || command == "land"){
+            // command_object.set_velocity(0,0,0,0,1);
+            // command = receiver_object.check_command();
+          
+            if(command == "land"){
+                mode_object.set_Mode("LAND");
+            }else if(command != "stop"){
+                break;
+            }
+            
+        }
+
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+    }
+}
+
+void LeaderClass::go2target(float target_lon,float target_lat){
+
+    current_location=GPS_object.gps_position(); 
+
+    float target_heading,error_heading;
+    float heading = GPS_object.get_heading();
+
+    float x,y;
+    x = (target_lon/100) - (current_location.lon*1e5);
+    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
+
+    while(heading_status != 1){
+        face2target(target_heading);
+    }
+
+    drone_speed.x = PID_x.update(current_location.lon*1e5 , target_lon/100 ,leader_pid);   //leader_pid defined in header file
+    drone_speed.y = PID_y.update(current_location.lat*1e5 , target_lat/100 ,leader_pid);
+
+    // float error_x = PID_x.update1(current_location.lon*1e5 , target_location.lon );
+    // float error_y = PID_y.update1(current_location.lat*1e5 , target_location.lat );
+
+    if (drone_speed.x < ignore_small && drone_speed.x > -ignore_small){                     //ignore small errors                  
+        drone_speed.x = 0;
+    }
+    if (drone_speed.y < ignore_small && drone_speed.y > -ignore_small){
+        drone_speed.y = 0;
+    }
+    if (drone_speed.x < -limit_speed){                     //max velociy                 
+        drone_speed.x = -limit_speed;
+    }
+    if (drone_speed.x > limit_speed){                                      
+        drone_speed.x = limit_speed;
+    }
+    if (drone_speed.y < -limit_speed){
+        drone_speed.y = -limit_speed;
+    }
+    if (drone_speed.y > limit_speed){
+        drone_speed.y = limit_speed;
+    }
+
+    if (drone_speed.x == 0 && drone_speed.y == 0){
+        flag = 1;
+    }
+
+    // PubData_object.publishData(drone_speed);
+    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("target:%f,%f",target_location.lon ,target_location.lat);
+    // ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5);
+    // ROS_INFO("************************************");
+    
+
+
+}
+
+void LeaderClass::face2target(float target_heading){
+
+    float error_heading;
+    float heading = GPS_object.get_heading();
+    float error_yaw = 0.25;
+    error_heading =  target_heading - heading;
+    error_yaw = check_direction(error_heading)*error_yaw*0.5; 
+
+    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("face2target...%f",target_heading);
+}
+
+int LeaderClass::isOnTarget(){
+    return OnTarget;
+}

class_model/src/PID.cpp

@@ -1,23 +1,38 @@
 #include "class_model/PID.h"
 
-// PIDClass::PIDClass() : node_handle_(""){
+PIDClass::PIDClass() : node_handle_("~"){
         
         
+    
+	// if (!node_handle_.hasParam("droneID"))
+	// {
+	// }else{
+	// 	node_handle_.getParam("droneID", droneID);
 	// }
 
-// PIDClass::~PIDClass() { ros::shutdown(); }
+    if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+
+}
+
+PIDClass::~PIDClass() { ros::shutdown(); }
 
 float PIDClass::update(float current ,float target ,float pidvals[3]){
 
     current_time = std::clock();
     // pTime = current_time -
-    double t = (current_time - pTime);
-    float error = target - current;
+    double t = (current_time - pTime)/1e6;
+    float error = (target - current);
 
     double P,D,result;
     P = pidvals[0] * error;
     I = I + (pidvals[1] * error *t);
-    D = (pidvals[2] * (error - pError))/t;
+    // D = (pidvals[2] * (error - pError))/t;
+    // I = 0;
+    D = 0;
 
     result = P + I + D;
 
@@ -30,6 +45,13 @@ float PIDClass::update(float current ,float target ,float pidvals[3]){
         }
     }
 
+
+        logFile.open("/home/dodo/ardupilot_ws/src/class_model/src/log"+ros_namespace+"PID.txt", std::ofstream::app);
+        logFile << P <<","<< I <<","<< D << std::endl;
+
+        logFile.close();
+
+
     pError = error;
     pTime = current_time; 
 

class_model/src/PID.py

@@ -1,49 +0,0 @@
-import cvzone
-import cv2
-import numpy as np
-import time
-
-
-class PID:
-    def __init__(self, pidVals, targetVal, axis=0, limit=None):
-        self.pidVals = pidVals
-        self.targetVal = targetVal
-        self.axis = axis
-        self.pError = 0
-        self.limit = limit
-        self.I = 0
-        self.pTime = 0
-
-    def update(self, cVal):
-        # Current Value - Target Value
-        t = time.time() - self.pTime
-        error = cVal - self.targetVal
-        P = self.pidVals[0] * error
-        self.I = self.I + (self.pidVals[1] * error * t)
-        D = (self.pidVals[2] * (error - self.pError)) / t
-
-        result = P + self.I + D
-
-        if self.limit is not None:
-            result = float(np.clip(result, self.limit[0], self.limit[1]))
-        self.pError = error
-        self.ptime = time.time()
-
-        return result
-
-
-
-def main():
-    # For a 640x480 image center target is 320 and 240
-    xPID = PID([1, 0.000000000001, 1], 640 // 2)
-    yPID = PID([1, 0.000000000001, 1], 480 // 2, axis=1, limit=[-100, 100])
-
-    while True:
-
-        xVal = int(xPID.update(cx))
-        yVal = int(yPID.update(cy))
-
-
-
-if __name__ == "__main__":
-    main()

class_model/src/PubData.cpp

@@ -0,0 +1,32 @@
+#include "class_model/PubData.h"
+
+PubDataClass::PubDataClass() : node_handle_("~"){
+
+    std::string ros_namespace;
+    if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+
+    PubData=node_handle_.advertise<mavros_msgs::Waypoint>(ros_namespace+"/DroneStatus",100);
+    Compass=node_handle_.subscribe(ros_namespace+"/mavros/global_position/compass_hdg", 10,
+                                   &PubDataClass::Compass_callback, this);
+
+}
+
+PubDataClass::~PubDataClass() { ros::shutdown(); }
+
+void PubDataClass::publishData(velocity v){
+
+    msg.param2 = v.x;
+    msg.param3 = v.y;
+    PubData.publish(msg);
+    ROS_INFO("pub drone status");
+
+}
+
+void PubDataClass::Compass_callback(const std_msgs::Float64::ConstPtr &degree){
+    msg.param1 = degree->data;
+    
+}

class_model/src/PubInfo.cpp

@@ -0,0 +1,22 @@
+#include "class_model/PubInfo.h"
+
+PubInfoClass::PubInfoClass():node_handle_("~"){
+
+    std::string ros_namespace;
+    if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+    PubInfo = node_handle_.advertise<diagnostic_msgs::KeyValue>(ros_namespace+"/Flight_Information_reciver",100);
+
+}
+
+PubInfoClass::~PubInfoClass() { ros::shutdown(); }
+
+void PubInfoClass::pub_info(INFO msg){
+    info.key = msg.type;           //key == drone's formation
+    info.value = msg.mission;      //value == drone's mission
+    PubInfo.publish(info);
+}
+

class_model/src/README.md

@@ -0,0 +1,4 @@
+# Compile proto
+`PROTO_DIR='proto`
+`protoc -I proto --python_out=proto proto/*.proto`
+

class_model/src/bio_main.cpp

@@ -0,0 +1,193 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+#define changeLeader
+// #define NoChangeLeader
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "drone1_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;
+    ReceiverClass receiver_object;
+    
+
+    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["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();
+    M1 = request_object.get_M1_GPS();
+    M2 = request_object.get_M2_GPS();
+    M3 = request_object.get_M3_GPS();
+    M4 = request_object.get_M4_GPS();
+
+
+    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]);
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(2);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
+
+    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); 
+    }
+
+    #ifdef changeLeader   
+
+    while(ros::ok()){
+
+        type = receiver_object.check_command();
+
+            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;
+
+            // 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) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
+
+        while(errorX > 150 || errorY > 150){
+
+            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();
+
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            if(errorX < 150 && errorY < 150){
+                break;
+            }
+
+            if(checkLeader == 1){
+
+                std::cout << "Drone1 is Leader" << std::endl;
+                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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
+        }
+        sleep(3);
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
+                routeIndex++;
+                checkLeader = 0;
+                // std::cout <<"Drone1routeIndex: "<<routeIndex<<std::endl;
+            // OnTarget = 0;
+            }
+            // else if(routeIndex == routeNum -1){
+            //     routeIndex = 0;
+            // }
+        // }
+        // type = "x";
+        // 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

@@ -0,0 +1,192 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+#define changeLeader
+// #define NoChangeLeader
+
+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;
+    ReceiverClass receiver_object;
+    
+
+    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["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();
+    M1 = request_object.get_M1_GPS();
+    M2 = request_object.get_M2_GPS();
+    M3 = request_object.get_M3_GPS();
+    M4 = request_object.get_M4_GPS();
+
+
+    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]);
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(3);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
+
+    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); 
+    }
+
+    #ifdef changeLeader   
+
+    while(ros::ok()){
+
+        type = receiver_object.check_command();
+
+            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;
+
+            // 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) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
+
+        while(errorX > 150 || errorY > 150){
+
+            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();
+
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            if(errorX < 150 && errorY < 150){
+                break;
+            }
+
+            if(checkLeader == 1){
+
+                std::cout << "Drone2 is Leader" << std::endl;
+                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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
+        }
+        sleep(3);
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
+                routeIndex++;
+                checkLeader = 0;
+                // std::cout <<"Drone2routeIndex: "<<routeIndex<<std::endl;
+            // OnTarget = 0;
+            }
+            // else if(routeIndex == routeNum -1){
+            //     routeIndex = 0;
+            // }
+        // }
+        // type = "x";
+        // 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_main3.cpp

@@ -0,0 +1,192 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+#define changeLeader
+// #define NoChangeLeader
+
+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;
+    ReceiverClass receiver_object;
+    
+
+    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["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();
+    M1 = request_object.get_M1_GPS();
+    M2 = request_object.get_M2_GPS();
+    M3 = request_object.get_M3_GPS();
+    M4 = request_object.get_M4_GPS();
+
+
+    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]);
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(4);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
+
+    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); 
+    }
+
+    #ifdef changeLeader   
+
+    while(ros::ok()){
+
+        type = receiver_object.check_command();
+
+            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;
+
+            // 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) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
+
+        while(errorX > 150 || errorY > 150){
+
+            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();
+
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            if(errorX < 150 && errorY < 150){
+                break;
+            }
+
+            if(checkLeader == 1){
+
+                std::cout << "Drone3 is Leader" << std::endl;
+                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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
+        }
+        sleep(3);
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
+                routeIndex++;
+                checkLeader = 0;
+                // std::cout <<"Drone3routeIndex: "<<routeIndex<<std::endl;
+            // OnTarget = 0;
+            }
+            // else if(routeIndex == routeNum -1){
+            //     routeIndex = 0;
+            // }
+        // }
+        // type = "x";
+        // 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_main4.cpp

@@ -0,0 +1,186 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+#define changeLeader
+// #define NoChangeLeader
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "drone4_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;
+    ReceiverClass receiver_object;
+    
+
+    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["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();
+    M1 = request_object.get_M1_GPS();
+    M2 = request_object.get_M2_GPS();
+    M3 = request_object.get_M3_GPS();
+    M4 = request_object.get_M4_GPS();
+
+
+    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]);
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(3);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
+
+    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); 
+    }
+
+    #ifdef changeLeader   
+
+    while(ros::ok()){
+
+        type = receiver_object.check_command();
+
+            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 > 150 || errorY > 150){
+
+            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();
+
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            if(errorX < 150 && errorY < 150){
+                break;
+            }
+
+            if(checkLeader == 1){
+
+                std::cout << "Drone4 is Leader" << std::endl;
+                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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
+        }
+
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
+                routeIndex++;
+                checkLeader = 0;
+                // std::cout <<"routeIndex: "<<routeIndex<<std::endl;
+            // OnTarget = 0;
+            }
+        // }
+        // type = "x";
+        // 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_main5.cpp

@@ -0,0 +1,187 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+#define changeLeader
+// #define NoChangeLeader
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "drone5_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;
+    ReceiverClass receiver_object;
+    
+
+    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["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();
+    M1 = request_object.get_M1_GPS();
+    M2 = request_object.get_M2_GPS();
+    M3 = request_object.get_M3_GPS();
+    M4 = request_object.get_M4_GPS();
+
+
+    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]);
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(4);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
+
+    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); 
+    }
+
+    #ifdef changeLeader   
+
+    while(ros::ok()){
+
+        type = receiver_object.check_command();
+
+            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 > 150 || errorY > 150){
+
+            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();
+
+            errorX = abs(pos[leaderIndex]->lon - target.lon) ;
+            errorY = abs(pos[leaderIndex]->lat - target.lat) ;
+            if(errorX < 150 && errorY < 150){
+                break;
+            }
+
+            if(checkLeader == 1){
+
+                std::cout << "Drone5 is Leader" << std::endl;
+                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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
+        }
+        sleep(3);
+        // while(type != "n"){
+        //     type = receiver_object.check_command();
+            if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
+                routeIndex++;
+                checkLeader = 0;
+                // std::cout <<"routeIndex: "<<routeIndex<<std::endl;
+            // OnTarget = 0;
+            }
+        // }
+        // type = "x";
+        // 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/command.cpp

@@ -9,8 +9,10 @@ CommandClass::CommandClass() : node_handle_("~"){
 		node_handle_.getParam("namespace", ros_namespace);
 	}
 
-    velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",100);
+    velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",10);
+    // gps_command=node_handle_.advertise<geographic_msgs::GeoPoseStamped>(ros_namespace+"/mavros/setpoint_position/global",10);
     gps_command=node_handle_.advertise<mavros_msgs::GlobalPositionTarget>(ros_namespace+"/mavros/setpoint_raw/global",10);
+
 }
 
 CommandClass::~CommandClass() { ros::shutdown(); }
@@ -26,13 +28,21 @@ void CommandClass::velocity_init(){
 
     velocity_command.publish(msg);
 
-    uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+	double begin = ros::Time::now().toSec();
 	while(true){
 		velocity_command.publish(msg);
-		uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
-		if((now_ms-last_ms)>10){
+		double now = ros::Time::now().toSec();
+		if((now-begin)>0.01){
 			break;
 	}
+
+    // uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+	// while(true){
+	// 	velocity_command.publish(msg);
+	// 	uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+	// 	if((now_ms-last_ms)>10){
+	// 		break;
+	// }
 	}
 
 }
@@ -49,19 +59,32 @@ void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second
 	pre_location=gps_object.gps_position();
 	ROS_INFO("%f,%f",pre_location.lat,pre_location.lon);
 
-    uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    //uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
 	int flag = 0;
+	
+	double begin = ros::Time::now().toSec();
 	while(true){
 		velocity_command.publish(msg);
-
-	uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
-	if((now_ms-last_ms)>second*1000){
-		// ROS_INFO("%ld",now_ms-last_ms);
+		double now = ros::Time::now().toSec();
+		if((now-begin)>second){
+			//ROS_INFO("%ld",now-begin);
 			velocity_init();
 			flag=1;
 			ROS_INFO("fin");
 			break;
 	}
+	
+	// while(true){
+	// velocity_command.publish(msg);
+
+	// uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+	// if((now_ms-last_ms)>second*1000){
+	// 	// ROS_INFO("%ld",now_ms-last_ms);
+	// 	velocity_init();
+	// 	flag=1;
+	// 	ROS_INFO("fin");
+	// 	break;
+	// }
 	}
 
 	while(flag==1){
@@ -75,15 +98,41 @@ void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second
 
 }
 
-void CommandClass::set_global_position(float lat,float lon,float alt){
+void CommandClass::set_global_position(double lon,double lat,float alt,float yaw){
     
     mavros_msgs::GlobalPositionTarget goal_position;
-	goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_TERRAIN_ALT;
-	goal_position.type_mask = goal_position.IGNORE_VX | goal_position.IGNORE_VY | goal_position.IGNORE_VZ | goal_position.IGNORE_AFX | goal_position.IGNORE_AFY | goal_position.IGNORE_AFZ | goal_position.IGNORE_YAW | goal_position.IGNORE_YAW_RATE;
+	goal_position.coordinate_frame = mavros_msgs::GlobalPositionTarget::FRAME_GLOBAL_REL_ALT;
+	goal_position.type_mask =  mavros_msgs::GlobalPositionTarget::IGNORE_VX |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_VY |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_VZ |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_AFX |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_AFY |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_AFZ |
+                              mavros_msgs::GlobalPositionTarget::FORCE |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_YAW_RATE ;
     goal_position.latitude = lat;
     goal_position.longitude = lon;
     goal_position.altitude = alt;
+	goal_position.yaw = yaw;
+	// goal_position.header.frame_id = "map";
+	// goal_position.type_mask = 65535;
+
+
+	// geographic_msgs::GeoPoseStamped goal_position;
+    // goal_position.header.frame_id = "map";
+    // goal_position.pose.position.latitude = lat;
+    // goal_position.pose.position.longitude = lon;
+    // goal_position.pose.position.altitude = 67;
+
+	ROS_INFO("%f,%f",lat,lon);
+	double begin = ros::Time::now().toSec();
+	while(true){
 		gps_command.publish(goal_position);
+		double now = ros::Time::now().toSec();
+		if((now-begin)>0.5){
+			break;
+	}
+	}
 
 }
 
@@ -104,8 +153,10 @@ int CommandClass::fix_position(global_location pre_location,float x,float y,floa
 		return 1;
 	}
 }
-void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second){
 
+#ifdef DEBUG
+void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second,uint64_t get_t,uint64_t cal_t){
+	uint64_t fun_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
     msg.twist.linear.x = x;
     msg.twist.angular.x = x;
     msg.twist.linear.y = y;
@@ -113,21 +164,90 @@ void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second
     msg.twist.linear.z = alt;
     msg.twist.angular.z = yaw;
     // ROS_INFO("set_velocity: x:%f, y:%f, alt:%f, yaw:%f",x,y,alt,yaw);
-	global_location pre_location=gps_object.gps_position();
+	// global_location pre_location=gps_object.gps_position();
+	int flag = 1;
+	double begin = ros::Time::now().toSec();
+	while(true){
+		velocity_command.publish(msg);
+		uint64_t pub_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+		int ID = param_object.getID();
+		std::string path;
+		if(flag == 1){
+			switch (ID)
+			{
+			case 1:
+				path = "/home/dodo/Desktop/TimerTest/drone1.csv";
+				break;
+			
+			case 2:
+				path = "/home/dodo/Desktop/TimerTest/drone2.csv";
+				break;
+	
+			case 3:
+				path = "/home/dodo/Desktop/TimerTest/drone3.csv";
+				break;	
+			}
+			std::fstream file(path,std::ios::app); 
+  			file << get_t << " " << cal_t << " " << fun_t <<" "<<pub_t<< "\n";
+  			file.close();
+			flag = 0;
+		}
+		double now = ros::Time::now().toSec();
+		if((now-begin)>second){
+			velocity_init();
+			break;
+	}
 
-    uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
 
+    // uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+	// while(true){
+	// velocity_command.publish(msg);
+	// uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+	// if((now_ms-last_ms)>second*1000){
+	// 	// ROS_INFO("%ld",now_ms-last_ms);
+	// 	//velocity_init();
+	// 	break;
+	// }
+	}
+}
+#endif
+
+#ifndef DEBUG
+void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second){
+
+    msg.twist.linear.x = x;
+    msg.twist.angular.x = x;
+    msg.twist.linear.y = y;
+    msg.twist.angular.y = y;
+    msg.twist.linear.z = alt;
+    msg.twist.angular.z = yaw;
+    // ROS_INFO("set_velocity: x:%f, y:%f, alt:%f, yaw:%f",x,y,alt,yaw);
+	// global_location pre_location=gps_object.gps_position();
+	
+	double begin = ros::Time::now().toSec();
 	while(true){
 		velocity_command.publish(msg);
-
-	uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
-	if((now_ms-last_ms)>second*1000){
-		// ROS_INFO("%ld",now_ms-last_ms);
-		//velocity_init();
+		double now = ros::Time::now().toSec();
+		if((now-begin)>second){
+			velocity_init();
 			break;
 	}
+
+
+    // uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+	// while(true){
+	// velocity_command.publish(msg);
+	// uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+	// if((now_ms-last_ms)>second*1000){
+	// 	// ROS_INFO("%ld",now_ms-last_ms);
+	// 	//velocity_init();
+	// 	break;
+	// }
 	}
 }
+#endif
 
 void CommandClass::set_target_position(float x ,float y ){
 

class_model/src/command.py

@@ -1,77 +0,0 @@
-#! /usr/bin/env python3
-#coding:utf-8
-
-import ssl
-import rospy
-from std_msgs.msg import String
-import paho.mqtt.client as mqtt
-import json
-
-
- # Ros
-def ros_pub(dataJson):
-    global publisher, rate ,publisher1 ,publisher2,publisher3,publisher4,publisher5
-    # data = json.loads(dataJson)
-    publisher.publish(dataJson)     #將date字串發布到topic
-    publisher1.publish(dataJson)
-    publisher2.publish(dataJson)
-    publisher3.publish(dataJson)
-    publisher4.publish(dataJson)
-    publisher5.publish(dataJson)
-
-    rate.sleep()
-    # print(f"publish data {data}")
-
-# MQTT
-def on_connect(self, userdata, flags, rc):
-    print("Connected with result code " + str(rc))
-    client.subscribe("cmd/broadcast")
-
-
-def on_message(self, userdata, msg):
-    #msg = msg.payload.decode('utf-8')
-    print(f"msg.topic {msg}")
-    ros_pub(msg.payload.decode('utf-8'))
-
-
-def initialise_clients(clientName):
-    # callback assignment
-    initialise_client = mqtt.Client(clientName, False)
-    initialise_client.topic_ack = []
-    return initialise_client
-
-
-if __name__ == '__main__':
-    # Mqtt
-    mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "cmd/broadcast"}
-    client = initialise_clients("receiver")
-    client.on_connect = on_connect
-    client.on_message = on_message
-    client.connect(mqtt_config["host"], mqtt_config["port"], 60)
-
-    # Ros
-    Mqtt_Node = 'publisher_py'
-    rospy.init_node("cmd_receiver")
-    # initialize Ros node
-    topicName = 'cmd_receiver'
-    publisher = rospy.Publisher(topicName,String,queue_size=10)
-
-    topicName1 = '/drone1/cmd_receiver'
-    publisher1 = rospy.Publisher(topicName1,String,queue_size=10)
-
-    topicName2 = '/drone2/cmd_receiver'
-    publisher2 = rospy.Publisher(topicName2,String,queue_size=10)
-
-    topicName3 = '/drone3/cmd_receiver'
-    publisher3 = rospy.Publisher(topicName3,String,queue_size=10)
-
-    topicName4 = '/drone4/cmd_receiver'
-    publisher4 = rospy.Publisher(topicName4,String,queue_size=10)
-
-    topicName5 = '/drone5/cmd_receiver'
-    publisher5 = rospy.Publisher(topicName5,String,queue_size=10)
-
-
-    rate = rospy.Rate(10)
-
-    client.loop_forever()

class_model/src/config.yaml

@@ -0,0 +1,25 @@
+DroneParam:
+  ID: 1 
+route1:
+  x: 120.6743161             #(24.1218859, 120.6743161)
+  y: 24.1221859
+  z: 0.0
+route2:
+  x: 120.6745161             #(24.1218859, 120.6743161)
+  y: 24.1218859
+  z: 0.0
+route3:
+  x: 120.6743161             #(24.1218859, 120.6743161)
+  y: 24.1216859
+  z: 0.0
+route4:
+  x: 120.6740161             #(24.1218859, 120.6743161)
+  y: 24.1217359
+  z: 0.0
+formation:
+  distance: 4.0
+  angle: 45
+routeNum:
+  num: 4
+
+  

class_model/src/config2.yaml

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

class_model/src/config3.yaml

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

class_model/src/drone2.cpp

@@ -1,19 +0,0 @@
-#include "class_model/follower.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();   
-
-    FollowerClass follower_object;
-    follower_object.follower();
-    // RequestClass test_object;
-
-    ros::waitForShutdown();
-
-    return 0;
-}

class_model/src/follower.cpp

@@ -2,36 +2,373 @@
 
 FollowerClass::FollowerClass() : node_handle_(""){
     
-
+    std::string ros_namespace;
+	if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
 }
 
 FollowerClass::~FollowerClass() { ros::shutdown(); }
 
-void FollowerClass::follower(){
+void FollowerClass::follower(global_location** member_data, size_t index, int refID){
+
+    if(refID == member_data[0]->ID){
+        ref = &RequestClass::get_M1_GPS;
+    }else if (refID == member_data[1]->ID)
+    {
+        ref = &RequestClass::get_M2_GPS;
+    }else if (refID == member_data[2]->ID)
+    {
+        ref = &RequestClass::get_M3_GPS;
+    }else if (refID == member_data[3]->ID)
+    {
+        ref = &RequestClass::get_M4_GPS;
+    }
+
+    std::string command = "";
+    std::string pre_command = receiver_object.check_command();
+    
+    // while(true){
+
+    calculate_position(distance,angle,self.plane);
+    // command = receiver_object.check_command();
+
+    // if(command != pre_command ){
+    //     ROS_INFO("change formation");
+    //     break;
+    // }
+    
+    //}
+}
+
+void FollowerClass::plane(global_location* target, global_location* leader, global_location* follower){
+    
+    //calculate drone is locate on R/L plane
+
+    float phi = leader->heading/100 ; 
+    // phi = phi*3.14/180; 
 
-    mode_object.set_Mode("GUIDED");
-    control_object.arm();
-    control_object.takeoff(1.5);
-    sleep(5);
-    while(true){
-        calculate_position(4,30);
+    if (phi > 355 && phi < 5){
+        phi = 0;
     }  
 
-    sleep(2);
+    // float ref_y = leader->lat + 5000;
+    // float ref_x = leader->lon + 1;
+    // ref_point.lon = (cos(-phi)*leader->lon + sin(-phi)*ref_y);
+    // ref_point.lat = (-sin(-phi)*leader->lon + cos(-phi)*ref_y);
 
-    mode_object.set_Mode("LAND");
+    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
+
+    y = slope * follower->lon + c;
+
+    // std::cout<< follower.ID << ","<< follower.lat <<","<< y <<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;              
+        switch (follower->ID){
+            case 1:
+                drone1.init_location.lat = follower->lat;
+                drone1.init_location.lon = follower->lon;
+                drone1.ID = follower->ID; 
+                drone1.plane = 1;        //right plane
+                break;
+            case 2:
+                drone2.init_location.lat = follower->lat;
+                drone2.init_location.lon = follower->lon;
+                drone2.ID = follower->ID; 
+                drone2.plane = 1;        
+                break;            
+            case 3:
+                drone3.init_location.lat = follower->lat;
+                drone3.init_location.lon = follower->lon;
+                drone3.ID = follower->ID; 
+                drone3.plane = 1;        
+                break;
+            case 4:
+                drone4.init_location.lat = follower->lat;
+                drone4.init_location.lon = follower->lon;
+                drone4.ID = follower->ID; 
+                drone4.plane = 1;        
+                break; 
+            case 5:
+                drone5.init_location.lat = follower->lat;
+                drone5.init_location.lon = follower->lon;
+                drone5.ID = follower->ID; 
+                drone5.plane = 1;        
+                break;  
+        }
+    }else if((follower->lat > y && phi < 180) || (follower->lat <= y && phi >= 180)){
+        // std::cout <<"drone :"<<follower->ID <<", plane: -1" <<std::endl;    
+        switch (follower->ID){
+            case 1:
+                drone1.init_location.lat = follower->lat;
+                drone1.init_location.lon = follower->lon;
+                drone1.ID = follower->ID; 
+                drone1.plane = -1;        //left plane
+                break;
+            case 2:
+                drone2.init_location.lat = follower->lat;
+                drone2.init_location.lon = follower->lon;
+                drone2.ID = follower->ID; 
+                drone2.plane = -1;        
+                break;            
+            case 3:
+                drone3.init_location.lat = follower->lat;
+                drone3.init_location.lon = follower->lon;
+                drone3.ID = follower->ID; 
+                drone3.plane = -1;       
+                break; 
+            case 4:
+                drone4.init_location.lat = follower->lat;
+                drone4.init_location.lon = follower->lon;
+                drone4.ID = follower->ID; 
+                drone4.plane = -1;        
+                break; 
+            case 5:
+                drone5.init_location.lat = follower->lat;
+                drone5.init_location.lon = follower->lon;
+                drone5.ID = follower->ID; 
+                drone5.plane = -1;        
+                break;
+    }
     }
     
-void FollowerClass::calculate_position(float k,float theta){
     
-    theta = theta*3.14/180;
-    float phi = request_object.get_leader_heading()/100;
+}
+
+int FollowerClass::direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member){
+    
+    //Calculate whether the vector (SM) of self and member N is the same as the flight direction (LT)
+
+    float phi = leader->heading/100 ; 
     phi = phi*3.14/180;   
-    leader_location=request_object.get_leader_GPS();
+
+
+    // 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_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_LR dot vector_SM
+    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_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))){
+
+        u = (vector_LT.lon*pre_LT.lon)+(vector_LT.lat*pre_LT.lat);
+        LTcos = u / (m*pre_m);
+        if (LTcos < 0){
+            m = pre_m;
+            k = pre_k;
+            // std::cout <<"memberID: "<<member.ID << " fix" << std::endl;
+        }
+        // std::cout <<"dir_test "<< std::endl;
+
+    }else{
+        pre_LT = vector_LT;
+        pre_tar.lat = target->lat;
+        pre_tar.lon = target->lon;
+
+    }
+    /***************************************/
+    #endif
+    
+    cosTheta = k / (m*n);
+    // std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl;
+
+    // std::cout <<"drone "<< self.ID <<"cosTheta: "<< cosTheta << std::endl;
+    if(cosTheta >= 0){
+        return 1;
+    }else{
+        return 10000;
+    }
+
+}
+
+int FollowerClass::find_ref_drone(global_location** data, size_t index, int leaderID){
+
+    for(int i=1 ; i<index ; i++){                //check leader drone
+        if(leaderID == data[i]->ID){
+            leader_drone = *data[i];
+            switch(leaderID){
+                case 1:
+                    drone1.init_location.lat = data[i]->lat;        
+                    drone1.init_location.lon = data[i]->lon;
+                    drone1.ID = data[i]->ID;
+                    drone1.plane = 0;               //center
+                    break;
+                case 2:
+                    drone2.init_location.lat = data[i]->lat;
+                    drone2.init_location.lon = data[i]->lon;
+                    drone2.ID = data[i]->ID;
+                    drone2.plane = 0;
+                    break;
+                case 3:
+                    drone3.init_location.lat = data[i]->lat;
+                    drone3.init_location.lon = data[i]->lon;
+                    drone3.ID = data[i]->ID;
+                    drone3.plane = 0;
+                    break;
+                case 4:
+                    drone4.init_location.lat = data[i]->lat;
+                    drone4.init_location.lon = data[i]->lon;
+                    drone4.ID = data[i]->ID;
+                    drone4.plane = 0;
+                    break;
+                case 5:
+                    drone5.init_location.lat = data[i]->lat;
+                    drone5.init_location.lon = data[i]->lon;
+                    drone5.ID = data[i]->ID;
+                    drone5.plane = 0;
+                    break;
+
+        }
+        // std::cout << data[i].ID << std::endl;
+        }
+    }
+
+    for(int i=1 ; i<index ; i++){
+        if(data[i]->ID != leader_drone.ID){          //caculate the plane of each drone
+            plane(data[0],&leader_drone,data[i]);
+        }
+    }
+
+    int j=0,s_index=0;
+    if(drone1.ID == data[1]->ID){                   //seperate self and other
+        self = drone1;                              //problem
+    }else{
+        memberDrone[j] = drone1;
+        j+=1;
+        // std::cout << "drone1" <<std::endl;
+    }
+    if(drone2.ID == data[1]->ID){
+        self = drone2;
+    }else{
+        memberDrone[j] = drone2;
+        j+=1;
+        // std::cout << "drone2" <<std::endl;
+    }
+    if(drone3.ID == data[1]->ID){
+        self = drone3;
+    }else{
+        memberDrone[j] = drone3;
+        j+=1;
+        // std::cout << "drone3" <<std::endl;
+    }
+    if(drone4.ID == data[1]->ID){
+        self = drone4;
+    }else{
+        memberDrone[j] = drone4;
+        j+=1;
+        // std::cout << "drone4" <<std::endl;
+    }
+    if(drone5.ID == data[1]->ID){
+        self = drone5;
+    }else{
+        memberDrone[j] = drone5;
+        j+=1;
+        // std::cout << "drone4" <<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 <<"drone"<<self.ID<<", memberdrone"<< memberDrone[i].ID<<", plane: "<< memberDrone[i].plane <<std::endl;
+            s_index++;
+        }
+
+    }
+
+
+    // 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;
+    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 <<"drone" <<samePlane[i].ID<<" ,d = "<<d[i] <<std::endl;
+    }
+
+    std::vector<float> _d(d,d+s_index); 
+    
+    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;
+    return ref_ID;   
+
+}
+
+
+
+void FollowerClass::calculate_position(float k,float theta,int plane){
+
+    refpos = (request_object.*ref)();    //get reference drone data(GPS+heading)
+    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
 
-    double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};
+    // if(deg_phi >= 180 && deg_phi < 350){
+    //     plane = plane*-1;
+    // }else if((deg_phi >= 350 || deg_phi < 5) && pre_plane != 0){
+    //     plane = pre_plane;
+    // }
+    theta = plane*theta*3.14/180;
+    pre_plane = plane;
+    double Pf[]={},Pl[]={refpos.lon ,refpos.lat};       //transfer maxtrix  
     float transfer[2][2]={
                             cos(phi),-sin(phi),
                             sin(phi),cos(phi)
@@ -42,23 +379,99 @@ void FollowerClass::calculate_position(float k,float theta){
     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;
     
-    float error_x = Pf[0] - (follower_location.lon*1e+5);
-    float error_y = Pf[1] - (follower_location.lat*1e+5);
+    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);
     
-    if (error_x < 0.3 & error_x > -0.3){
-        error_x = 0;
+
+    // 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_lat = Pf[1] - (follower_location.lat*1e+5);
+
+    
+    float error_degree = deg_phi - heading;
+    float error_yaw = 0.4;   //CCW
+
+    if (error_degree >= 360){
+        error_degree -= 360;
+    }
+    if (error_degree <= -360){
+        error_degree += 360;
+    }
+
+    error_yaw = check_direction(error_degree)*error_yaw;              //check yaw direction
+
+    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 < -limit_speed){                                      
+        error_lon = -limit_speed;
     }
-    if (error_y < 0.3 & error_y > -0.3){
-        error_y = 0;
+    if (error_lon > limit_speed){                                      
+        error_lon = limit_speed;
     }
+    if (error_lat < -limit_speed){
+        error_lat = -limit_speed;
+    }    
+    if (error_lat > limit_speed){
+        error_lat = limit_speed;
+    }
+
+    // ROS_INFO("%d,%f,%f",refpos.ID,refpos.lon ,refpos.lat );
+    // 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("************************************");
+    
+    // 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_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
+}
+
+void FollowerClass::face2target(global_location* target, global_location* leader){
 
-    // 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("************************************");
+    float x,y,target_heading;
+    float heading = GPS_object.get_heading();
+    heading_status = 0;
+    error_yaw = 0.25;
+    x = (target->lon/100) - (leader->lon/100);
+    y = (target->lat/100) - (leader->lat/100);
+    target_heading = ConvertDeg(x,y);
 
-    command_object.fix_velocity(error_x,error_y,0,0,0.1);
-    // sleep(0.5);
+    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/formation.cpp

@@ -20,54 +20,54 @@ FormationClass::~FormationClass() { ros::shutdown(); }
 
 void FormationClass::leader(){
 
-    int counter=0;
-    std::string command = "",pre_command = "";
-    
-    while(true){
-    if(flag==0){
-    command_object.set_velocity(0,0,0,0.1,1);
-    sleep(5);
-    while(counter<=5){
-        command_object.set_velocity(-1,1,0,0,1);
-        command = receiver_object.check_command();
-        
-        while(command =="stop" || command == "land"){
-            command_object.set_velocity(0,0,0,0,1);
-            command = receiver_object.check_command();
-            
-            if(command == "land"){
-                mode_object.set_Mode("LAND");
-            }else if(command != "stop"){
-                break;
-            }
-            
-        }
-
-    counter++;
-    std::cout << command <<std::endl;
-    }
-    if(counter>=5){
-        flag = 1;
-    }
-    command = receiver_object.check_command();
-    // command_object.set_velocity(-1,1,0,0,10);
-    // // sleep(5);    
-    // command_object.set_velocity(0,0,0,0,10);
-    // command_object.set_velocity(1,-1,0,0,10);
-    // command_object.set_velocity(1,-1,0,0,10);
-    // sleep(2);
-    }
-    if(command == "land"){
-        mode_object.set_Mode("LAND");
-        ROS_INFO("xxxx");
-    }
+    // int counter=0;
+    // std::string command = "",pre_command = "";
+    
+    // while(true){
+    // if(flag==0){
+    // command_object.set_velocity(0,0,0,0.1,1);
+    // sleep(5);
+    // while(counter<=5){
+    //     command_object.set_velocity(-1,1,0,0,1);
+    //     command = receiver_object.check_command();
+        
+    //     while(command =="stop" || command == "land"){
+    //         command_object.set_velocity(0,0,0,0,1);
+    //         command = receiver_object.check_command();
+            
+    //         if(command == "land"){
+    //             mode_object.set_Mode("LAND");
+    //         }else if(command != "stop"){
+    //             break;
+    //         }
+            
+    //     }
+
+    // counter++;
+    // std::cout << command <<std::endl;
+    // }
+    // if(counter>=5){
+    //     flag = 1;
+    // }
+    // command = receiver_object.check_command();
+    // // command_object.set_velocity(-1,1,0,0,10);
+    // // // sleep(5);    
+    // // command_object.set_velocity(0,0,0,0,10);
+    // // command_object.set_velocity(1,-1,0,0,10);
+    // // command_object.set_velocity(1,-1,0,0,10);
+    // // sleep(2);
+    // }
+    // if(command == "land"){
+    //     mode_object.set_Mode("LAND");
+    //     ROS_INFO("xxxx");
+    // }
     
-}
+// }
 }
 
 void FormationClass::leader1(float x,float y){
 
-    std::string command = "",pre_command = "";
+    std::string command = "",pre_command = receiver_object.check_command();;
     std_msgs::String message;
 
     leader_location=request_object.get_leader_GPS();
@@ -88,8 +88,8 @@ void FormationClass::leader1(float x,float y){
 
         command = receiver_object.check_command();
         while(command =="stop" || command == "land"){
-            command_object.set_velocity(0,0,0,0,1);
-            command = receiver_object.check_command();
+            // command_object.set_velocity(0,0,0,0,1);
+            // command = receiver_object.check_command();
           
             if(command == "land"){
                 mode_object.set_Mode("LAND");
@@ -109,12 +109,13 @@ void FormationClass::leader1(float x,float y){
 
 void FormationClass::follower1(int type){
 
-    std::string command,pre_command = "";
+    std::string command = "";
+    std::string pre_command = receiver_object.check_command();
     int message;
     while(true){
 
         if(type == 0){
-            calculate_position(4,30);
+            calculate_position(4,45);
         }else if(type == 1){
             calculate_position(4,0);
         }else if(type == 2){
@@ -125,17 +126,20 @@ void FormationClass::follower1(int type){
             calculate_position(2.5,120);
         }else if(type == 5){
             calculate_position(4,60,60);
+        }else if(type == 6){
+            calculate_position(4,45);
         }
 
         command = receiver_object.check_command();
         // message = request_object.get_formation_message();
         // ROS_INFO("%d",message);
+        // std::cout<<command<<std::endl;
+        // std::cout<<"/"<<pre_command<<std::endl;
 
         if(command != pre_command ){
-            // ROS_INFO("change formation");
+            ROS_INFO("change formation");
             break;
         }
-        pre_command = command;
         
 
     }
@@ -148,11 +152,12 @@ void FormationClass::follower1(int type){
 
 void FormationClass::follower2(int type){
 
-    std::string message,command,pre_command = "";
+    std::string message,command;
+    std::string pre_command = receiver_object.check_command();
     while(true){
         
         if(type == 0){
-            calculate_position(4,-30);
+            calculate_position(4,-45);
         }else if(type == 1){
             calculate_position(8,0);
         }else if(type == 2){
@@ -163,17 +168,20 @@ void FormationClass::follower2(int type){
             calculate_position(2.5,-120);
         }else if(type == 5){
             calculate_position(4,-60,-60);
+        }else if(type == 6){
+            calculate_position(4,-45);
         }
 
         command = receiver_object.check_command();
         // message = request_object.get_formation_message();
+        // ROS_INFO("%d",message);
+        // std::cout<<command<<std::endl;
+        // std::cout<<"/"<<pre_command<<std::endl;
 
         if(command != pre_command ){
-            // ROS_INFO("change formation");
+            ROS_INFO("change formation");
             break;
         }
-        pre_command = command;
-        
 
     }
 
@@ -181,11 +189,12 @@ void FormationClass::follower2(int type){
 
 void FormationClass::follower3(int type){
 
-    std::string message,command,pre_command = "";
+    std::string command = "";
+    std::string pre_command = receiver_object.check_command();
     while(true){
         
         if(type == 0){
-            calculate_position(8,30);
+            calculate_position(8,45);
         }else if(type == 1){
             calculate_position(12,0);
         }else if(type == 2){
@@ -196,17 +205,21 @@ void FormationClass::follower3(int type){
             calculate_position(2.5,60);
         }else if(type == 5){
             calculate_position(6.93,30,120);
+        }else if(type == 6){
+            calculate_position(8.4,21);
         }
         
 
         command = receiver_object.check_command();
         // message = request_object.get_formation_message();
+        // ROS_INFO("%d",message);
+        // std::cout<<command<<std::endl;
+        // std::cout<<"/"<<pre_command<<std::endl;
 
         if(command != pre_command ){
-            // ROS_INFO("change formation");
+            ROS_INFO("change formation");
             break;
         }
-        pre_command = command;
         
 
     }
@@ -215,11 +228,12 @@ void FormationClass::follower3(int type){
 
 void FormationClass::follower4(int type){
 
-    std::string message,command,pre_command = "";
+    std::string command = "";
+    std::string pre_command = receiver_object.check_command();
     while(true){
         
         if(type == 0){
-            calculate_position(8,-30);
+            calculate_position(8,-45);
         }else if(type == 1){
             calculate_position(16,0);
         }else if(type == 2){
@@ -230,16 +244,20 @@ void FormationClass::follower4(int type){
             calculate_position(2.5,-60);
         }else if(type == 5){
             calculate_position(6.93,-30,-120);
+        }else if(type == 6){
+            calculate_position(5.657,0);
         }
 
         command = receiver_object.check_command();
         // message = request_object.get_formation_message();
+        // ROS_INFO("%d",message);
+        // std::cout<<command<<std::endl;
+        // std::cout<<"/"<<pre_command<<std::endl;
 
         if(command != pre_command ){
-            // ROS_INFO("change formation");
+            ROS_INFO("change formation");
             break;
         }
-        pre_command = command;
 
     }
 
@@ -247,31 +265,74 @@ void FormationClass::follower4(int type){
 
 void FormationClass::follower5(int type){
 
-    std::string message,command,pre_command = "";
+    std::string command = "";
+    std::string pre_command = receiver_object.check_command();
     while(true){
         
         if(type == 0){
-            calculate_position(12,-30);
+            calculate_position(12,-45);
         }else if(type == 1){
             calculate_position(20,0);
         }else if(type == 2){
-            calculate_position(6,-90);
-        }else if(type == 3){
-            calculate_position(6,180);
-        }else if(type == 4){
-            calculate_position(3,0);
-        }else if(type == 5){
-            calculate_position(8,0,180);
+            calculate_position(12,90);
+        }else if(type == 6){
+            calculate_position(8.4,-21);
+        }
+        // else if(type == 3){
+        //     calculate_position(6,180);
+        // }else if(type == 4){
+        //     calculate_position(3,0);
+        // }else if(type == 5){
+        //     calculate_position(8,0,180);
+        // }
+
+        command = receiver_object.check_command();
+        // message = request_object.get_formation_message();
+        // ROS_INFO("%d",message);
+        // std::cout<<command<<std::endl;
+        // std::cout<<"/"<<pre_command<<std::endl;
+
+        if(command != pre_command ){
+            ROS_INFO("change formation");
+            break;
+        }
+        
+
+    }
+
 }
 
+void FormationClass::follower6(int type){
+
+    std::string command = "";
+    std::string pre_command = receiver_object.check_command();
+    while(true){
+        
+        if(type == 0){
+            calculate_position(12,-30);
+        }else if(type == 1){
+            calculate_position(24,0);
+        }else if(type == 2){
+            calculate_position(12,-90);
+        }
+        // else if(type == 3){
+        //     calculate_position(6,180);
+        // }else if(type == 4){
+        //     calculate_position(3,0);
+        // }else if(type == 5){
+        //     calculate_position(8,0,180);
+        // }
+
         command = receiver_object.check_command();
         // message = request_object.get_formation_message();
+        // ROS_INFO("%d",message);
+        // std::cout<<command<<std::endl;
+        // std::cout<<"/"<<pre_command<<std::endl;
 
         if(command != pre_command ){
-            // ROS_INFO("change formation");
+            ROS_INFO("change formation");
             break;
         }
-        pre_command = command;
         
 
     }
@@ -280,7 +341,8 @@ void FormationClass::follower5(int type){
 
 void FormationClass::sph_follower1(int type){
 
-    std::string message,command,pre_command = "";
+    std::string message,command = "";
+    std::string pre_command = receiver_object.check_command();
     while(true){
         
         if(type == 0){
@@ -411,11 +473,16 @@ void FormationClass::calculate_position(float k,float theta,int direction){
 
     float deg_phi = request_object.get_leader_heading()/100;
     float heading = GPS_object.get_heading();
+    std::string type = receiver_object.check_command();
     
     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         
+    #ifdef DEBUG
+    uint64_t get_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    #endif
+
     follower_location=GPS_object.gps_position();
 
 
@@ -430,15 +497,16 @@ void FormationClass::calculate_position(float k,float theta,int direction){
     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);
     
-    float error_x = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error
-    float error_y = 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] );
 
-    // float error_x = Pf[0] - (follower_location.lon*1e+5);            //caculate error
-    // float error_y = Pf[1] - (follower_location.lat*1e+5);
+    // error_lon = Pf[0] - (follower_location.lon*1e+5);            //caculate error
+    // error_lat = Pf[1] - (follower_location.lat*1e+5);
+
     
     float error_degree = deg_phi - heading + direction;
     float error_yaw = 0.2;   //CCW
@@ -452,11 +520,11 @@ void FormationClass::calculate_position(float k,float theta,int direction){
 
     error_yaw = check_direction(error_degree)*error_yaw;              //check yaw direction
 
-    if (error_x < 0.3 && error_x > -0.3){                     //ignore small errors                  
-        error_x = 0;
+    if (error_lon < ignore_small && error_lon > -ignore_small){                     //ignore small errors                  
+        error_lon = 0;
     }
-    if (error_y < 0.3 && error_y > -0.3){
-        error_y = 0;
+    if (error_lat < ignore_small && error_lat > -ignore_small){
+        error_lat = 0;
     }
     if (error_degree < 5 && error_degree > -5 ){
         error_yaw = 0;
@@ -464,18 +532,41 @@ void FormationClass::calculate_position(float k,float theta,int direction){
         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("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("%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("************************************");
+    #ifdef DEBUG
+    uint64_t cal_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    #endif
 
-    command_object.set_velocity(error_x,error_y,0,error_yaw,0.1);
-
+    #ifndef DEBUG
+    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);
+    #endif
+    // command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
+    // command_object.set_velocity(0,0,0,error_yaw,0.1);
 }
 
 void FormationClass::spherical_coordinate(float k,float theta,float psi){
@@ -512,15 +603,15 @@ void FormationClass::spherical_coordinate(float k,float theta,float psi){
         Pf[2] = k*sin(psi) + cur_alt/100;
     }
     
-    float error_x = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error
-    float error_y = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);       //follower_pid defined in header file
+    // float error_x = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error
+    // float error_y = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);       //follower_pid defined in header file
     float error_alt = Pf[2] - follower_location.alt;
     
     // 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] );
 
-    // float error_x = Pf[0] - (follower_location.lon*1e+5);            //caculate error
-    // float error_y = Pf[1] - (follower_location.lat*1e+5);
+    float error_x = Pf[0] - (follower_location.lon*1e+5);            //caculate error
+    float error_y = Pf[1] - (follower_location.lat*1e+5);
     
     float error_degree = deg_phi - heading;
     float error_yaw = 0.2;
@@ -548,7 +639,7 @@ void FormationClass::spherical_coordinate(float k,float theta,float psi){
     // 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_x,error_y);
     // ROS_INFO("deg:%f",deg_phi);
     // ROS_INFO("heading:%f",heading);
     // ROS_INFO("error_degree:%f",error_degree);
@@ -557,13 +648,17 @@ void FormationClass::spherical_coordinate(float k,float theta,float psi){
     ROS_INFO("Q[2]:%f ,Pf[2]:%f ,error_z:%f",k*sin(psi),Pf[2],error_z);
     // ROS_INFO("************************************");
     
-    command_object.set_velocity(error_x,error_y,error_z,error_yaw,0.1);
+    // command_object.set_velocity(error_x,error_y,error_z,error_yaw,0.1);
 
 }
 
 void FormationClass::go2target(float x,float y){
 
     leader_location=request_object.get_leader_GPS();
+    #ifdef DEBUG
+    uint64_t get_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    #endif
+
     current_location=GPS_object.gps_position(); 
 
     float target_heading,error_heading;
@@ -578,27 +673,50 @@ void FormationClass::go2target(float x,float y){
         face2target(target_heading);
     }
 
-    float error_x = PID_x.update(current_location.lon*1e5 , target_location.lon ,leader_pid);   //leader_pid defined in header file
-    float error_y = PID_y.update(current_location.lat*1e5 , target_location.lat ,leader_pid);
+    drone_speed.x = PID_x.update(current_location.lon*1e5 , target_location.lon ,leader_pid);   //leader_pid defined in header file
+    drone_speed.y = PID_y.update(current_location.lat*1e5 , target_location.lat ,leader_pid);
 
     // float error_x = PID_x.update1(current_location.lon*1e5 , target_location.lon );
     // float error_y = PID_y.update1(current_location.lat*1e5 , target_location.lat );
 
-    if (error_x < 0.3 && error_x > -0.3){                     //ignore small errors                  
-        error_x = 0;
+    if (drone_speed.x < ignore_small && drone_speed.x > -ignore_small){                     //ignore small errors                  
+        drone_speed.x = 0;
     }
-    if (error_y < 0.3 && error_y > -0.3){
-        error_y = 0;
+    if (drone_speed.y < ignore_small && drone_speed.y > -ignore_small){
+        drone_speed.y = 0;
+    }
+    if (drone_speed.x < -1){                     //max velociy                 
+        drone_speed.x = -1;
+    }
+    if (drone_speed.x > 1){                                      
+        drone_speed.x = 1;
+    }
+    if (drone_speed.y < -1){
+        drone_speed.y = -1;
+    }
+    if (drone_speed.y > 1){
+        drone_speed.y = 1;
     }
 
-    if (error_x == 0 && error_y == 0){
+    if (drone_speed.x == 0 && drone_speed.y == 0){
         flag = 1;
     }
 
-    command_object.set_velocity(error_x ,error_y ,0,0,0.1);
+    #ifdef DEBUG
+    uint64_t cal_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    #endif
+
+    PubData_object.publishData(drone_speed);
+    #ifndef DEBUG
+    command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
+    #endif
+    #ifdef DEBUG
+    command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1,get_data_t,cal_data_t);
+    #endif
+    // command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,target_heading);
     
     // ROS_INFO("slope:%f,%f,%f",slope,x,y);
-    // ROS_INFO("%f,%f",error_x,error_y);
+    ROS_INFO("%f,%f",drone_speed.x,drone_speed.y);
     // 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);
@@ -620,7 +738,14 @@ void FormationClass::face2target(float target_heading){
         error_yaw = 0;
         heading_status = 1;
     }
+    
+    #ifndef DEBUG
     command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
+    #endif
+    #ifdef DEBUG
+    command_object.set_velocity(0 ,0 ,0,error_yaw,0.1,0,0);
+    #endif
+    ROS_INFO("face2target...");
 }
 
 

class_model/src/json.cpp

@@ -11,13 +11,13 @@ void callBack(const std_msgs::String::ConstPtr &data){
 
     json j_data = json::parse(data->data);
     std::cout <<"original message"<< data->data << std::endl;
-    std::cout <<"parse message"<< j_data << std::endl;
-    std::cout <<"parse message"<< j_data["lat"] << std::endl;
+    // std::cout <<"parse message"<< j_data << std::endl;
+    // std::cout <<"parse message"<< j_data["lat"] << std::endl;
 
     std::string k = j_data["lat"] , q = j_data["lon"];
     int lat = std::stoi(k) ,lon = std::stoi(q);
-    std::cout <<"lat:"<< k << std::endl;
-    std::cout <<"lon:"<< q << std::endl;
+    std::cout <<"lat:"<< j_data["lat"] << std::endl;
+    std::cout <<"lon:"<< j_data["lon"] << std::endl;
 
 }
 
@@ -37,15 +37,15 @@ int main(int argc ,char **argv)
     ros::AsyncSpinner spinner(0);
 	spinner.start();   
 
-    std::string rostopicName = "jsonData";
-    ros::Publisher data_pub = json_node.advertise<std_msgs::String>(rostopicName,100);
+    std::string rostopicName = "/uav_message";
+    // ros::Publisher data_pub = json_node.advertise<std_msgs::String>(rostopicName,100);
     ros::Subscriber data_receive = json_node.subscribe<std_msgs::String>(rostopicName,100,callBack); 
     //ros::Subscriber data_receive1 = json_node.subscribe<std_msgs::String>("/uav_message",100,callBack);
 
     mes.data = data;
-    while(ros::ok()){
-    data_pub.publish(mes);
-    }
+    // while(ros::ok()){
+    // data_pub.publish(mes);
+    // }
     // json j_no_init_list = json::array();
     // json j_empty_init_list = json::array({});
     // json j_nonempty_init_list = json::array({1, 2, 3, 4});

class_model/src/local_mqtt_pub_data_to_ros.py

@@ -5,12 +5,12 @@ import ssl
 import rospy
 from std_msgs.msg import String
 import paho.mqtt.client as mqtt
-
+import json
 
 
 # Ros
 def ros_pub(dataJson):
-    # global publisher, rate
+    global publisher, rate
     # data = json.loads(dataJson)
     publisher.publish(dataJson)     #將date字串發布到topic
     rate.sleep()
@@ -26,7 +26,7 @@ def initialise_clients(clientname):
 def on_connect(client, userdata, flags, rc):
     print("Connected with result code " + str(rc))
     # client.subscribe(mqtt_config["topic"])
-    client.subscribe("data/sensor")
+    client.subscribe("data/imu")
 
 
 def on_message(client, userdata, msg):
@@ -35,11 +35,9 @@ def on_message(client, userdata, msg):
     ros_pub(msg.payload.decode('utf-8'))
 
 
-
 if __name__ == '__main__':
     # Mqtt
-    mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"}
-     
+    mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "data/imu"}
     client = initialise_clients("123456")
     client.on_connect = on_connect
     client.on_message = on_message
@@ -53,7 +51,6 @@ if __name__ == '__main__':
     # initialize Ros node
     topicName = 'uav_message'
     publisher = rospy.Publisher(topicName,String,queue_size=10)
-
     rate = rospy.Rate(10)
 
 

class_model/src/local_mqtt_sub_data_from_ros.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 #coding:utf-8
 import sys
-import orjson
+import json
 import paho.mqtt.client as mqtt
 
 
@@ -9,15 +9,12 @@ from traceback import print_tb
 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
-import time
 
-mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"}
+mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "data/imu"}
 data ={}
 # Ros
 def callBack_imu(IMU):
@@ -32,49 +29,34 @@ def callBack_imu(IMU):
     dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z}
     data.update(dataImuUpdate)
     # print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n')
-    print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n')
+    # print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n')
 
 
 def callBack_gps(GPS):
-    lat=int(GPS.latitude*10000000)   #change the scale to centimeters
-    lon=int(GPS.longitude*10000000)
-    alt=int(GPS.altitude*100)
-    dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt}
+    lat=str(int(GPS.latitude*10000000))
+    lon=str(int(GPS.longitude*10000000))
+    dataGpsUpdate = {"lat": lat, "lon": lon}
     data.update(dataGpsUpdate)
-    # dataJsonFormate = orjson.dumps(data)
+    # dataJsonFormate = json.dumps(data)
     # mqtt_Pub(message=dataJsonFormate)
     # print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
 
 
 def callBack_compass_hdg(Compass):
-    heading = int(Compass.data*100)
+    heading = str(int(Compass.data*100))
     dataGpsUpdate = {"heading": heading}
     data.update(dataGpsUpdate)
-    dataJsonFormate = orjson.dumps(data)
+    dataJsonFormate = json.dumps(data)
     mqtt_Pub(message=dataJsonFormate)
 
-def callBack_state(state):
-    mode = state.mode
-    dataGpsUpdate = {"mode": mode}
-    data.update(dataGpsUpdate)
-    
-
-
-# get parameter
-def GetParam(self,param_name):
-    param = self.get_param_srv(param_name)
-    if param.success:
-        if param.value.integer != 0:
-            value = param.value.integer
-    
-        else:
-            value = param.value.real
-
-    else:
-        rospy.logwarn("Parameter "+param_name+" not read") 
-        value = 0
-    return value 
 
+# def callBack_velocity(velocity):
+#     Vy=str(int(velocity.latitude*100))
+#     Vx=str(int(velocity.longitude*100))
+#     dataGpsUpdate = {"Vx": Vx, "Vy": Vy}
+#     data.update(dataGpsUpdate)
+#     dataJsonFormate = json.dumps(data)
+#     mqtt_Pub(message=dataJsonFormate)
 
 # MQTT
 def initialise_clients(clientname):
@@ -94,6 +76,7 @@ def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False):
             time.sleep(0.25)
         client.topic_ack.remove(mid)
 
+
 def on_publish(self, userdata, mid):
     client.topic_ack.append(mid)
 
@@ -122,15 +105,10 @@ if __name__ == '__main__':
 	
     ros_namespace="/drone1"
     # topicName = 'data_topic'
-    # subscriber = rospy.Subscriber(ros_namespace+'/mavros/imu/data_raw',Imu,callBack_imu)  #從topic獲取string再呼叫callback
+    # subscriber = rospy.Subscriber('/mavros/imu/data_raw',Imu,callBack_imu)  #從topic獲取string再呼叫callback
     subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/global',NavSatFix,callBack_gps)  #從topic獲取string再呼叫callback
     subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg',Float64,callBack_compass_hdg)  #從topic獲取string再呼叫callback
-    # subscriber = rospy.Subscriber(ros_namespace+"/mavros/next_command",String,callBack_message)
-    #subscriber = rospy.Subscriber(ros_namespace+'/mavros/mavros_msgs/State',Header,callBack_state)
-    # ID = rospy.get_param(ros_namespace+'/leader/droneID')
+    # subscriber = rospy.Subscriber(ros_namespace+'/mavros/leader_velocity',NavSatFix,callBack_velocity)  #從topic獲取string再呼叫callback
     # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng)  #從topic獲取string再呼叫callback
 
-    # ID = rospy.GetParam("droneID")
-    # print(ID)
-
     rospy.spin()

class_model/src/main.cpp

@@ -1,5 +1,7 @@
 #include "class_model/mission.h"
 #include "class_model/choose_leader.h"
+#include "class_model/PubInfo.h"
+#include "yaml-cpp/yaml.h"
 
 int main(int argc, char **argv) {
 
@@ -9,34 +11,92 @@ int main(int argc, char **argv) {
     // reate Assync spiner
     ros::AsyncSpinner spinner(0);
 	spinner.start();
+    // ros::Rate rate(5);   
 
     ModeClass mode_object;
     ControlClass control_object;
     ReceiverClass receiver_object;
     MissionClass mission_object;
     CommandClass command_object;
-    SelectionClass selection_object;
+    SelectClass select_formation;
+    PubInfoClass PubInfo_object;
+    
+    // SelectionClass selection_object;
     
+    // YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
+    std::string type = "",pre_type ="x";
+    int flag = 0;
 
+    mode_object.set_Mode("GUIDED");
+    while(flag!=1){
+        type = receiver_object.check_command();
+        if(type == "go"){
             mode_object.set_Mode("GUIDED");
             control_object.arm();
-    control_object.takeoff(4.5);
-    sleep(5);
+            control_object.takeoff(2);
+            sleep(2);
+            flag = 1;
+            type ="";
+        }   
+    }
+
 
-    selection_object.choose_leader();
+    // selection_object.choose_leader();
     // mission_object.start();   //default hover in goose formation
 
-    while(ros::ok()){
+    // INFO msg;
+    // msg.type = "V_formation";
+    // msg.mission = "fly2target";
 
-    //json_object.get_command();       //for follower
     
 
+    while(ros::ok() && flag == 1){
+        type = receiver_object.check_command();
+        
+    if(pre_type == type){
+        type = "alreadly_set";
+        // ROS_INFO("set");
+    }
+    
+    if(type == "init"){
+        select_formation.goose_formation();
+        ROS_INFO("init formation");
+        pre_type ="init";
+    }else if(type == "line"){
+        select_formation.line_formation();
+        pre_type ="line";
+    }else if(type == "row"){
+        select_formation.row_formation();
+        pre_type ="row";
+    }else if(type == "v1"){                         
+        select_formation.goose_formation(0,5);
+        pre_type ="v1";
+    }else if(type == "v2"){
+        select_formation.house_formation(-5,0);
+        pre_type ="v2";
+    }else if(type == "v3"){
+        select_formation.goose_formation(0,-5);
+        pre_type ="v3";
+    }else if(type == "v4"){
+        select_formation.house_formation(5,0);
+        pre_type ="v4";
+    }else if(type == "house"){
+        select_formation.house_formation();
+        pre_type ="house";
+    }else if(type == "stop"){
+        select_formation.stop();
+    }else if(type == "land"){
+        // ROS_INFO("Land");
+        mode_object.set_Mode("LAND");
+    }
+
+    }
 
     ros::waitForShutdown();
 
     return 0;
 }
-//////////////////////////////////////////////////////
+
 
 /*
 ThreadGPSClass gps_object;

class_model/src/mode.cpp

@@ -26,8 +26,9 @@ int ModeClass::set_Mode(std::string mode) {
     // ROS_INFO("setmode %s ok",mode.c_str());
     return 0;
     }else{
-      ROS_ERROR("Failed SetMode %s",mode.c_str());
-      return -1;
+      ROS_ERROR("Failed SetMode %s,retry...",mode.c_str());
+      set_Mode(mode);
+      return 0;
     }
 
 }

class_model/src/mqttCmdMain.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import paho.mqtt.client as mqtt
+import os
+import sys
+import time
+from utils.readConfig import Read_CMD_Config
+from utils.basicMqtt import MQTTClient
+from utils.protoJson_mqtt_cmd_data_to_ros import Json_msg_to_ros
+import logging
+
+from std_msgs.msg import String
+import rospy
+
+def init_dataFormat(cfg:Read_CMD_Config):
+    if cfg.msg_format == "Json":
+        Json_msg_to_ros.rate = rospy.Rate(10)
+        Json_msg_to_ros.publisher_Cmd_Broadcast = rospy.Publisher(cfg.ROStopicName_Cmd_Broadcast_Receiver,String,queue_size=10)        
+        client.on_message = Json_msg_to_ros.on_message
+        Json_msg_to_ros.Cmd_Broadcast_topicToMqtt = cfg.Cmd_Broadcast_topicToMqtt
+    elif cfg.msg_format == "Proto":
+        pass
+    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.Cmd_Broadcast_topicToMqtt)
+
+def on_disconnect(client, userdata, rc):
+    logger.info("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_CMD.yml")
+    cfg = Read_CMD_Config(FilePath)
+    client = MQTTClient(cfg.MQTTClientNameCmd)
+
+    # 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("__CMD__")
+    logger.setLevel(logging.DEBUG)
+    logger.addHandler(file_handler)
+    logger.addHandler(stream_handler)
+    logger.info(cfg)
+    
+    # Mqtt
+    client.on_connect = on_connect
+    client.on_disconnect = on_disconnect
+    client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
+    client.loop_start()
+
+    # Ros
+    rospy.init_node(cfg.ROSClientNameCmd)
+    
+    # init data format
+    init_dataFormat(cfg)
+
+    try:
+        rospy.spin()
+    except BaseException as error:
+        client.disconnect()
+        logger.info("End of program")
+        sys.exit()
+