add 5 bio_drones test

class_model/CMakeLists.txt

@@ -309,6 +309,13 @@ 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)

class_model/include/class_model/follower.h

@@ -31,6 +31,8 @@ public:
     DroneStatus drone1;
     DroneStatus drone2;    
     DroneStatus drone3;
+    DroneStatus drone4;
+    DroneStatus drone5;
     DroneStatus self;
     DroneStatus samePlane[5];
     
@@ -56,7 +58,7 @@ private:
     float distance = config["formation"]["distance"].as<float>();
     float angle = config["formation"]["angle"].as<float>(); 
     float error_lon,error_lat;
-    float follower_pid[3] = {1 , 0.00000000001 ,0.5};
+    float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
     float ignore_small = 0.50;
 
     void calculate_position(float k,float theta,int plane);

class_model/include/class_model/formation.h

@@ -37,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);
@@ -54,8 +55,8 @@ private:
     int flag = 0,heading_status = 0,buf = 0;
     float pre_alt,cur_alt;
     float error_lon,error_lat;
-    float leader_pid[3] = {1 , 0.000000000001 ,0.5};
-    float follower_pid[3] = {1 , 0.00000000001 ,0.5};
+    float leader_pid[3] = {1 , 0.000000000001 ,0};
+    float follower_pid[3] = {1 , 0.00000000001 ,0};
     float ignore_small = 0.50;
 
     void calculate_position(float k,float theta,int direction=0);

class_model/include/class_model/requestData.h

@@ -25,6 +25,8 @@ public:
     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();
@@ -37,16 +39,18 @@ private:
     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 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};
+    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,j_data2,j_data3;
+    json j_data,j_data2,j_data3,j_data4,j_data5;
     int self_ID,drone_ID;
 
   // SUBSCRIBE
@@ -55,6 +59,8 @@ private:
     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/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/launch/mqtt_old.launch

@@ -11,7 +11,7 @@
 	</group>
 	<group>
 		
-		<node pkg="class_model" type="mqttCmdMain.py" name="command" output="screen">
+		<node pkg="class_model" type="command.py" name="command" output="screen">
 		</node>
 	</group>
 	

class_model/src/PID.cpp

@@ -12,7 +12,7 @@ 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)/1.1;
+    float error = (target - current);
 
     double P,D,result;
     P = pidvals[0] * error;

class_model/src/bio_main.cpp

