From 272060ec4e06e62daab27bab380254d5645d8609 Mon Sep 17 00:00:00 2001
From: Xuan0319 <new310049@gmail.com>
Date: Tue, 14 Feb 2023 23:01:39 +0800
Subject: [PATCH] add FindLeader function

---
 class_model/include/class_model/DroneStatus.h |  2 +-
 class_model/include/class_model/FindLeader.h  | 30 +++++++
 class_model/src/bio_main.cpp                  | 52 +++++++++++
 class_model/src/config.yaml                   |  8 +-
 class_model/src/test_lib.cpp                  | 88 +++++++++++++++++--
 5 files changed, 172 insertions(+), 8 deletions(-)
 create mode 100644 class_model/include/class_model/FindLeader.h
 create mode 100644 class_model/src/bio_main.cpp

diff --git a/class_model/include/class_model/DroneStatus.h b/class_model/include/class_model/DroneStatus.h
index f36be80..274f089 100644
--- a/class_model/include/class_model/DroneStatus.h
+++ b/class_model/include/class_model/DroneStatus.h
@@ -4,7 +4,7 @@ typedef struct DroneStatus
 {
     /* data */
     global_location init_location;
-    int plane;
+    int plane,ID;
     std::string leader;
     std::string DroneName;
 } DroneStatus;
diff --git a/class_model/include/class_model/FindLeader.h b/class_model/include/class_model/FindLeader.h
new file mode 100644
index 0000000..79f0a27
--- /dev/null
+++ b/class_model/include/class_model/FindLeader.h
@@ -0,0 +1,30 @@
+#include "class_model/gps_location.h"
+#include "class_model/DroneStatus.h"
+#include <iterator> 
+
+bool is_leader(global_location data[], size_t index){
+    
+    float d[index];
+    int check_ID,leader_ID;
+
+    for(int i=1 ; i<index ; i++){
+
+        d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
+
+        // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
+    }
+
+    std::vector<float> _d(d+1,d+index-1); 
+    
+    check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
+    leader_ID = check_ID + 1;
+    
+    std::cout <<"Drone " << leader_ID << " is leader " << std::endl;
+    
+    if (data[1].ID == leader_ID){
+        return 1;
+    }else{
+        return 0;
+    }
+    
+}
\ No newline at end of file
diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp
new file mode 100644
index 0000000..579c52c
--- /dev/null
+++ b/class_model/src/bio_main.cpp
@@ -0,0 +1,52 @@
+#include "class_model/mission.h"
+#include "class_model/choose_leader.h"
+#include "class_model/PubInfo.h"
+#include "class_model/FindLeader.h"
+#include "yaml-cpp/yaml.h"
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "drone1_node");
+
+    // reate Assync spiner
+    ros::AsyncSpinner spinner(0);
+	spinner.start();
+    // ros::Rate rate(5);   
+
+    ModeClass mode_object;
+    ControlClass control_object;
+    ReceiverClass receiver_object;
+    RequestClass request_object;
+    MissionClass mission_object;
+    CommandClass command_object;
+    SelectClass select_formation;
+    PubInfoClass PubInfo_object;
+    
+    // SelectionClass selection_object;
+
+    global_location target,self,M1,M2;
+    global_location pos[] = {target,self,M1,M2};
+    size_t index = sizeof(pos)/sizeof(pos[0]);
+    
+    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>();
+    target.lat = config["routh1"]["y"].as<float>();
+
+    self = request_object.get_self_GPS();
+    M1 = request_object.get_M1_GPS();
+    M2 = request_object.get_M2_GPS();
+
+    if(is_leader(pos,index) == 1){
+        //fly2target
+    }else{
+        //find reference drone
+    }
+
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(2);
+
+
+}
\ No newline at end of file
diff --git a/class_model/src/config.yaml b/class_model/src/config.yaml
index 222a336..4f54336 100644
--- a/class_model/src/config.yaml
+++ b/class_model/src/config.yaml
@@ -1,4 +1,10 @@
+DroneParam:
+  ID: 1 
 routh1:
   x: 1.0
   y: 1.5
-  z: 0.0
\ No newline at end of file
+  z: 0.0
+formation:
+  distance: 4.0
+  angel: 45
+  
diff --git a/class_model/src/test_lib.cpp b/class_model/src/test_lib.cpp
index 9750ab6..3ccfa0a 100644
--- a/class_model/src/test_lib.cpp
+++ b/class_model/src/test_lib.cpp
@@ -1,9 +1,42 @@
 #include <iostream>
 #include "yaml-cpp/yaml.h"
+#include "class_model/gps_location.h"
 #include <algorithm>
+#include <math.h>
 
 using namespace std;
 
+bool is_leader(global_location data[], size_t index){
+    
+    // int index = sizeof(data)/sizeof(data[0]);
+    float d[index];
+    int check_ID,leader_ID;
+
+    for(int i=1 ; i<index ; i++){
+
+        d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
+
+        // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
+    }
+
+    std::vector<float> _d(d+1,d+index-1); 
+    
+    check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
+    leader_ID = check_ID + 1;
+    
+    std::cout <<"Drone " << leader_ID << " is leader " << std::endl;
+    
+    if (data[1].ID == leader_ID){
+        return 1;
+    }else{
+        return 0;
+    }
+    
+}
+
+
+
+
 int main(int argc,char** argv){
 
     // YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
@@ -11,13 +44,56 @@ int main(int argc,char** argv){
     // cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
     // cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
 
-    int ID[3] = {1,2,3};
-    int b = 1;
-    std::array <int,3> a {1,2,3};
-    remove(a.begin(),a.end(),b);
-    std::cout << a[0]<<a[1] <<std::endl;
+    // int ID[3] = {1,2,3};
+    // int b = 1;
+    // std::array <int,3> a {1,2,3};
+    // remove(a.begin(),a.end(),b);
+    // std::cout << a[0]<<a[1] <<std::endl;
+
+    global_location self;
+    global_location M1;
+    global_location M2;
+    global_location target;
+    
+    target.lon = 0;
+    target.lat = 0;
+    target.heading = 0;
+    
+    self.lat = 1;
+    self.lon = 1;
+    self.ID = 1;
+
+    M1.lat = 2;
+    M1.lon = 2;
+    M1.ID = 2;
 
+    M2.lat = 6;
+    M2.lon = 6;
+    M2.ID = 3;
+
+    global_location data[]={target,self,M1,M2};
+
+
+    std::cout << is_leader(data,sizeof(data)/sizeof(data[0])) <<std::endl;
+
+    // int index = sizeof(data)/sizeof(target); //4
+    // float d[index];
+    // int check_ID,leader_ID;
+
+    // for(int i=1 ; i<index ; i++){
+
+    //     d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
+
+    //     std::cout << i<<"," <<d[data[i].ID] <<std::endl;
+    // }
+
+    // std::vector<float> _d(d+1,d+index-1); 
+    
+    // check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));
+    // leader_ID = check_ID + 1;
+    // std::cout <<"Drone " << leader_ID << " is leader " << std::endl;
 
 
     return 0;
-}
\ No newline at end of file
+}
+