From 66581e37d249bfc1ee43f2cfba9ebbd0de1a1a60 Mon Sep 17 00:00:00 2001
From: Xuan0319 <new310049@gmail.com>
Date: Thu, 9 Feb 2023 23:56:07 +0800
Subject: [PATCH] bionic (unfinished)

---
 class_model/include/class_model/DroneStatus.h |  3 +++
 class_model/include/class_model/follower.h    |  4 ++++
 class_model/src/follower.cpp                  | 19 +++++++++++++++++--
 class_model/src/utils/mqttConfig_PUB.yml      |  2 +-
 class_model/src/utils/mqttConfig_SUB.yml      |  2 +-
 5 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/class_model/include/class_model/DroneStatus.h b/class_model/include/class_model/DroneStatus.h
index cbf3387..f36be80 100644
--- a/class_model/include/class_model/DroneStatus.h
+++ b/class_model/include/class_model/DroneStatus.h
@@ -1,7 +1,10 @@
 #include <std_msgs/String.h>
+#include "class_model/gps_location.h"
 typedef struct DroneStatus
 {
     /* data */
+    global_location init_location;
     int plane;
     std::string leader;
+    std::string DroneName;
 } DroneStatus;
diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h
index a24bfbf..e3b92ef 100755
--- a/class_model/include/class_model/follower.h
+++ b/class_model/include/class_model/follower.h
@@ -26,7 +26,10 @@ public:
     DroneStatus drone3;
 
     void follower();
+
+    float lon[3],lat[3];
     
+
 private:
     // ROS NodeHandle
     ros::NodeHandle node_handle_;
@@ -39,6 +42,7 @@ private:
     void plane(global_location target_location,global_location leader_location,global_location follower_location);
     void choose_leader();
     void follow();
+    void get_location();
 
 };
 
diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp
index ea772c4..6a38a01 100755
--- a/class_model/src/follower.cpp
+++ b/class_model/src/follower.cpp
@@ -22,13 +22,28 @@ void FollowerClass::plane(global_location target_location,global_location leader
     c = target_location.lat - slope*target_location.lon;
     y = slope * follower_location.lon + c;
 
-    // if()
+    if(follower_location.lat >= y){
+        drone1.plane = 1;        //right plane
+    }else if(follower_location.lat < y){
+        drone1.plane = 0;        //left plane
+    }
     
-    //choose_leader()
+    choose_leader();
 }
 
 void FollowerClass::choose_leader(){
 
+
+
+    
+
+}
+
+void FollowerClass::get_location(){
+
+    // lon[0]
+
+
 }
 
 void FollowerClass::calculate_position(float k,float theta){
diff --git a/class_model/src/utils/mqttConfig_PUB.yml b/class_model/src/utils/mqttConfig_PUB.yml
index 566e029..45f9d60 100644
--- a/class_model/src/utils/mqttConfig_PUB.yml
+++ b/class_model/src/utils/mqttConfig_PUB.yml
@@ -1,5 +1,5 @@
 MQTT:
-  msg_format: Proto
+  msg_format: Json
   MQTTClientNamePub: Drone550Pub
   host: 140.120.31.133
   port: 1883
diff --git a/class_model/src/utils/mqttConfig_SUB.yml b/class_model/src/utils/mqttConfig_SUB.yml
index 313539a..a19bf8b 100644
--- a/class_model/src/utils/mqttConfig_SUB.yml
+++ b/class_model/src/utils/mqttConfig_SUB.yml
@@ -1,5 +1,5 @@
 MQTT:
-  msg_format: Proto
+  msg_format: Json
   MQTTClientNameSub: Drone550Sub
   host: 140.120.31.133
   port: 1883