add 5 bio_drones test

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();