add bio drones into simulation environment(need debugging)

class_model/include/class_model/DroneStatus.h

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

class_model/include/class_model/Leader.h

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

class_model/include/class_model/follower.h

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

class_model/include/class_model/requestData.h

@@ -4,10 +4,14 @@
 
 #include <ros/ros.h>
 #include <std_msgs/String.h>
+#include <iostream>
 #include <string>
 #include <class_model/gps_location.h>
 #include <nlohmann/json.hpp>
 #include <algorithm>
+#include <ros/message_event.h>
+#include "yaml-cpp/yaml.h"
+
 // #include "rapidjson/document.h"
 
 using json = nlohmann::json;
@@ -30,28 +34,28 @@ private:
     ros::NodeHandle node_handle_;
 
     void Data_callback(const std_msgs::String::ConstPtr &gps);
-    void self_callback(const std_msgs::String::ConstPtr &gps);
-    void M1_callback(const std_msgs::String::ConstPtr &gps);
-    void M2_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(std::string data);
+    void Bio_INFO(json json_data,int drone_ID);
     float heading;
     global_location leader_position;
     global_location self_position,M1_position,M2_position;
-    std::array <int,3> ID {1,2,3};
+    std::array <int,3> ID_array {1,2,3};
     // rapidjson::Document document;
-    json j_data;
-    int self_ID,drone_ID;
+    json j_data,j_data2,j_data3;
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
+    int self_ID= config["DroneParam"]["ID"].as<int>(),drone_ID;
 
   // SUBSCRIBE
     ros::Subscriber mqtt_data; 
     ros::Subscriber formation_data;
-    ros::Subscriber self_data;
-    ros::Subscriber member1_data;
-    ros::Subscriber member2_data;
-
+    ros::Subscriber drone1_data;
+    ros::Subscriber drone2_data;
+    ros::Subscriber drone3_data;
 };
 
 #endif // REQUESTDATA_H