From b59265bc808c65bae5df5270cd9b1ac78033136d Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Fri, 2 Jun 2023 02:34:04 +0800 Subject: [PATCH] change leader when target change --- class_model/CMakeLists.txt | 2 +- class_model/include/class_model/FindLeader.h | 8 +- class_model/include/class_model/Leader.h | 2 + class_model/include/class_model/command.h | 2 +- class_model/include/class_model/follower.h | 6 +- class_model/include/class_model/formation.h | 2 +- class_model/src/Leader.cpp | 14 ++- class_model/src/bio_main.cpp | 99 +++++++++++++------- class_model/src/bio_main2.cpp | 85 +++++++++++++---- class_model/src/bio_main3.cpp | 84 +++++++++++++---- class_model/src/bio_main4.cpp | 84 +++++++++++++---- class_model/src/bio_main5.cpp | 82 ++++++++++++---- class_model/src/command.cpp | 2 +- class_model/src/config.yaml | 21 ++++- class_model/src/follower.cpp | 9 +- class_model/src/formation.cpp | 2 +- 16 files changed, 381 insertions(+), 123 deletions(-) diff --git a/class_model/CMakeLists.txt b/class_model/CMakeLists.txt index 84e2a33..de6d03c 100755 --- a/class_model/CMakeLists.txt +++ b/class_model/CMakeLists.txt @@ -294,7 +294,7 @@ add_dependencies(follower_lib main_generate_messages_cpp) ################## add_executable(main src/main.cpp) -target_link_libraries(main ${catkin_LIBRARIES} mission_lib PubInfo_lib) +target_link_libraries(main ${catkin_LIBRARIES} mission_lib PubInfo_lib ) add_dependencies(main class_model_generate_messages_cpp) add_executable(bio_main src/bio_main.cpp) diff --git a/class_model/include/class_model/FindLeader.h b/class_model/include/class_model/FindLeader.h index 4e1239c..131776d 100644 --- a/class_model/include/class_model/FindLeader.h +++ b/class_model/include/class_model/FindLeader.h @@ -3,7 +3,7 @@ #include typedef struct result{ - int leader_ID; + int leader_ID,leader_index; bool is_leader; }result; @@ -24,6 +24,12 @@ result check_leader(global_location* data, size_t index){ check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index _result.leader_ID = check_ID + 1; + + for(int j=1 ; j #include #include diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index 539a948..7672022 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -1,7 +1,7 @@ /*Follow the leader in a fixed formation */ #ifndef FOLLOWER_H #define FOLLOWER_H - +#define swarmNum 5 #include #include "class_model/sensor.h" #include "class_model/mode.h" @@ -34,7 +34,7 @@ public: DroneStatus drone4; DroneStatus drone5; DroneStatus self; - DroneStatus samePlane[5]; + DroneStatus samePlane[swarmNum]; void follower(global_location** member_data, size_t index, int refID); int find_ref_drone(global_location** data, size_t index, int leaderID); @@ -52,7 +52,7 @@ private: global_location leader_location; global_location leader_drone; global_location refpos; - DroneStatus memberDrone[5]; + DroneStatus memberDrone[swarmNum]; global_location (RequestClass::*ref)(); YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); diff --git a/class_model/include/class_model/formation.h b/class_model/include/class_model/formation.h index ebfebac..f0b388f 100755 --- a/class_model/include/class_model/formation.h +++ b/class_model/include/class_model/formation.h @@ -1,7 +1,7 @@ /*Follow the leader in a fixed formation */ #ifndef FORMATION_H #define FORMATION_H -#define DEBUG +// #define DEBUG #include #include "class_model/sensor.h" #include "class_model/mode.h" diff --git a/class_model/src/Leader.cpp b/class_model/src/Leader.cpp index 39b590b..a4b2cd3 100644 --- a/class_model/src/Leader.cpp +++ b/class_model/src/Leader.cpp @@ -20,11 +20,13 @@ void LeaderClass::leader(global_location target){ heading_status = 0; while(true){ if(flag==0){ + OnTarget = 0; go2target(target.lon,target.lat); }else{ // next_command.publish(message); pre_target.lon = target.lon; pre_target.lat = target.lat; + OnTarget = 1; ROS_INFO("break"); break; } @@ -100,14 +102,14 @@ void LeaderClass::go2target(float target_lon,float target_lat){ } // 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); // 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("************************************"); @@ -125,6 +127,10 @@ void LeaderClass::face2target(float target_heading){ error_yaw = 0; heading_status = 1; } - // command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); + command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); // ROS_INFO("face2target...%f",target_heading); +} + +int LeaderClass::isOnTarget(){ + return OnTarget; } \ No newline at end of file diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index 5787590..ce3d7f2 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -21,13 +21,18 @@ int main(int argc, char **argv) { ReceiverClass receiver_object; - global_location target,self,M1,M2,M3,M4; - int ref_ID,checkLeader,leaderID,flag; + global_location target,self,M1,M2,M3,M4,leader; + int ref_ID,checkLeader,leaderID,leaderIndex,flag; + int routeNum,routeIndex,OnTarget; + float errorX,errorY; 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) - target.lon = config["routh1"]["x"].as()*1e7; - target.lat = config["routh1"]["y"].as()*1e7; + target.lon = config["route1"]["x"].as()*1e7; + target.lat = config["route1"]["y"].as()*1e7; + routeNum = config["routeNum"]["num"].as(); + + std::string route[4]={"route1","route2","route3","route4"}; sleep(5); self = request_object.get_self_GPS(); @@ -61,44 +66,72 @@ int main(int argc, char **argv) { 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); - } + } while(ros::ok()){ // type = receiver_object.check_command(); - self = request_object.get_self_GPS(); - M1 = request_object.get_M1_GPS(); - M2 = request_object.get_M2_GPS(); - M3 = request_object.get_M3_GPS(); - M4 = request_object.get_M4_GPS(); - - if(checkLeader == 1){ - target.lon = config["routh1"]["x"].as()*1e7 ; - target.lat = config["routh1"]["y"].as()*1e7 + 500; - leader_object.leader(target); - - target.lon = config["routh1"]["x"].as()*1e7 + 1000; - target.lat = config["routh1"]["y"].as()*1e7 + 500; - leader_object.leader(target); - - target.lon = config["routh1"]["x"].as()*1e7 + 1000; - target.lat = config["routh1"]["y"].as()*1e7 - 500; - leader_object.leader(target); - - target.lon = config["routh1"]["x"].as()*1e7 ; - target.lat = config["routh1"]["y"].as()*1e7 - 500; - 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 + + target.lon = config[route[routeIndex]]["x"].as()*1e7 ; + target.lat = config[route[routeIndex]]["y"].as()*1e7 ; + + checkLeader = check_leader(*pos,index).is_leader; + leaderID = check_leader(*pos,index).leader_ID; + leaderIndex = check_leader(*pos,index).leader_index; + + std::cout << leaderID << std::endl; + std::cout << target.lon << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; + + while(errorX > 100 || errorY > 100){ + + self = request_object.get_self_GPS(); + M1 = request_object.get_M1_GPS(); + M2 = request_object.get_M2_GPS(); + M3 = request_object.get_M3_GPS(); + M4 = request_object.get_M4_GPS(); + + if(checkLeader == 1){ + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // 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 + } + // OnTarget = leader_object.isOnTarget(); + // std::cout <<"tar: "<< OnTarget << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; } + if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ + routeIndex++; + // OnTarget = 0; + }else if(routeIndex >= routeNum){ + mode_object.set_Mode("LAND"); + } // if(type == "land"){ // mode_object.set_Mode("LAND"); // } - - } ros::waitForShutdown(); diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp index 637a8b0..13c15c6 100644 --- a/class_model/src/bio_main2.cpp +++ b/class_model/src/bio_main2.cpp @@ -22,12 +22,17 @@ int main(int argc, char **argv) { global_location target,self,M1,M2,M3,M4; - int ref_ID,checkLeader,leaderID,flag; + int ref_ID,checkLeader,leaderID,leaderIndex,flag; + int routeNum,routeIndex,OnTarget; + float errorX,errorY; 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) - target.lon = config["routh1"]["x"].as()*1e7; - target.lat = config["routh1"]["y"].as()*1e7; + target.lon = config["route1"]["x"].as()*1e7; + target.lat = config["route1"]["y"].as()*1e7; + routeNum = config["routeNum"]["num"].as(); + + std::string route[4]={"route1","route2","route3","route4"}; sleep(5); self = request_object.get_self_GPS(); @@ -57,34 +62,78 @@ int main(int argc, char **argv) { // } // } - checkLeader = check_leader(*pos,index).is_leader; + 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); - } + } while(ros::ok()){ // type = receiver_object.check_command(); - self = request_object.get_self_GPS(); - M1 = request_object.get_M1_GPS(); - M2 = request_object.get_M2_GPS(); - M3 = request_object.get_M3_GPS(); - M4 = request_object.get_M4_GPS(); - - if(checkLeader == 1){ - 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 + + target.lon = config[route[routeIndex]]["x"].as()*1e7 ; + target.lat = config[route[routeIndex]]["y"].as()*1e7 ; + + checkLeader = check_leader(*pos,index).is_leader; + leaderID = check_leader(*pos,index).leader_ID; + leaderIndex = check_leader(*pos,index).leader_index; + + std::cout << leaderID << std::endl; + std::cout << target.lon << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; + + while(errorX > 100 || errorY > 100){ + + self = request_object.get_self_GPS(); + M1 = request_object.get_M1_GPS(); + M2 = request_object.get_M2_GPS(); + M3 = request_object.get_M3_GPS(); + M4 = request_object.get_M4_GPS(); + + if(checkLeader == 1){ + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // 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 + } + // OnTarget = leader_object.isOnTarget(); + // std::cout <<"tar: "<< OnTarget << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; } + if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ + routeIndex++; + // OnTarget = 0; + }else if(routeIndex >= routeNum){ + mode_object.set_Mode("LAND"); + } // if(type == "land"){ // mode_object.set_Mode("LAND"); // } - + // mode_object.set_Mode("LAND"); } ros::waitForShutdown(); diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp index b290cfe..c20a5cc 100644 --- a/class_model/src/bio_main3.cpp +++ b/class_model/src/bio_main3.cpp @@ -22,12 +22,17 @@ int main(int argc, char **argv) { global_location target,self,M1,M2,M3,M4; - int ref_ID,checkLeader,leaderID,flag; + int ref_ID,checkLeader,leaderID,leaderIndex,flag; + int routeNum,routeIndex,OnTarget; + float errorX,errorY; 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) - target.lon = config["routh1"]["x"].as()*1e7; - target.lat = config["routh1"]["y"].as()*1e7; + target.lon = config["route1"]["x"].as()*1e7; + target.lat = config["route1"]["y"].as()*1e7; + routeNum = config["routeNum"]["num"].as(); + + std::string route[4]={"route1","route2","route3","route4"}; sleep(5); self = request_object.get_self_GPS(); @@ -57,33 +62,78 @@ int main(int argc, char **argv) { // } // } - checkLeader = check_leader(*pos,index).is_leader; + 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); - } + } while(ros::ok()){ // type = receiver_object.check_command(); - self = request_object.get_self_GPS(); - M1 = request_object.get_M1_GPS(); - M2 = request_object.get_M2_GPS(); - M3 = request_object.get_M3_GPS(); - M4 = request_object.get_M4_GPS(); - - if(checkLeader == 1){ - 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 + + target.lon = config[route[routeIndex]]["x"].as()*1e7 ; + target.lat = config[route[routeIndex]]["y"].as()*1e7 ; + + checkLeader = check_leader(*pos,index).is_leader; + leaderID = check_leader(*pos,index).leader_ID; + leaderIndex = check_leader(*pos,index).leader_index; + + std::cout << leaderID << std::endl; + std::cout << target.lon << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; + + while(errorX > 100 || errorY > 100){ + + self = request_object.get_self_GPS(); + M1 = request_object.get_M1_GPS(); + M2 = request_object.get_M2_GPS(); + M3 = request_object.get_M3_GPS(); + M4 = request_object.get_M4_GPS(); + + if(checkLeader == 1){ + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // 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 + } + // OnTarget = leader_object.isOnTarget(); + // std::cout <<"tar: "<< OnTarget << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; } + if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ + routeIndex++; + // OnTarget = 0; + }else if(routeIndex >= routeNum){ + mode_object.set_Mode("LAND"); + } // if(type == "land"){ // mode_object.set_Mode("LAND"); // } - + // mode_object.set_Mode("LAND"); } ros::waitForShutdown(); diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp index 2be79b0..139271b 100644 --- a/class_model/src/bio_main4.cpp +++ b/class_model/src/bio_main4.cpp @@ -22,12 +22,17 @@ int main(int argc, char **argv) { global_location target,self,M1,M2,M3,M4; - int ref_ID,checkLeader,leaderID,flag; + int ref_ID,checkLeader,leaderID,leaderIndex,flag; + int routeNum,routeIndex,OnTarget; + float errorX,errorY; 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) - target.lon = config["routh1"]["x"].as()*1e7; - target.lat = config["routh1"]["y"].as()*1e7; + target.lon = config["route1"]["x"].as()*1e7; + target.lat = config["route1"]["y"].as()*1e7; + routeNum = config["routeNum"]["num"].as(); + + std::string route[4]={"route1","route2","route3","route4"}; sleep(5); self = request_object.get_self_GPS(); @@ -57,33 +62,78 @@ int main(int argc, char **argv) { // } // } - checkLeader = check_leader(*pos,index).is_leader; + 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); - } + } while(ros::ok()){ // type = receiver_object.check_command(); - self = request_object.get_self_GPS(); - M1 = request_object.get_M1_GPS(); - M2 = request_object.get_M2_GPS(); - M3 = request_object.get_M3_GPS(); - M4 = request_object.get_M4_GPS(); - - if(checkLeader == 1){ - 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 + + target.lon = config[route[routeIndex]]["x"].as()*1e7 ; + target.lat = config[route[routeIndex]]["y"].as()*1e7 ; + + checkLeader = check_leader(*pos,index).is_leader; + leaderID = check_leader(*pos,index).leader_ID; + leaderIndex = check_leader(*pos,index).leader_index; + + std::cout << leaderID << std::endl; + std::cout << target.lon << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; + + while(errorX > 100 || errorY > 100){ + + self = request_object.get_self_GPS(); + M1 = request_object.get_M1_GPS(); + M2 = request_object.get_M2_GPS(); + M3 = request_object.get_M3_GPS(); + M4 = request_object.get_M4_GPS(); + + if(checkLeader == 1){ + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // 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 + } + // OnTarget = leader_object.isOnTarget(); + // std::cout <<"tar: "<< OnTarget << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; } + if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ + routeIndex++; + // OnTarget = 0; + }else if(routeIndex >= routeNum){ + mode_object.set_Mode("LAND"); + } // if(type == "land"){ // mode_object.set_Mode("LAND"); // } - + // mode_object.set_Mode("LAND"); } ros::waitForShutdown(); diff --git a/class_model/src/bio_main5.cpp b/class_model/src/bio_main5.cpp index 7bfc4c7..4f8cf82 100644 --- a/class_model/src/bio_main5.cpp +++ b/class_model/src/bio_main5.cpp @@ -22,12 +22,17 @@ int main(int argc, char **argv) { global_location target,self,M1,M2,M3,M4; - int ref_ID,checkLeader,leaderID,flag; + int ref_ID,checkLeader,leaderID,leaderIndex,flag; + int routeNum,routeIndex,OnTarget; + float errorX,errorY; 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) - target.lon = config["routh1"]["x"].as()*1e7; - target.lat = config["routh1"]["y"].as()*1e7; + target.lon = config["route1"]["x"].as()*1e7; + target.lat = config["route1"]["y"].as()*1e7; + routeNum = config["routeNum"]["num"].as(); + + std::string route[4]={"route1","route2","route3","route4"}; sleep(5); self = request_object.get_self_GPS(); @@ -61,29 +66,74 @@ int main(int argc, char **argv) { 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); - } + } while(ros::ok()){ // type = receiver_object.check_command(); - self = request_object.get_self_GPS(); - M1 = request_object.get_M1_GPS(); - M2 = request_object.get_M2_GPS(); - M3 = request_object.get_M3_GPS(); - M4 = request_object.get_M4_GPS(); - - if(checkLeader == 1){ - 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 + + target.lon = config[route[routeIndex]]["x"].as()*1e7 ; + target.lat = config[route[routeIndex]]["y"].as()*1e7 ; + + checkLeader = check_leader(*pos,index).is_leader; + leaderID = check_leader(*pos,index).leader_ID; + leaderIndex = check_leader(*pos,index).leader_index; + + std::cout << leaderID << std::endl; + std::cout << target.lon << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; + + while(errorX > 100 || errorY > 100){ + + self = request_object.get_self_GPS(); + M1 = request_object.get_M1_GPS(); + M2 = request_object.get_M2_GPS(); + M3 = request_object.get_M3_GPS(); + M4 = request_object.get_M4_GPS(); + + if(checkLeader == 1){ + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 + 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 + 1000; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // leader_object.leader(target); + + // target.lon = config["routh1"]["x"].as()*1e7 ; + // target.lat = config["routh1"]["y"].as()*1e7 - 500; + // 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 + } + // OnTarget = leader_object.isOnTarget(); + // std::cout <<"tar: "<< OnTarget << std::endl; + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + std::cout <<"x: "<< errorX << std::endl; + std::cout <<"y: "<< errorY << std::endl; } + if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ + routeIndex++; + // OnTarget = 0; + }else if(routeIndex >= routeNum){ + mode_object.set_Mode("LAND"); + } // if(type == "land"){ // mode_object.set_Mode("LAND"); // } - + // mode_object.set_Mode("LAND"); } ros::waitForShutdown(); diff --git a/class_model/src/command.cpp b/class_model/src/command.cpp index 0e048f8..7448433 100644 --- a/class_model/src/command.cpp +++ b/class_model/src/command.cpp @@ -109,7 +109,7 @@ void CommandClass::set_global_position(double lon,double lat,float alt,float yaw mavros_msgs::GlobalPositionTarget::IGNORE_AFY | mavros_msgs::GlobalPositionTarget::IGNORE_AFZ | mavros_msgs::GlobalPositionTarget::FORCE | - mavros_msgs::GlobalPositionTarget::IGNORE_YAW_RATE; + mavros_msgs::GlobalPositionTarget::IGNORE_YAW_RATE ; goal_position.latitude = lat; goal_position.longitude = lon; goal_position.altitude = alt; diff --git a/class_model/src/config.yaml b/class_model/src/config.yaml index a21ee8c..e4dc906 100644 --- a/class_model/src/config.yaml +++ b/class_model/src/config.yaml @@ -1,14 +1,25 @@ DroneParam: ID: 1 -routh1: +route1: x: 120.6743161 #(24.1218859, 120.6743161) - y: 24.1219310 + y: 24.1219860 z: 0.0 -routh2: - x: 120.6743661 #(24.1218859, 120.6743161) - y: 24.1219310 +route2: + x: 120.6744161 #(24.1218859, 120.6743161) + y: 24.1219860 + z: 0.0 +route3: + x: 120.6744161 #(24.1218859, 120.6743161) + y: 24.1218859 + z: 0.0 +route4: + x: 120.6743161 #(24.1218859, 120.6743161) + y: 24.1218859 z: 0.0 formation: distance: 4.0 angle: 45 +routeNum: + num: 4 + diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index 1817f2d..53694e8 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -77,7 +77,7 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob pre_target.lat = target->lat; pre_target.lon = target->lon; // std::cout<< follower.ID << ","<< follower.lat <<","<< y <lat <= y && phi < 180) || (follower->lat > y && phi >= 180)){ // std::cout <<"drone :"<ID <<", plane: 1" <