merge tony conflict

class_model/include/class_model/DroneStatus.h

@@ -0,0 +1,7 @@
+#include <std_msgs/String.h>
+typedef struct DroneStatus
+{
+    /* data */
+    int plane;
+    std::string leader;
+} DroneStatus;

class_model/include/class_model/follower.h

@@ -8,6 +8,7 @@
 #include "class_model/control.h"
 #include "class_model/command.h"
 #include "class_model/requestData.h"
+#include "class_model/DroneStatus.h"
 
 
 class FollowerClass {
@@ -20,7 +21,12 @@ public:
     ControlClass control_object;
     CommandClass command_object;
     RequestClass request_object;
+    DroneStatus drone1;
+    DroneStatus drone2;    
+    DroneStatus drone3;
+
     void follower();
+    
 private:
     // ROS NodeHandle
     ros::NodeHandle node_handle_;
@@ -30,6 +36,9 @@ private:
     global_location leader_location;
 
     void calculate_position(float k,float theta);
+    void plane(global_location target_location,global_location leader_location,global_location follower_location);
+    void choose_leader();
+    void follow();
 
 };
 

class_model/src/follower.cpp

@@ -9,17 +9,25 @@ FollowerClass::~FollowerClass() { ros::shutdown(); }
 
 void FollowerClass::follower(){
 
-    mode_object.set_Mode("GUIDED");
-    control_object.arm();
-    control_object.takeoff(1.5);
-    sleep(5);
-    while(true){
-        calculate_position(4,30);
-    }
+    //plane()
+    //follow()
 
-    sleep(2);
+}
 
-    mode_object.set_Mode("LAND");
+void FollowerClass::plane(global_location target_location,global_location leader_location,global_location follower_location){
+    
+    //caculate drone is locate on R/L plane
+    float slope,c,y;
+    slope = (target_location.lat-leader_location.lat)/(target_location.lon-leader_location.lon);
+    c = target_location.lat - slope*target_location.lon;
+    y = slope * follower_location.lon + c;
+
+    // if()
+    
+    //choose_leader()
+}
+
+void FollowerClass::choose_leader(){
 
 }
 

class_model/src/mode.cpp

@@ -27,7 +27,7 @@ int ModeClass::set_Mode(std::string mode) {
     return 0;
     }else{
       ROS_ERROR("Failed SetMode %s,retry...",mode.c_str());
-      // set_Mode(mode);
+      set_Mode(mode);
       return 0;
     }
 

class_model/src/mqttPubMain.py

@@ -82,7 +82,6 @@ if __name__ == '__main__':
     
     # Mqtt
     client.on_connect = on_connect
-    client.on_message = on_message
     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)

class_model/src/requestData.cpp

@@ -7,12 +7,12 @@ int command;
 
 RequestClass::RequestClass() : node_handle_(""){
 
-  // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
-  //                                  &RequestClass::Data_callback, this);
+  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", 100,
-                                   &RequestClass::Data_callback, this);
+  // mqtt_data = node_handle_.subscribe("/uav_message", 100,
+  //                                  &RequestClass::Data_callback, this);
 
 }
 

class_model/src/utils/__init__.py

@@ -2,5 +2,6 @@ from utils.protoJson_mqtt_pub_data_to_ros import Proto_msg_to_ros
 from utils.protoJson_mqtt_pub_data_to_ros import Json_msg_to_ros
 from utils.protoJson_mqtt_sub_data_from_ros import Proto_msg_from_ros
 from utils.protoJson_mqtt_sub_data_from_ros import Json_msg_from_ros
-from utils.readConfig import MQTT_ROS_Config
-from utils.basicMqtt import MQTTClient
+from utils.readConfig import Read_PUB_Config
+from utils.readConfig import Read_SUB_Config
+from utils.basicMqtt import MQTTClient

class_model/src/utils/mqttConfig_PUB.yml

@@ -1,7 +1,7 @@
 MQTT:
-  msg_format: Json
+  msg_format: Proto
   MQTTClientNamePub: Drone550Pub
-  host: 192.168.50.118
+  host: 140.120.31.133
   port: 1883
   keepalive: 60
   willTopic: CheckDoneConnect

class_model/src/utils/mqttConfig_SUB.yml

@@ -1,7 +1,7 @@
 MQTT:
-  msg_format: Json
+  msg_format: Proto
   MQTTClientNameSub: Drone550Sub
-  host: 192.168.50.118
+  host: 140.120.31.133
   port: 1883
   keepalive: 60
   willTopic: CheckDoneConnect

class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py

@@ -87,8 +87,8 @@ class Json_msg_from_ros:
     def callBack_compass_hdg(cls, Compass):
         heading = int(Compass.data*100)
         dataGpsUpdate = {"heading": heading}
-        cls.data.update(dataGpsUpdate)
-        dataJsonFormate = orjson.dumps(cls.data)
+        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):
@@ -103,4 +103,4 @@ class Json_msg_from_ros:
             while mid not in cls.client.topic_ack:
                 print("wait for ack")
                 time.sleep(0.25)
-            cls.client.topic_ack.remove(mid)
+            cls.client.topic_ack.remove(mid)