From 2efa5165de7b53d63a25a030e72fd1ac80f2b9f7 Mon Sep 17 00:00:00 2001
From: Xuan0319 <new310049@gmail.com>
Date: Tue, 18 Apr 2023 23:21:56 +0800
Subject: [PATCH 1/2] bio drone debug complete

---
 class_model/include/class_model/Leader.h   |   1 +
 class_model/include/class_model/command.h  |   4 +-
 class_model/include/class_model/follower.h |   6 +-
 class_model/src/Leader.cpp                 |  11 +-
 class_model/src/bio_main.cpp               |   7 +-
 class_model/src/bio_main2.cpp              |   6 +-
 class_model/src/bio_main3.cpp              |   7 +-
 class_model/src/bio_main4.cpp              |   5 +-
 class_model/src/bio_main5.cpp              |   5 +-
 class_model/src/command.cpp                |  37 +++++--
 class_model/src/config.yaml                |   4 +
 class_model/src/follower.cpp               | 115 +++++++++++++++------
 class_model/src/formation.cpp              |   7 +-
 class_model/src/main.cpp                   |   1 +
 class_model/src/test_lib.cpp               |   6 +-
 iq_sim/worlds/five_drone.world             |   8 +-
 16 files changed, 169 insertions(+), 61 deletions(-)

diff --git a/class_model/include/class_model/Leader.h b/class_model/include/class_model/Leader.h
index c929d25..b87dd5a 100644
--- a/class_model/include/class_model/Leader.h
+++ b/class_model/include/class_model/Leader.h
@@ -35,6 +35,7 @@ private:
     ros::NodeHandle node_handle_;
 
     global_location target_location;
+    global_location pre_target;
     global_location follower_location;
     global_location leader_location;
     global_location current_location;
diff --git a/class_model/include/class_model/command.h b/class_model/include/class_model/command.h
index c5a670a..d5f54b9 100755
--- a/class_model/include/class_model/command.h
+++ b/class_model/include/class_model/command.h
@@ -6,18 +6,20 @@
 #include <geometry_msgs/TwistStamped.h>
 #include <mavros_msgs/GlobalPositionTarget.h>
 #include <class_model/sensor.h>