@@ -21,7 +21,7 @@ int main(int argc, char **argv) {
     
     // SelectionClass selection_object;
 
-    global_location target,self,M1,M2;
+    global_location target,self,M1,M2,M3,M4;
     int ref_ID,checkLeader;
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
@@ -32,9 +32,13 @@ int main(int argc, char **argv) {
     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};
-    global_location member[] = {M1,M2};
+    // global_location pos[] = {target,self,M1,M2};
+    // global_location member[] = {M1,M2};
+    global_location pos[] = {target,self,M1,M2,M3,M4};
+    global_location member[] = {M1,M2,M3,M4};
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 

class_model/src/bio_main2.cpp

@@ -21,10 +21,10 @@ int main(int argc, char **argv) {
     
     // SelectionClass selection_object;
 
-    global_location target,self,M1,M2;
+    global_location target,self,M1,M2,M3,M4;
     int ref_ID,checkLeader;
     
-    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config2.yaml");  //reading yaml parameter(target point)
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
     target.lon = config["routh1"]["x"].as<float>()*1e7;
     target.lat = config["routh1"]["y"].as<float>()*1e7;
 
@@ -32,11 +32,13 @@ int main(int argc, char **argv) {
     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();
 
-    std::cout <<M1.lat<<M1.ID<<std::endl;
-
-    global_location pos[] = {target,self,M1,M2};
-    global_location member[] = {M1,M2};
+    // global_location pos[] = {target,self,M1,M2};
+    // global_location member[] = {M1,M2};
+    global_location pos[] = {target,self,M1,M2,M3,M4};
+    global_location member[] = {M1,M2,M3,M4};
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 

class_model/src/bio_main3.cpp

@@ -21,10 +21,10 @@ int main(int argc, char **argv) {
     
     // SelectionClass selection_object;
 
-    global_location target,self,M1,M2;
+    global_location target,self,M1,M2,M3,M4;
     int ref_ID,checkLeader;
     
-    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config3.yaml");  //reading yaml parameter(target point)
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
     target.lon = config["routh1"]["x"].as<float>()*1e7;
     target.lat = config["routh1"]["y"].as<float>()*1e7;
 
@@ -32,9 +32,13 @@ int main(int argc, char **argv) {
     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};
-    global_location member[] = {M1,M2};
+    // global_location pos[] = {target,self,M1,M2};
+    // global_location member[] = {M1,M2};
+    global_location pos[] = {target,self,M1,M2,M3,M4};
+    global_location member[] = {M1,M2,M3,M4};
     size_t index = sizeof(pos)/sizeof(pos[0]);
     size_t m_index = sizeof(member)/sizeof(member[0]);
 

class_model/src/bio_main4.cpp

@@ -0,0 +1,66 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "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;
+    
+    // SelectionClass selection_object;
+
+    global_location target,self,M1,M2,M3,M4;
+    int ref_ID,checkLeader;
+    
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
+    target.lon = config["routh1"]["x"].as<float>()*1e7;
+    target.lat = config["routh1"]["y"].as<float>()*1e7;
+
+    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};
+    // global_location member[] = {M1,M2};
+    global_location pos[] = {target,self,M1,M2,M3,M4};
+    global_location member[] = {M1,M2,M3,M4};
+    size_t index = sizeof(pos)/sizeof(pos[0]);
+    size_t m_index = sizeof(member)/sizeof(member[0]);
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(2);
+
+    checkLeader = check_leader(pos,index).is_leader;
+    if(checkLeader != 1){
+        ref_ID = follower_object.find_ref_drone(pos,index,check_leader(pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
+    }
+
+    while(true){
+        
+        if(checkLeader == 1){
+            // leader_object.leader(target);
+        }else{
+            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
+        }
+    }
+    ros::waitForShutdown();
+
+    return 0;
+
+}

class_model/src/bio_main5.cpp

@@ -0,0 +1,66 @@
+#include "class_model/FindLeader.h"
+#include "class_model/Leader.h"
+#include "class_model/follower.h"
+#include "yaml-cpp/yaml.h"
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "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;
+    
+    // SelectionClass selection_object;
+
+    global_location target,self,M1,M2,M3,M4;
+    int ref_ID,checkLeader;
+    
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
+    target.lon = config["routh1"]["x"].as<float>()*1e7;
+    target.lat = config["routh1"]["y"].as<float>()*1e7;
+
+    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};
+    // global_location member[] = {M1,M2};
+    global_location pos[] = {target,self,M1,M2,M3,M4};
+    global_location member[] = {M1,M2,M3,M4};
+    size_t index = sizeof(pos)/sizeof(pos[0]);
+    size_t m_index = sizeof(member)/sizeof(member[0]);
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(2);
+
+    checkLeader = check_leader(pos,index).is_leader;
+    if(checkLeader != 1){
+        ref_ID = follower_object.find_ref_drone(pos,index,check_leader(pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
+    }
+
+    while(true){
+        
+        if(checkLeader == 1){
+            // leader_object.leader(target);
+        }else{
+            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
+        }
+    }
+    ros::waitForShutdown();
+
+    return 0;
+
+}

class_model/src/command.py

@@ -18,6 +18,7 @@ def ros_pub(dataJson):
     publisher3.publish(dataJson)
     publisher4.publish(dataJson)
     publisher5.publish(dataJson)
+    publisher6.publish(dataJson)
 
     rate.sleep()
     # print(f"publish data {data}")
@@ -71,6 +72,9 @@ if __name__ == '__main__':
     topicName5 = '/drone5/cmd_receiver'
     publisher5 = rospy.Publisher(topicName5,String,queue_size=10)
 
+    topicName6 = '/drone6/cmd_receiver'
+    publisher6 = rospy.Publisher(topicName6,String,queue_size=10)
+
 
     rate = rospy.Rate(10)
 

class_model/src/follower.cpp

@@ -19,6 +19,12 @@ void FollowerClass::follower(global_location* member_data, size_t index, int ref
     }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 = "";
@@ -66,7 +72,19 @@ void FollowerClass::plane(global_location target, global_location leader, global
                 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){
         // std::cout<< follower.lat <<","<< y <<std::endl;   
@@ -89,6 +107,18 @@ void FollowerClass::plane(global_location target, global_location leader, global
                 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;
     }
     }
     
@@ -140,6 +170,18 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
                     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;
@@ -174,6 +216,21 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
         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
@@ -234,15 +291,15 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
     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);
+    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.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);
+    // 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;

class_model/src/formation.cpp

@@ -115,7 +115,7 @@ void FormationClass::follower1(int type){
     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){
@@ -126,6 +126,8 @@ 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();
@@ -155,7 +157,7 @@ void FormationClass::follower2(int type){
     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){
@@ -166,16 +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();
+        // 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;
         }
-        
 
     }
 
@@ -183,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){
@@ -198,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();
+        // 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;
         
 
     }
@@ -217,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){
@@ -232,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();
+        // 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;
 
     }
 
@@ -249,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,-45);
+        }else if(type == 1){
+            calculate_position(20,0);
+        }else if(type == 2){
+            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(20,0);
+            calculate_position(24,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 == 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();
+        // 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;
         
 
     }
@@ -282,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){
@@ -529,15 +589,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;

class_model/src/main.cpp

@@ -72,14 +72,17 @@ int main(int argc, char **argv) {
         select_formation.goose_formation(0,5);
         pre_type ="v1";
     }else if(type == "v2"){
-        select_formation.line_formation(-5,0);
+        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.row_formation(5,0);
+        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"){

class_model/src/mqttPubMain4.py

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

class_model/src/mqttPubMain5.py

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

class_model/src/mqttSubMain.py

@@ -33,11 +33,17 @@ def init_dataFormat(cfg:utils.Read_SUB_Config):
         utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
         utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
         utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone555_publisher_Flight_Information = rospy.Publisher(cfg.Dron555_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone888_publisher_Flight_Information = rospy.Publisher(cfg.Dron888_ROStopicName_Flight_Information,String,queue_size=10)
+
         # set callback to each topic
         client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
         client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
         client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
         client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
+
+        client.message_callback_add(cfg.Drone888_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone888_on_message_Flight_Information)
+        client.message_callback_add(cfg.Drone555_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone555_on_message_Flight_Information)
     else:
         logger.debug("msg_format not found")
 
@@ -49,6 +55,9 @@ def on_connect(self, userdata, flags, rc):
     client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
     client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
 
+    client.subscribe(cfg.Drone888_Flight_Information_topicToMqtt)
+    client.subscribe(cfg.Drone555_Flight_Information_topicToMqtt)
+
 def on_disconnect(client, userdata, rc):
     logger.info("disconnecting reason  "  +str(rc))
     client.connected_flag=False

class_model/src/mqttSubMain2.py

@@ -33,11 +33,17 @@ def init_dataFormat(cfg:utils.Read_SUB_Config):
         utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
         utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
         utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone555_publisher_Flight_Information = rospy.Publisher(cfg.Dron555_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone888_publisher_Flight_Information = rospy.Publisher(cfg.Dron888_ROStopicName_Flight_Information,String,queue_size=10)
+
         # set callback to each topic
         client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
         client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
         client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
         client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
+
+        client.message_callback_add(cfg.Drone888_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone888_on_message_Flight_Information)
+        client.message_callback_add(cfg.Drone555_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone555_on_message_Flight_Information)
     else:
         logger.debug("msg_format not found")
 
@@ -49,6 +55,9 @@ def on_connect(self, userdata, flags, rc):
     client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
     client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
 
+    client.subscribe(cfg.Drone888_Flight_Information_topicToMqtt)
+    client.subscribe(cfg.Drone555_Flight_Information_topicToMqtt)
+
 def on_disconnect(client, userdata, rc):
     logger.info("disconnecting reason  "  +str(rc))
     client.connected_flag=False

class_model/src/mqttSubMain3.py

@@ -33,11 +33,17 @@ def init_dataFormat(cfg:utils.Read_SUB_Config):
         utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
         utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
         utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone555_publisher_Flight_Information = rospy.Publisher(cfg.Dron555_ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.Drone888_publisher_Flight_Information = rospy.Publisher(cfg.Dron888_ROStopicName_Flight_Information,String,queue_size=10)
+
         # set callback to each topic
         client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
         client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
         client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
         client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
+
+        client.message_callback_add(cfg.Drone888_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone888_on_message_Flight_Information)
+        client.message_callback_add(cfg.Drone555_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone555_on_message_Flight_Information)
     else:
         logger.debug("msg_format not found")
 
@@ -49,6 +55,9 @@ def on_connect(self, userdata, flags, rc):
     client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
     client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
 
+    client.subscribe(cfg.Drone888_Flight_Information_topicToMqtt)
+    client.subscribe(cfg.Drone555_Flight_Information_topicToMqtt)
+
 def on_disconnect(client, userdata, rc):
     logger.info("disconnecting reason  "  +str(rc))
     client.connected_flag=False

class_model/src/mqttSubMain4.py

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

class_model/src/mqttSubMain5.py

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

class_model/src/requestData.cpp

@@ -17,13 +17,13 @@ RequestClass::RequestClass() : node_handle_("~"){
 	}
   node_handle_.getParam(ros_namespace+ros_namespace+"/droneID", self_ID);
   ROS_INFO("using namespace %s,ID:%d", ros_namespace.c_str(),self_ID);
-  
+
   // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
   //                                  &RequestClass::Data_callback, this);
   formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
                                    &RequestClass::Message_callback, this);
-  // mqtt_data = node_handle_.subscribe("/uav_message", 10,
-  //                                  &RequestClass::Data_callback, this);
+  mqtt_data = node_handle_.subscribe("/uav_message", 10,
+                                   &RequestClass::Data_callback, this);
 
   drone1_data = node_handle_.subscribe("/Dron650_Flight_Information_reciver", 10,
                                    &RequestClass::drone1_callback, this);       //test bionic
@@ -31,6 +31,11 @@ RequestClass::RequestClass() : node_handle_("~"){
                                    &RequestClass::drone2_callback, this);
   drone3_data = node_handle_.subscribe("/Dron380_Flight_Information_reciver", 10,
                                    &RequestClass::drone3_callback, this);
+  drone4_data = node_handle_.subscribe("/Dron888_Flight_Information_reciver", 10,
+                                   &RequestClass::drone4_callback, this);
+  drone5_data = node_handle_.subscribe("/Dron555_Flight_Information_reciver", 10,
+                                   &RequestClass::drone5_callback, this);
+
 
 
 }
@@ -122,6 +127,16 @@ void RequestClass::drone3_callback(const std_msgs::String::ConstPtr &sensor) {
   j_data3 = json::parse(sensor->data); 
   Bio_INFO(j_data3,drone_ID);
 }
+void RequestClass::drone4_callback(const std_msgs::String::ConstPtr &sensor) {
+  drone_ID = 4;
+  j_data4 = json::parse(sensor->data); 
+  Bio_INFO(j_data4,drone_ID);
+}
+void RequestClass::drone5_callback(const std_msgs::String::ConstPtr &sensor) {
+  drone_ID = 5;
+  j_data5 = json::parse(sensor->data); 
+  Bio_INFO(j_data5,drone_ID);
+}
 
 void RequestClass::Bio_INFO(json json_data,int drone_ID){
 
@@ -146,6 +161,18 @@ void RequestClass::Bio_INFO(json json_data,int drone_ID){
     // M2_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
     M2_position.heading = json_data["heading"];
     M2_position.ID = drone_ID; 
+  }else if (drone_ID == ID_array[2]){
+    M3_position.lat = json_data["lat"]; 
+	  M3_position.lon = json_data["lon"]; 
+    // M2_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
+    M3_position.heading = json_data["heading"];
+    M3_position.ID = drone_ID; 
+  }else if (drone_ID == ID_array[3]){
+    M4_position.lat = json_data["lat"]; 
+	  M4_position.lon = json_data["lon"]; 
+    // M2_position.alt = std::stoi(json_data["alt"].get<std::string>()); 
+    M4_position.heading = json_data["heading"];
+    M4_position.ID = drone_ID; 
   }
 
 }
@@ -159,4 +186,12 @@ global_location RequestClass::get_M1_GPS(){
 global_location RequestClass::get_M2_GPS(){
 
   return M2_position; 
+}
+global_location RequestClass::get_M3_GPS(){
+
+  return M3_position; 
+}
+global_location RequestClass::get_M4_GPS(){
+
+  return M4_position; 
 }

class_model/src/select.cpp

@@ -57,6 +57,8 @@ void SelectClass::goose_formation(float x ,float y){
         formation_object.follower4(counter);
     }else if(param_object.getID() == 6){
         formation_object.follower5(counter);
+    }else if(param_object.getID() == 7){
+        formation_object.follower6(counter);
     }
 
 
@@ -81,6 +83,8 @@ void SelectClass::line_formation(float x ,float y){
         formation_object.follower4(counter);
     }else if(param_object.getID() == 6){
         formation_object.follower5(counter);
+    }else if(param_object.getID() == 7){
+        formation_object.follower6(counter);
     }
     // sleep(2);
 
@@ -103,6 +107,8 @@ void SelectClass::row_formation(float x ,float y){
         formation_object.follower4(counter);
     }else if(param_object.getID() == 6){
         formation_object.follower5(counter);
+    }else if(param_object.getID() == 7){
+        formation_object.follower6(counter);
     }
     // sleep(2);
 
@@ -191,6 +197,32 @@ void SelectClass::start_formation(){
     }
 }
 
+void SelectClass::house_formation(float x ,float y){
+
+    counter = 6;
+    if(param_object.getID() == 1){
+        formation_object.leader1(x,y);
+    }else if(param_object.getID() == 2){
+        formation_object.follower1(counter);
+    }else if(param_object.getID() == 3){
+        formation_object.follower2(counter);
+    }else if(param_object.getID() == 4){
+        formation_object.follower3(counter);
+    }else if(param_object.getID() == 5){
+        formation_object.follower4(counter);
+    }else if(param_object.getID() == 6){
+        formation_object.follower5(counter);
+    }else if(param_object.getID() == 7){
+        formation_object.follower6(counter);
+    }
+
+
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+
+}
+
 void SelectClass::stop(){
 
 }

class_model/src/utils/mqttConfig_PUB4.yml

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

class_model/src/utils/mqttConfig_PUB5.yml

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

class_model/src/utils/mqttConfig_SUB.yml

@@ -16,6 +16,10 @@ MQTT:
 
   Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
 
+  Drone888_Flight_Information_topicToMqtt: Drone888/Flight_Information
+
+  Drone555_Flight_Information_topicToMqtt: Drone555/Flight_Information
+
   # Change formate qos
   Fly_Formation_topicToMqtt_QOS: 2
 ROS:
@@ -26,5 +30,8 @@ ROS:
   Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
 
   Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
+
+  Dron888_ROStopicName_Flight_Information: Dron888_Flight_Information_reciver
+  Dron555_ROStopicName_Flight_Information: Dron555_Flight_Information_reciver
 LOG:
   logFileName: sub.log

class_model/src/utils/mqttConfig_SUB2.yml

@@ -6,7 +6,7 @@ MQTT:
   keepalive: 60
   willTopic: CheckDoneConnect
   willTopicQOS: 1
-  lwt: Dron550 Gone Offline
+  lwt: Dron650 Gone Offline
   willRetain: False
   # Mqtt topic
   Fly_Formation_topicToMqtt: Drone650/Formation
@@ -16,6 +16,10 @@ MQTT:
 
   Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
 
+  Drone888_Flight_Information_topicToMqtt: Drone888/Flight_Information
+
+  Drone555_Flight_Information_topicToMqtt: Drone555/Flight_Information
+
   # Change formate qos
   Fly_Formation_topicToMqtt_QOS: 2
 ROS:
@@ -26,5 +30,8 @@ ROS:
   Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
 
   Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
+
+  Dron888_ROStopicName_Flight_Information: Dron888_Flight_Information_reciver
+  Dron555_ROStopicName_Flight_Information: Dron555_Flight_Information_reciver
 LOG:
   logFileName: sub.log

class_model/src/utils/mqttConfig_SUB3.yml

@@ -6,7 +6,7 @@ MQTT:
   keepalive: 60
   willTopic: CheckDoneConnect
   willTopicQOS: 1
-  lwt: Dron550 Gone Offline
+  lwt: Dron380 Gone Offline
   willRetain: False
   # Mqtt topic
   Fly_Formation_topicToMqtt: Drone380/Formation
@@ -16,6 +16,9 @@ MQTT:
 
   Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
 
+  Drone888_Flight_Information_topicToMqtt: Drone888/Flight_Information
+  Drone555_Flight_Information_topicToMqtt: Drone555/Flight_Information
+
   # Change formate qos
   Fly_Formation_topicToMqtt_QOS: 2
 ROS:
@@ -26,5 +29,8 @@ ROS:
   Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
 
   Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
+
+  Dron888_ROStopicName_Flight_Information: Dron888_Flight_Information_reciver
+  Dron555_ROStopicName_Flight_Information: Dron555_Flight_Information_reciver
 LOG:
   logFileName: sub.log

class_model/src/utils/mqttConfig_SUB4.yml

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

class_model/src/utils/mqttConfig_SUB5.yml

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

class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py

@@ -41,6 +41,8 @@ class Json_msg_to_ros:
     Drone550_publisher_Flight_Information = None
     Drone380_publisher_Flight_Information = None
     Drone650_publisher_Flight_Information = None
+    Drone888_publisher_Flight_Information = None
+    Drone555_publisher_Flight_Information = None
     #Mqtt topic: check data from which topic
 
     
@@ -76,4 +78,22 @@ class Json_msg_to_ros:
             cls.rate.sleep()
 
         t = threading.Thread(target=thread_function)
-        t.start()        
+        t.start()
+        
+    @classmethod
+    def Drone888_on_message_Flight_Information(cls, client, userdata, msg):
+        def thread_function():
+            cls.Drone888_publisher_Flight_Information.publish(msg.payload.decode("UTF-8"))
+            cls.rate.sleep()
+
+        t = threading.Thread(target=thread_function)
+        t.start()
+       
+    @classmethod
+    def Drone555_on_message_Flight_Information(cls, client, userdata, msg):
+        def thread_function():
+            cls.Drone555_publisher_Flight_Information.publish(msg.payload.decode("UTF-8"))
+            cls.rate.sleep()
+
+        t = threading.Thread(target=thread_function)
+        t.start()

class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py

@@ -78,8 +78,7 @@ class Json_msg_from_ros:
         alt=int(GPS.altitude*100)
         dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt}
         cls.GPS_Data.update(dataGpsUpdate)
-        dataJsonFormate = orjson.dumps(cls.GPS_Data)
-        cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
+
 
 
 # TODO: does decode need utf8 
@@ -90,6 +89,7 @@ class Json_msg_from_ros:
         cls.GPS_Data.update(dataGpsUpdate)
         dataJsonFormate = orjson.dumps(cls.GPS_Data)
         cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
+        
     @classmethod
     def callBack_fly_formation(cls, Formation):
         Formation_data = {"velocity": Formation.velocity, "type": Formation.type}

class_model/src/utils/readConfig.py

@@ -86,6 +86,8 @@ class Read_SUB_Config(Config):
                             "Drone550_Flight_Information_topicToMqtt": (str,False),
                             "Drone380_Flight_Information_topicToMqtt":(str,False),
                             "Drone650_Flight_Information_topicToMqtt":(str,False),
+                            "Drone888_Flight_Information_topicToMqtt":(str,False),
+                            "Drone555_Flight_Information_topicToMqtt":(str,False),
                             "Fly_Formation_topicToMqtt": (str,False),
                             "Fly_Formation_topicToMqtt_QOS":(int,False)},
             self.sectionNames[1]:{
@@ -94,6 +96,9 @@ class Read_SUB_Config(Config):
                             "Dron380_ROStopicName_Flight_Information": (str,False),
                             "Dron380_ROStopicName_Flight_Information": (str,False),
                             "Dron650_ROStopicName_Flight_Information": (str,False),
+                            "Dron888_ROStopicName_Flight_Information": (str,False),
+                            "Dron555_ROStopicName_Flight_Information": (str,False),
+                            "Dron_ROStopicName_Flight_Information": (str,False),
                             "ROStopicName_Fly_Formation": (str,False)},
             self.sectionNames[2]:{
                             "logFileName":(str,False)}}