+#include <geographic_msgs/GeoPoseStamped.h>
 
 class CommandClass {
 public:
     CommandClass();
     virtual ~CommandClass();
-    void set_global_position(float lat,float lon,float alt);
+    void set_global_position(double lon,double lat,float alt,float yaw);
     void set_velocity(float x,float y,float alt,float yaw,float second);
     void fix_velocity(float x,float y,float alt,float yaw,float second);
     void velocity_init();
     void set_target_position(float x,float y);
     ThreadGPSClass gps_object;
 
+
 private:
     // ROS NodeHandle
     ros::NodeHandle node_handle_;
diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h
index a8ef125..300aaf8 100755
--- a/class_model/include/class_model/follower.h
+++ b/class_model/include/class_model/follower.h
@@ -40,7 +40,7 @@ public:
     int find_ref_drone(global_location** data, size_t index, int leaderID);
 
     float lon[3],lat[3];
-    global_location vector_LT,vector_SM;
+    global_location vector_LT,vector_SM,pre_LT,vector_LR;
     
 
 private:
@@ -61,6 +61,10 @@ private:
     float error_lon,error_lat;
     float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
     float ignore_small = 0.50;
+    float m,n,k,cosTheta,pre_m,pre_k,u,LTcos;  // function direct()'s variable
+    global_location pre_tar,ref_point;  // function plane()'s variable
+    float slope,c,y;              // function plane()'s variable
+    global_location pre_target;  // function plane()'s variable
     int flag = 0;
 
     void calculate_position(float k,float theta,int plane);
diff --git a/class_model/src/Leader.cpp b/class_model/src/Leader.cpp
index cdbd9a8..874d9e8 100644
--- a/class_model/src/Leader.cpp
+++ b/class_model/src/Leader.cpp
@@ -13,12 +13,17 @@ void LeaderClass::leader(global_location target){
     std_msgs::String message;
 
     flag = 0;
-    // heading_status = 0;
+    if(pre_target.lon != target.lon || pre_target.lat != target.lat){
+        heading_status = 0;
+    }
+
     while(true){
         if(flag==0){
             go2target(target.lon,target.lat);
         }else{
             // next_command.publish(message);
+            pre_target.lon = target.lon;
+            pre_target.lat = target.lat;
             ROS_INFO("break");
             break;
         }
@@ -111,9 +116,9 @@ void LeaderClass::face2target(float target_heading){
 
     float error_heading;
     float heading = GPS_object.get_heading();
-    float error_yaw = 0.2;
+    float error_yaw = 0.1;
     error_heading =  target_heading - heading;
-    error_yaw = check_direction(error_heading)*error_yaw; 
+    error_yaw = check_direction(error_heading)*error_yaw*0.5; 
 
     if (error_heading < 5 && error_heading > -5 ){
         error_yaw = 0;
diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp
index 6f0da70..8cd3f61 100644
--- a/class_model/src/bio_main.cpp
+++ b/class_model/src/bio_main.cpp
@@ -22,7 +22,7 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,flag;
+    int ref_ID,checkLeader,leaderID,flag;
     std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
@@ -58,6 +58,7 @@ int main(int argc, char **argv) {
     // }
 
     checkLeader = check_leader(*pos,index).is_leader;
+    leaderID = check_leader(*pos,index).leader_ID;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
@@ -73,8 +74,10 @@ int main(int argc, char **argv) {
 
         if(checkLeader == 1){
             leader_object.leader(target);
+            target.lon = config["routh2"]["x"].as<float>()*1e7;
+            target.lat = config["routh2"]["y"].as<float>()*1e7;
         }else{
-            ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
+            ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
             follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
 
diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp
index a26776e..033ca42 100644
--- a/class_model/src/bio_main2.cpp
+++ b/class_model/src/bio_main2.cpp
@@ -22,7 +22,7 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,flag;
+    int ref_ID,checkLeader,leaderID,flag;
     std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
@@ -58,6 +58,8 @@ int main(int argc, char **argv) {
     // }
 
     checkLeader = check_leader(*pos,index).is_leader;
+    leaderID = check_leader(*pos,index).leader_ID;
+
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
@@ -74,7 +76,7 @@ int main(int argc, char **argv) {
         if(checkLeader == 1){
             leader_object.leader(target);
         }else{
-            ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
+            ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
             follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
 
diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp
index db129d6..5fc7873 100644
--- a/class_model/src/bio_main3.cpp
+++ b/class_model/src/bio_main3.cpp
@@ -22,7 +22,7 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,flag;
+    int ref_ID,checkLeader,leaderID,flag;
     std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
@@ -58,6 +58,7 @@ int main(int argc, char **argv) {
     // }
 
     checkLeader = check_leader(*pos,index).is_leader;
+    leaderID = check_leader(*pos,index).leader_ID;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
@@ -72,9 +73,9 @@ int main(int argc, char **argv) {
         M4 = request_object.get_M4_GPS();
 
         if(checkLeader == 1){
-            // leader_object.l/eader(target);
+            leader_object.leader(target);
         }else{
-            ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
+            ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
             follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
 
diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp
index e023af1..83f9d9c 100644
--- a/class_model/src/bio_main4.cpp
+++ b/class_model/src/bio_main4.cpp
@@ -22,7 +22,7 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,flag;
+    int ref_ID,checkLeader,leaderID,flag;
     std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
@@ -58,6 +58,7 @@ int main(int argc, char **argv) {
     // }
 
     checkLeader = check_leader(*pos,index).is_leader;
+    leaderID = check_leader(*pos,index).leader_ID;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
@@ -74,7 +75,7 @@ int main(int argc, char **argv) {
         if(checkLeader == 1){
             leader_object.leader(target);
         }else{
-            ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
+            ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
             follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
 
diff --git a/class_model/src/bio_main5.cpp b/class_model/src/bio_main5.cpp
index 9125931..2f5d599 100644
--- a/class_model/src/bio_main5.cpp
+++ b/class_model/src/bio_main5.cpp
@@ -22,7 +22,7 @@ int main(int argc, char **argv) {
     
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader,flag;
+    int ref_ID,checkLeader,leaderID,flag;
     std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
@@ -58,6 +58,7 @@ int main(int argc, char **argv) {
     // }
 
     checkLeader = check_leader(*pos,index).is_leader;
+    leaderID = check_leader(*pos,index).leader_ID;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
@@ -74,7 +75,7 @@ int main(int argc, char **argv) {
         if(checkLeader == 1){
             leader_object.leader(target);
         }else{
-            ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
+            ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
             follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
 
diff --git a/class_model/src/command.cpp b/class_model/src/command.cpp
index c8dd142..18cdc2c 100644
--- a/class_model/src/command.cpp
+++ b/class_model/src/command.cpp
@@ -10,7 +10,7 @@ CommandClass::CommandClass() : node_handle_("~"){
 	}
 
     velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",10);
-    gps_command=node_handle_.advertise<mavros_msgs::GlobalPositionTarget>(ros_namespace+"/mavros/setpoint_raw/global",10);
+    gps_command=node_handle_.advertise<geographic_msgs::GeoPoseStamped>(ros_namespace+"/mavros/setpoint_position/global",10);
 }
 
 CommandClass::~CommandClass() { ros::shutdown(); }
@@ -96,15 +96,34 @@ void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second
 
 }
 
-void CommandClass::set_global_position(float lat,float lon,float alt){
+void CommandClass::set_global_position(double lon,double lat,float alt,float yaw){
     
-    mavros_msgs::GlobalPositionTarget goal_position;
-	goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_TERRAIN_ALT;
-	goal_position.type_mask = goal_position.IGNORE_VX | goal_position.IGNORE_VY | goal_position.IGNORE_VZ | goal_position.IGNORE_AFX | goal_position.IGNORE_AFY | goal_position.IGNORE_AFZ | goal_position.IGNORE_YAW | goal_position.IGNORE_YAW_RATE;
-    goal_position.latitude = lat;
-    goal_position.longitude = lon;
-    goal_position.altitude = alt;
-	gps_command.publish(goal_position);
+    // mavros_msgs::GlobalPositionTarget goal_position;
+	// goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_REL_ALT;
+	// goal_position.type_mask = goal_position.IGNORE_VZ | goal_position.IGNORE_YAW_RATE;
+    // goal_position.latitude = lat;
+    // goal_position.longitude = lon;
+    // goal_position.altitude = alt;
+	// goal_position.yaw = yaw;
+	// goal_position.header.frame_id = "map";
+	// // goal_position.type_mask = 65535;
+
+
+	geographic_msgs::GeoPoseStamped goal_position;
+    goal_position.header.frame_id = "map";
+    goal_position.pose.position.latitude = lat;
+    goal_position.pose.position.longitude = lon;
+    goal_position.pose.position.altitude = 67;
+
+	ROS_INFO("%f,%f",lat,lon);
+	double begin = ros::Time::now().toSec();
+	while(true){
+		gps_command.publish(goal_position);
+		double now = ros::Time::now().toSec();
+		if((now-begin)>0.5){
+			break;
+	}
+	}
 
 }
 
diff --git a/class_model/src/config.yaml b/class_model/src/config.yaml
index a8d4e77..a21ee8c 100644
--- a/class_model/src/config.yaml
+++ b/class_model/src/config.yaml
@@ -4,6 +4,10 @@ routh1:
   x: 120.6743161             #(24.1218859, 120.6743161)
   y: 24.1219310
   z: 0.0
+routh2:
+  x: 120.6743661             #(24.1218859, 120.6743161)
+  y: 24.1219310
+  z: 0.0
 formation:
   distance: 4.0
   angle: 45
diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp
index e718ce3..a37e6d5 100755
--- a/class_model/src/follower.cpp
+++ b/class_model/src/follower.cpp
@@ -45,15 +45,38 @@ void FollowerClass::follower(global_location** member_data, size_t index, int re
 
 void FollowerClass::plane(global_location* target, global_location* leader, global_location* follower){
     
-    //caculate drone is locate on R/L plane
-    float slope,c,y;
-    slope = (target->lat-leader->lat)/(target->lon-leader->lon);
+    //calculate drone is locate on R/L plane
+
+    float phi = leader->heading/100 ; 
+    phi = phi*3.14/180;   
+
+    // float ref_y = leader->lat + 5000;
+    // float ref_x = leader->lon + 1;
+    // ref_point.lon = (cos(-phi)*leader->lon + sin(-phi)*ref_y);
+    // ref_point.lat = (-sin(-phi)*leader->lon + cos(-phi)*ref_y);
+
+    ref_point.lon = 5000*sin(phi) + leader->lon;
+    ref_point.lat = 5000*cos(phi) + leader->lat;
+
+  
+    slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon);
     c = leader->lat - slope*leader->lon;
+    
+
+    // if((target->lat != pre_target.lat) || (target->lon != pre_target.lon)){
+    //     slope = (target->lat-leader->lat)/(target->lon-leader->lon);
+    //     c = leader->lat - slope*leader->lon;
+    // }
+
     y = slope * follower->lon + c;
+
+    pre_target.lat = target->lat;
+    pre_target.lon = target->lon;
     // std::cout<< follower.ID << ","<< follower.lat <<","<< y <<std::endl;
+    std::cout<< "slope: " << slope <<std::endl;
 
-    if(follower->lat <= y){ 
-        // std::cout<< follower.lat <<","<< y <<std::endl;              
+    if((follower->lat <= y && slope>0) || (follower->lat > y && slope<0)){ 
+        std::cout <<"drone :"<<follower->ID <<", plane: 1" <<std::endl;              
         switch (follower->ID){
             case 1:
                 drone1.init_location.lat = follower->lat;
@@ -86,8 +109,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
                 drone5.plane = 1;        
                 break;  
         }
-    }else if(follower->lat > y){
-        // std::cout<< follower.lat <<","<< y <<std::endl;   
+    }else if((follower->lat > y && slope>0) || (follower->lat <= y && slope<0)){
+        std::cout <<"drone :"<<follower->ID <<", plane: -1" <<std::endl;    
         switch (follower->ID){
             case 1:
                 drone1.init_location.lat = follower->lat;
@@ -126,22 +149,56 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
 }
 
 int FollowerClass::direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member){
+    
+    //Calculate whether the vector (SM) of self and member N is the same as the flight direction (LT)
 
-    float m,n,k,cosTheta;
+    float phi = leader->heading/100 ; 
+    phi = phi*3.14/180;   
+
+
+    // float ref_y = leader->lat + 5000;
+    // float ref_x = leader->lon + 1;
+    ref_point.lon = 5000*sin(phi) + leader->lon;
+    ref_point.lat = 5000*cos(phi) + leader->lat;
+
+    // vector_LT.lon = target->lon - leader->lon;       //vector_LT (leader->target)     
+    // vector_LT.lat = target->lat - leader->lat;
 
-    if(flag == 0){
-    vector_LT.lon = target->lon - leader->lon;       //vector_LT (leader->target)     
-    vector_LT.lat = target->lat - leader->lat;
-    flag = 1;
-    }
     vector_SM.lon = member.init_location.lon - self.init_location.lon;       //vector_SM (self->member)
     vector_SM.lat = member.init_location.lat - self.init_location.lat;
-    k = (vector_LT.lon*vector_SM.lon)+(vector_LT.lat*vector_SM.lat);     //k = vector_LT dot vector_SM
-    m = sqrt(pow(vector_LT.lon,2)+pow(vector_LT.lat,2));     //m = |vector_LT|
+
+    vector_LR.lon = ref_point.lon - leader->lon;       //vector_LR (leader->Ref)     
+    vector_LR.lat = ref_point.lat - leader->lat;
+        
+    k = (vector_LR.lon*vector_SM.lon)+(vector_LR.lat*vector_SM.lat);     //k = vector_LT dot vector_SM
+    m = sqrt(pow(vector_LR.lon,2)+pow(vector_LR.lat,2));     //m = |vector_LT|
     n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2));     //n = |vector_SM|
+    
+    // pre_m = sqrt(pow(pre_LT.lon,2)+pow(pre_LT.lat,2));     
+    // pre_k = (pre_LT.lon*vector_SM.lon)+(pre_LT.lat*vector_SM.lat);
+    // if(pre_m != 0 && ((target->lat == pre_tar.lat) && (target->lon == pre_tar.lon))){
+
+    //     u = (vector_LT.lon*pre_LT.lon)+(vector_LT.lat*pre_LT.lat);
+    //     LTcos = u / (m*pre_m);
+    //     if (LTcos < 0){
+    //         m = pre_m;
+    //         k = pre_k;
+    //         std::cout <<"memberID: "<<member.ID << " fix" << std::endl;
+    //     }
+    //     std::cout <<"dir_test "<< std::endl;
+
+    // }else{
+    //     pre_LT = vector_LT;
+    //     pre_tar.lat = target->lat;
+    //     pre_tar.lon = target->lon;
+
+    // }
+    
     cosTheta = k / (m*n);
+    std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl;
 
-    if(cosTheta > 0){
+    // std::cout <<"drone "<< self.ID <<"cosTheta: "<< cosTheta << std::endl;
+    if(cosTheta >= 0){
         return 1;
     }else{
         return 10000;
@@ -239,7 +296,7 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
         if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){    //load same plane member
             samePlane[s_index] = memberDrone[i];
             // std::cout << samePlane[s_index].init_location.lat <<std::endl;
-            // std::cout << memberDrone[i].ID <<std::endl;
+            // std::cout <<"drone"<<self.ID<<", memberdrone"<< memberDrone[i].ID<<", plane: "<< memberDrone[i].plane <<std::endl;
             s_index++;
         }
 
@@ -256,7 +313,7 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
         d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
 
         // std::cout << i<<"," <<d[i] <<std::endl;
-        // std::cout << samePlane[i].ID <<std::endl;
+        std::cout <<"drone" <<samePlane[i].ID<<" ,d = "<<d[i] <<std::endl;
     }
 
     std::vector<float> _d(d,d+s_index); 
@@ -264,8 +321,8 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
     check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
     ref_ID = samePlane[check_index].ID;
 
-    std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " <<  ref_ID << std::endl;
-
+    std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " <<  ref_ID << ",mem_index: "<< s_index << std::endl;
+    std::cout <<"\n"<< std::endl;
     return ref_ID;   
 
 }
@@ -306,7 +363,7 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
 
     
     float error_degree = deg_phi - heading;
-    float error_yaw = 0.2;   //CCW
+    float error_yaw = 0.4;   //CCW
 
     if (error_degree >= 360){
         error_degree -= 360;
@@ -329,17 +386,17 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
         error_yaw = 0;
     }
 
-    if (error_lon < -1){                                      
-        error_lon = -1;
+    if (error_lon < -2){                                      
+        error_lon = -2;
     }
-    if (error_lon > 1){                                      
-        error_lon = 1;
+    if (error_lon > 2){                                      
+        error_lon = 2;
     }
-    if (error_lat < -1){
-        error_lat = -1;
+    if (error_lat < -2){
+        error_lat = -2;
     }    
-    if (error_lat > 1){
-        error_lat = 1;
+    if (error_lat > 2){
+        error_lat = 2;
     }
 
     // ROS_INFO("%d,%f,%f",refpos.ID,refpos.lon ,refpos.lat );
diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp
index 5083e68..d6d747d 100644
--- a/class_model/src/formation.cpp
+++ b/class_model/src/formation.cpp
@@ -552,7 +552,9 @@ void FormationClass::calculate_position(float k,float theta,int direction){
     // // ROS_INFO("error_yaw:%f",error_yaw);
     // ROS_INFO("************************************");
 
-    command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3);
+    // command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3);
+    command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
+    // command_object.set_velocity(0,0,0,error_yaw,0.1);
 }
 
 void FormationClass::spherical_coordinate(float k,float theta,float psi){
@@ -685,7 +687,8 @@ void FormationClass::go2target(float x,float y){
     }
 
     PubData_object.publishData(drone_speed);
-    command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
+    // command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
+    command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,heading);
     
     // ROS_INFO("slope:%f,%f,%f",slope,x,y);
     ROS_INFO("%f,%f",drone_speed.x,drone_speed.y);
diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp
index 4f2d484..9aa1773 100755
--- a/class_model/src/main.cpp
+++ b/class_model/src/main.cpp
@@ -62,6 +62,7 @@ int main(int argc, char **argv) {
     if(type == "init"){
         select_formation.goose_formation();
         ROS_INFO("init formation");
+        pre_type ="init";
     }else if(type == "line"){
         select_formation.line_formation();
         pre_type ="line";
diff --git a/class_model/src/test_lib.cpp b/class_model/src/test_lib.cpp
index 976153b..0292f42 100644
--- a/class_model/src/test_lib.cpp
+++ b/class_model/src/test_lib.cpp
@@ -264,10 +264,14 @@ int main(int argc,char** argv){
     M2.lat = 1;
     M2.ID = 3;
 
-    std::cout<< data[3].lon<<std::endl;
+    // std::cout<< data[3].lon<<std::endl;
 
     // find_ref_drone(data,index,check_leader(data,index).leader_ID);
 
+    // float theta = -45;
+    // theta = theta *3.14/180;
+    // std::cout<< 3*sin(theta)<<","<< 3*cos(theta) <<std::endl;
+
 
 
 
diff --git a/iq_sim/worlds/five_drone.world b/iq_sim/worlds/five_drone.world
index 1921470..8666ff5 100644
--- a/iq_sim/worlds/five_drone.world
+++ b/iq_sim/worlds/five_drone.world
@@ -124,25 +124,25 @@
       </include>
     </model>
     <model name="drone2">
-      <pose> 0 4 0 0 0 0</pose>
+      <pose> -1 4 0 0 0 0</pose>
       <include>
         <uri>model://drone2</uri>
       </include>
     </model>
     <model name="drone3">
-      <pose> 0 8 0 0 0 0</pose>
+      <pose> -2 8 0 0 0 0</pose>
       <include>
         <uri>model://drone3</uri>
       </include>
     </model>
         <model name="drone4">
-      <pose> 0 -4 0 0 0 0</pose>
+      <pose> -1 -4 0 0 0 0</pose>
       <include>
         <uri>model://drone4</uri>
       </include>
     </model>
         <model name="drone5">
-      <pose> 0 -8 0 0 0 0</pose>
+      <pose> -2 -8 0 0 0 0</pose>
       <include>
         <uri>model://drone5</uri>
       </include>

From 8831cb7e6ee0488854e5139ebf8a1a002ed76f03 Mon Sep 17 00:00:00 2001
From: Xuan0319 <new310049@gmail.com>
Date: Tue, 25 Apr 2023 14:46:02 +0800
Subject: [PATCH 2/2] test route finish

---
 class_model/include/class_model/command.h  |  4 +--
 class_model/include/class_model/follower.h |  2 +-
 class_model/src/Leader.cpp                 | 11 ++++---
 class_model/src/bio_main.cpp               | 18 +++++++++--
 class_model/src/command.cpp                | 37 ++++++++++++++--------
 class_model/src/follower.cpp               | 30 ++++++++++++------
 class_model/src/formation.cpp              |  4 +--
 iq_sim/worlds/three_drone.world            |  4 +--
 8 files changed, 72 insertions(+), 38 deletions(-)

diff --git a/class_model/include/class_model/command.h b/class_model/include/class_model/command.h
index d5f54b9..e67f08a 100755
--- a/class_model/include/class_model/command.h
+++ b/class_model/include/class_model/command.h
@@ -6,7 +6,7 @@
 #include <geometry_msgs/TwistStamped.h>
 #include <mavros_msgs/GlobalPositionTarget.h>
 #include <class_model/sensor.h>
-#include <geographic_msgs/GeoPoseStamped.h>
+// #include <geographic_msgs/GeoPoseStamped.h>
 
 class CommandClass {
 public:
@@ -25,7 +25,7 @@ private:
     ros::NodeHandle node_handle_;
 
     geometry_msgs::TwistStamped msg;
-    mavros_msgs::GlobalPositionTarget goal_position;
+    // mavros_msgs::GlobalPositionTarget goal_position;
     global_location current_location;
     global_location target_location;
     global_location origin_location;
diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h
index 300aaf8..539a948 100755
--- a/class_model/include/class_model/follower.h
+++ b/class_model/include/class_model/follower.h
@@ -65,7 +65,7 @@ private:
     global_location pre_tar,ref_point;  // function plane()'s variable
     float slope,c,y;              // function plane()'s variable
     global_location pre_target;  // function plane()'s variable
-    int flag = 0;
+    int flag = 0,pre_plane = 0;
 
     void calculate_position(float k,float theta,int plane);
     void plane(global_location* target, global_location* leader, global_location* follower);
diff --git a/class_model/src/Leader.cpp b/class_model/src/Leader.cpp
index 874d9e8..57d9dfc 100644
--- a/class_model/src/Leader.cpp
+++ b/class_model/src/Leader.cpp
@@ -15,8 +15,9 @@ void LeaderClass::leader(global_location target){
     flag = 0;
     if(pre_target.lon != target.lon || pre_target.lat != target.lat){
         heading_status = 0;
+        ROS_INFO("heading_flag");
     }
-
+    heading_status = 0;
     while(true){
         if(flag==0){
             go2target(target.lon,target.lat);
@@ -103,10 +104,10 @@ void LeaderClass::go2target(float target_lon,float target_lat){
     
     // ROS_INFO("slope:%f,%f,%f",slope,x,y);
     // ROS_INFO("%f,%f",drone_speed.x,drone_speed.y);
-    // ROS_INFO("heading:%f,target_heading:%f",heading,target_heading);
+    ROS_INFO("heading:%f,target_heading:%f",heading,target_heading);
     // ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat);
     // ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5);
-    // ROS_INFO("************************************");
+    ROS_INFO("************************************");
     
 
 
@@ -116,11 +117,11 @@ void LeaderClass::face2target(float target_heading){
 
     float error_heading;
     float heading = GPS_object.get_heading();
-    float error_yaw = 0.1;
+    float error_yaw = 0.2;
     error_heading =  target_heading - heading;
     error_yaw = check_direction(error_heading)*error_yaw*0.5; 
 
-    if (error_heading < 5 && error_heading > -5 ){
+    if (error_heading < 3 && error_heading > -3 ){
         error_yaw = 0;
         heading_status = 1;
     }
diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp
index 8cd3f61..132b9dd 100644
--- a/class_model/src/bio_main.cpp
+++ b/class_model/src/bio_main.cpp
@@ -74,8 +74,22 @@ int main(int argc, char **argv) {
 
         if(checkLeader == 1){
             leader_object.leader(target);
-            target.lon = config["routh2"]["x"].as<float>()*1e7;
-            target.lat = config["routh2"]["y"].as<float>()*1e7;
+            // for(int i=0;i<=360;i++){
+            //     target.lon = config["routh1"]["x"].as<float>()*1e7 + 300*sin(i*0.2);
+            //     target.lat = config["routh1"]["y"].as<float>()*1e7 + 300*cos(i*0.2);
+            //     leader_object.leader(target);
+            //     std::cout<< "radin: " << i*3.14/180 <<std::endl;
+            // }
+            target.lon = config["routh1"]["x"].as<float>()*1e7 + 500;
+            target.lat = config["routh1"]["y"].as<float>()*1e7 ;
+            leader_object.leader(target);
+            target.lon = config["routh1"]["x"].as<float>()*1e7 ;
+            target.lat = config["routh1"]["y"].as<float>()*1e7 -500;
+            leader_object.leader(target);
+            target.lon = config["routh1"]["x"].as<float>()*1e7 -500;
+            target.lat = config["routh1"]["y"].as<float>()*1e7 ; 
+            leader_object.leader(target);
+
         }else{
             ref_ID = follower_object.find_ref_drone(pos,index,leaderID);
             follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
diff --git a/class_model/src/command.cpp b/class_model/src/command.cpp
index 18cdc2c..0e8b9cb 100644
--- a/class_model/src/command.cpp
+++ b/class_model/src/command.cpp
@@ -10,7 +10,9 @@ CommandClass::CommandClass() : node_handle_("~"){
 	}
 
     velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",10);
-    gps_command=node_handle_.advertise<geographic_msgs::GeoPoseStamped>(ros_namespace+"/mavros/setpoint_position/global",10);
+    // gps_command=node_handle_.advertise<geographic_msgs::GeoPoseStamped>(ros_namespace+"/mavros/setpoint_position/global",10);
+    gps_command=node_handle_.advertise<mavros_msgs::GlobalPositionTarget>(ros_namespace+"/mavros/setpoint_raw/global",10);
+
 }
 
 CommandClass::~CommandClass() { ros::shutdown(); }
@@ -98,22 +100,29 @@ void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second
 
 void CommandClass::set_global_position(double lon,double lat,float alt,float yaw){
     
-    // mavros_msgs::GlobalPositionTarget goal_position;
-	// goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_REL_ALT;
-	// goal_position.type_mask = goal_position.IGNORE_VZ | goal_position.IGNORE_YAW_RATE;
-    // goal_position.latitude = lat;
-    // goal_position.longitude = lon;
-    // goal_position.altitude = alt;
-	// goal_position.yaw = yaw;
+    mavros_msgs::GlobalPositionTarget goal_position;
+	goal_position.coordinate_frame = mavros_msgs::GlobalPositionTarget::FRAME_GLOBAL_REL_ALT;
+	goal_position.type_mask =  mavros_msgs::GlobalPositionTarget::IGNORE_VX |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_VY |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_VZ |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_AFX |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_AFY |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_AFZ |
+                              mavros_msgs::GlobalPositionTarget::FORCE |
+                              mavros_msgs::GlobalPositionTarget::IGNORE_YAW_RATE;
+    goal_position.latitude = lat;
+    goal_position.longitude = lon;
+    goal_position.altitude = alt;
+	goal_position.yaw = yaw;
 	// goal_position.header.frame_id = "map";
-	// // goal_position.type_mask = 65535;
+	// goal_position.type_mask = 65535;
 
 
-	geographic_msgs::GeoPoseStamped goal_position;
-    goal_position.header.frame_id = "map";
-    goal_position.pose.position.latitude = lat;
-    goal_position.pose.position.longitude = lon;
-    goal_position.pose.position.altitude = 67;
+	// geographic_msgs::GeoPoseStamped goal_position;
+    // goal_position.header.frame_id = "map";
+    // goal_position.pose.position.latitude = lat;
+    // goal_position.pose.position.longitude = lon;
+    // goal_position.pose.position.altitude = 67;
 
 	ROS_INFO("%f,%f",lat,lon);
 	double begin = ros::Time::now().toSec();
diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp
index a37e6d5..bd300c2 100755
--- a/class_model/src/follower.cpp
+++ b/class_model/src/follower.cpp
@@ -48,15 +48,19 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
     //calculate drone is locate on R/L plane
 
     float phi = leader->heading/100 ; 
-    phi = phi*3.14/180;   
+    // phi = phi*3.14/180; 
+
+    if (phi > 355 && phi < 5){
+        phi = 0;
+    }  
 
     // float ref_y = leader->lat + 5000;
     // float ref_x = leader->lon + 1;
     // ref_point.lon = (cos(-phi)*leader->lon + sin(-phi)*ref_y);
     // ref_point.lat = (-sin(-phi)*leader->lon + cos(-phi)*ref_y);
 
-    ref_point.lon = 5000*sin(phi) + leader->lon;
-    ref_point.lat = 5000*cos(phi) + leader->lat;
+    ref_point.lon = 5000*sin(phi*3.14/180) + leader->lon + 1;
+    ref_point.lat = 5000*cos(phi*3.14/180) + leader->lat;
 
   
     slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon);
@@ -75,8 +79,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
     // std::cout<< follower.ID << ","<< follower.lat <<","<< y <<std::endl;
     std::cout<< "slope: " << slope <<std::endl;
 
-    if((follower->lat <= y && slope>0) || (follower->lat > y && slope<0)){ 
-        std::cout <<"drone :"<<follower->ID <<", plane: 1" <<std::endl;              
+    if((follower->lat <= y && phi < 180) || (follower->lat > y && phi >= 180)){ 
+        // std::cout <<"drone :"<<follower->ID <<", plane: 1" <<std::endl;              
         switch (follower->ID){
             case 1:
                 drone1.init_location.lat = follower->lat;
@@ -109,8 +113,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
                 drone5.plane = 1;        
                 break;  
         }
-    }else if((follower->lat > y && slope>0) || (follower->lat <= y && slope<0)){
-        std::cout <<"drone :"<<follower->ID <<", plane: -1" <<std::endl;    
+    }else if((follower->lat > y && phi < 180) || (follower->lat <= y && phi >= 180)){
+        // std::cout <<"drone :"<<follower->ID <<", plane: -1" <<std::endl;    
         switch (follower->ID){
             case 1:
                 drone1.init_location.lat = follower->lat;
@@ -195,7 +199,7 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
     // }
     
     cosTheta = k / (m*n);
-    std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl;
+    // std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl;
 
     // std::cout <<"drone "<< self.ID <<"cosTheta: "<< cosTheta << std::endl;
     if(cosTheta >= 0){
@@ -313,7 +317,7 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
         d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
 
         // std::cout << i<<"," <<d[i] <<std::endl;
-        std::cout <<"drone" <<samePlane[i].ID<<" ,d = "<<d[i] <<std::endl;
+        // std::cout <<"drone" <<samePlane[i].ID<<" ,d = "<<d[i] <<std::endl;
     }
 
     std::vector<float> _d(d,d+s_index); 
@@ -338,8 +342,14 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
     float heading = GPS_object.get_heading();
     
     float phi = deg_phi*3.14/180;             //degree-->radian
-    theta = plane*theta*3.14/180;
 
+    // if(deg_phi >= 180 && deg_phi < 350){
+    //     plane = plane*-1;
+    // }else if((deg_phi >= 350 || deg_phi < 5) && pre_plane != 0){
+    //     plane = pre_plane;
+    // }
+    theta = plane*theta*3.14/180;
+    pre_plane = plane;
     double Pf[]={},Pl[]={refpos.lon ,refpos.lat};       //transfer maxtrix  
     float transfer[2][2]={
                             cos(phi),-sin(phi),
diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp
index d6d747d..9157b54 100644
--- a/class_model/src/formation.cpp
+++ b/class_model/src/formation.cpp
@@ -687,8 +687,8 @@ void FormationClass::go2target(float x,float y){
     }
 
     PubData_object.publishData(drone_speed);
-    // command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
-    command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,heading);
+    command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
+    // command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,target_heading);
     
     // ROS_INFO("slope:%f,%f,%f",slope,x,y);
     ROS_INFO("%f,%f",drone_speed.x,drone_speed.y);
diff --git a/iq_sim/worlds/three_drone.world b/iq_sim/worlds/three_drone.world
index d00b822..9b30757 100644
--- a/iq_sim/worlds/three_drone.world
+++ b/iq_sim/worlds/three_drone.world
@@ -89,14 +89,14 @@
       </include>
     </model>
     <model name="drone2">
-      <pose> 0 5 0 0 0 0</pose>
+      <pose> 0 -5 0 0 0 0</pose>
       <include>
         <uri>model://drone2</uri>
       </include>
     </model>
 
     <model name="drone3">
-      <pose> 0 -5 0 0 0 0</pose>
+      <pose> 0 5 0 0 0 0</pose>
       <include>
         <uri>model://drone3</uri>
       </include>