iq_sim/launch/multi_drone.launch

@@ -1,7 +0,0 @@
-<launch>
-  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="world_name" value="$(find iq_sim)/worlds/multi_drone.world"/>
-    <!-- more default parameters can be changed here -->
-  </include>
-</launch>

iq_sim/worlds/five_drone.world

@@ -117,32 +117,32 @@
 
 
     <model name="drone1">
-      <pose> -10 -10 0 0 0 0</pose>
+      <pose> 0 0 0 0 0 0</pose>
       <include>
         <!-- <uri>model://drone1</uri> -->
         <uri>model://drone_with_camera</uri>
       </include>
     </model>
     <model name="drone2">
-      <pose> -10 -12 0 0 0 0</pose>
+      <pose> 0 4 0 0 0 0</pose>
       <include>
         <uri>model://drone2</uri>
       </include>
     </model>
     <model name="drone3">
-      <pose> -10 -8 0 0 0 0</pose>
+      <pose> 0 8 0 0 0 0</pose>
       <include>
         <uri>model://drone3</uri>
       </include>
     </model>
         <model name="drone4">
-      <pose> -10 -14 0 0 0 0</pose>
+      <pose> 0 -4 0 0 0 0</pose>
       <include>
         <uri>model://drone4</uri>
       </include>
     </model>
         <model name="drone5">
-      <pose> -10 -6 0 0 0 0</pose>
+      <pose> 0 -8 0 0 0 0</pose>
       <include>
         <uri>model://drone5</uri>
       </include>