diff --git a/class_model/include/class_model/convert_degree.h b/class_model/include/class_model/convert_degree.h index 271cb94..a4d8ef1 100755 --- a/class_model/include/class_model/convert_degree.h +++ b/class_model/include/class_model/convert_degree.h @@ -5,7 +5,7 @@ float ConvertDeg(float x ,float y){ degree = 90-(atan(y/x)*180/3.14); } if(x<0 && y>0){ - degree = atan(y/x)*180/3.14 + 360; + degree = abs(atan(y/x)*180/3.14) + 270; } if(x<0 && y<0){ degree = (atan(x/y)*180/3.14) + 180; diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index dc32323..ffb2af1 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -1,7 +1,9 @@ /*Follow the leader in a fixed formation */ #ifndef FOLLOWER_H #define FOLLOWER_H -#define swarmNum 5 +#define swarmNum 3 +#define changeLeader +// #define NoChangeLeader #include #include "class_model/sensor.h" #include "class_model/mode.h" @@ -38,6 +40,7 @@ public: void follower(global_location** member_data, size_t index, int refID); int find_ref_drone(global_location** data, size_t index, int leaderID); + void face2target(global_location* target, global_location* leader); float lon[3],lat[3]; global_location vector_LT,vector_SM,pre_LT,vector_LR; @@ -66,6 +69,8 @@ private: float slope,c,y; // function plane()'s variable global_location pre_target; // function plane()'s variable int flag = 0,pre_plane = 0; + float error_heading, error_yaw ; //function face2target()'s variable + int heading_status = 0; //function face2target()'s variable 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 4e78f22..8b4b244 100644 --- a/class_model/src/Leader.cpp +++ b/class_model/src/Leader.cpp @@ -16,8 +16,6 @@ void LeaderClass::leader(global_location target){ if(pre_target.lon != target.lon || pre_target.lat != target.lat){ heading_status = 0; ROS_INFO("heading_flag"); - std::cout <<"x: "<< target.lon << std::endl; - std::cout <<"y: "<< target.lat << std::endl; } // heading_status = 0; @@ -65,7 +63,9 @@ void LeaderClass::go2target(float target_lon,float target_lat){ float x,y; x = (target_lon/100) - (current_location.lon*1e5); y = (target_lat/100) - (current_location.lat*1e5); - target_heading = ConvertDeg(x,y); + target_heading = ConvertDeg(x,y); + + // ROS_INFO("x:%f,y:%f,target_heading:%f",x,y,target_heading); // float error_yaw = 0.2; // error_heading = target_heading - heading; diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index fe6afa1..5e59da9 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -3,6 +3,9 @@ #include "class_model/follower.h" #include "yaml-cpp/yaml.h" +#define changeLeader +// #define NoChangeLeader + int main(int argc, char **argv) { // Init ROS node @@ -42,8 +45,10 @@ int main(int argc, char **argv) { M4 = request_object.get_M4_GPS(); - global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; - global_location* member[] = {&M1,&M2,&M3,&M4}; + // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; + // global_location* member[] = {&M1,&M2,&M3,&M4}; + global_location* pos[] = {&target,&self,&M1,&M2}; + global_location* member[] = {&M1,&M2}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -66,11 +71,13 @@ 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); - } + } + + #ifdef changeLeader while(ros::ok()){ - // type = receiver_object.check_command(); + type = receiver_object.check_command(); target.lon = config[route[routeIndex]]["x"].as()*1e7 ; target.lat = config[route[routeIndex]]["y"].as()*1e7 ; @@ -78,6 +85,8 @@ int main(int argc, char **argv) { checkLeader = check_leader(*pos,index).is_leader; leaderID = check_leader(*pos,index).leader_ID; leaderIndex = check_leader(*pos,index).leader_index; + + // follower_object.face2target(pos[0], pos[leaderIndex]); // std::cout << leaderID << std::endl; // std::cout << target.lon << std::endl; @@ -95,21 +104,9 @@ int main(int argc, char **argv) { 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); + std::cout << "Drone1 is Leader" << std::endl; + leader_object.leader(target); }else{ ref_ID = follower_object.find_ref_drone(pos,index,leaderID); @@ -122,22 +119,68 @@ int main(int argc, char **argv) { // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; } - - if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ - routeIndex++; - std::cout <<"routeIndex: "<= routeNum){ - mode_object.set_Mode("LAND"); - std::cout <<"LAND"<= routeNum){ + // // mode_object.set_Mode("LAND"); + // // std::cout <<"LAND"<()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 + 1000; + leader_object.leader(target); + + target.lon = config["route1"]["x"].as()*1e7 - 4000; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + leader_object.leader(target); + + + target.lon = config["route1"]["x"].as()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 - 3000; + leader_object.leader(target); + + + target.lon = config["route1"]["x"].as()*1e7 ; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + 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 + } + + + } + #endif + ros::waitForShutdown(); return 0; -} \ No newline at end of file +} + diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp index 123d84a..60085bb 100644 --- a/class_model/src/bio_main2.cpp +++ b/class_model/src/bio_main2.cpp @@ -3,6 +3,9 @@ #include "class_model/follower.h" #include "yaml-cpp/yaml.h" +#define changeLeader +// #define NoChangeLeader + int main(int argc, char **argv) { // Init ROS node @@ -21,7 +24,7 @@ int main(int argc, char **argv) { ReceiverClass receiver_object; - global_location target,self,M1,M2,M3,M4; + global_location target,self,M1,M2,M3,M4,leader; int ref_ID,checkLeader,leaderID,leaderIndex,flag; int routeNum,routeIndex,OnTarget; float errorX,errorY; @@ -42,8 +45,10 @@ int main(int argc, char **argv) { M4 = request_object.get_M4_GPS(); - global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; - global_location* member[] = {&M1,&M2,&M3,&M4}; + // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; + // global_location* member[] = {&M1,&M2,&M3,&M4}; + global_location* pos[] = {&target,&self,&M1,&M2}; + global_location* member[] = {&M1,&M2}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -62,15 +67,17 @@ 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); - } + } + + #ifdef changeLeader while(ros::ok()){ - // type = receiver_object.check_command(); + type = receiver_object.check_command(); target.lon = config[route[routeIndex]]["x"].as()*1e7 ; target.lat = config[route[routeIndex]]["y"].as()*1e7 ; @@ -78,6 +85,8 @@ int main(int argc, char **argv) { checkLeader = check_leader(*pos,index).is_leader; leaderID = check_leader(*pos,index).leader_ID; leaderIndex = check_leader(*pos,index).leader_index; + + // follower_object.face2target(pos[0], pos[leaderIndex]); // std::cout << leaderID << std::endl; // std::cout << target.lon << std::endl; @@ -95,21 +104,9 @@ int main(int argc, char **argv) { 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); + std::cout << "Drone2 is Leader" << std::endl; + leader_object.leader(target); }else{ ref_ID = follower_object.find_ref_drone(pos,index,leaderID); @@ -122,23 +119,67 @@ int main(int argc, char **argv) { // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; } - - if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ - routeIndex++; + sleep(3); + // while(type != "n"){ + // type = receiver_object.check_command(); + if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + routeIndex++; + checkLeader = 0; + std::cout <<"Drone2routeIndex: "<= routeNum){ - mode_object.set_Mode("LAND"); - std::cout <<"LAND"<= routeNum){ + // // mode_object.set_Mode("LAND"); + // // std::cout <<"LAND"<()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 + 1000; + leader_object.leader(target); + + target.lon = config["route1"]["x"].as()*1e7 - 4000; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + leader_object.leader(target); - // mode_object.set_Mode("LAND"); + + target.lon = config["route1"]["x"].as()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 - 3000; + leader_object.leader(target); + + + target.lon = config["route1"]["x"].as()*1e7 ; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + 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 + } + } + #endif + ros::waitForShutdown(); return 0; -} \ No newline at end of file +} + diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp index 3c73937..fcf0970 100644 --- a/class_model/src/bio_main3.cpp +++ b/class_model/src/bio_main3.cpp @@ -3,6 +3,9 @@ #include "class_model/follower.h" #include "yaml-cpp/yaml.h" +#define changeLeader +// #define NoChangeLeader + int main(int argc, char **argv) { // Init ROS node @@ -21,7 +24,7 @@ int main(int argc, char **argv) { ReceiverClass receiver_object; - global_location target,self,M1,M2,M3,M4; + global_location target,self,M1,M2,M3,M4,leader; int ref_ID,checkLeader,leaderID,leaderIndex,flag; int routeNum,routeIndex,OnTarget; float errorX,errorY; @@ -42,8 +45,10 @@ int main(int argc, char **argv) { M4 = request_object.get_M4_GPS(); - global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; - global_location* member[] = {&M1,&M2,&M3,&M4}; + // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; + // global_location* member[] = {&M1,&M2,&M3,&M4}; + global_location* pos[] = {&target,&self,&M1,&M2}; + global_location* member[] = {&M1,&M2}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -62,15 +67,17 @@ 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); - } + } + + #ifdef changeLeader while(ros::ok()){ - // type = receiver_object.check_command(); + type = receiver_object.check_command(); target.lon = config[route[routeIndex]]["x"].as()*1e7 ; target.lat = config[route[routeIndex]]["y"].as()*1e7 ; @@ -78,6 +85,8 @@ int main(int argc, char **argv) { checkLeader = check_leader(*pos,index).is_leader; leaderID = check_leader(*pos,index).leader_ID; leaderIndex = check_leader(*pos,index).leader_index; + + // follower_object.face2target(pos[0], pos[leaderIndex]); // std::cout << leaderID << std::endl; // std::cout << target.lon << std::endl; @@ -95,21 +104,9 @@ int main(int argc, char **argv) { 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); + std::cout << "Drone3 is Leader" << std::endl; + leader_object.leader(target); }else{ ref_ID = follower_object.find_ref_drone(pos,index,leaderID); @@ -122,23 +119,67 @@ int main(int argc, char **argv) { // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; } - - if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ - routeIndex++; + sleep(3); + // while(type != "n"){ + // type = receiver_object.check_command(); + if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + routeIndex++; + checkLeader = 0; + std::cout <<"Drone3routeIndex: "<= routeNum){ - mode_object.set_Mode("LAND"); - std::cout <<"LAND"<= routeNum){ + // // mode_object.set_Mode("LAND"); + // // std::cout <<"LAND"<()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 + 1000; + leader_object.leader(target); + + target.lon = config["route1"]["x"].as()*1e7 - 4000; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + leader_object.leader(target); - // mode_object.set_Mode("LAND"); + + target.lon = config["route1"]["x"].as()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 - 3000; + leader_object.leader(target); + + + target.lon = config["route1"]["x"].as()*1e7 ; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + 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 + } + } + #endif + ros::waitForShutdown(); return 0; -} \ No newline at end of file +} + diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp index 5e07cd9..c5fb6b3 100644 --- a/class_model/src/bio_main4.cpp +++ b/class_model/src/bio_main4.cpp @@ -3,6 +3,9 @@ #include "class_model/follower.h" #include "yaml-cpp/yaml.h" +#define changeLeader +// #define NoChangeLeader + int main(int argc, char **argv) { // Init ROS node @@ -21,7 +24,7 @@ int main(int argc, char **argv) { ReceiverClass receiver_object; - global_location target,self,M1,M2,M3,M4; + global_location target,self,M1,M2,M3,M4,leader; int ref_ID,checkLeader,leaderID,leaderIndex,flag; int routeNum,routeIndex,OnTarget; float errorX,errorY; @@ -44,6 +47,8 @@ int main(int argc, char **argv) { global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; global_location* member[] = {&M1,&M2,&M3,&M4}; + // global_location* pos[] = {&target,&self,&M1,&M2}; + // global_location* member[] = {&M1,&M2}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -62,15 +67,17 @@ 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); - } + } + + #ifdef changeLeader while(ros::ok()){ - // type = receiver_object.check_command(); + type = receiver_object.check_command(); target.lon = config[route[routeIndex]]["x"].as()*1e7 ; target.lat = config[route[routeIndex]]["y"].as()*1e7 ; @@ -95,21 +102,9 @@ int main(int argc, char **argv) { 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); + std::cout << "Drone4 is Leader" << std::endl; + leader_object.leader(target); }else{ ref_ID = follower_object.find_ref_drone(pos,index,leaderID); @@ -123,22 +118,65 @@ int main(int argc, char **argv) { // std::cout <<"y: "<< errorY << std::endl; } - if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ - routeIndex++; + // while(type != "n"){ + // type = receiver_object.check_command(); + if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + routeIndex++; + checkLeader = 0; + std::cout <<"routeIndex: "<= routeNum){ - mode_object.set_Mode("LAND"); - std::cout <<"LAND"<= routeNum){ + // // mode_object.set_Mode("LAND"); + // // std::cout <<"LAND"<()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 + 1000; + leader_object.leader(target); + + target.lon = config["route1"]["x"].as()*1e7 - 4000; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + leader_object.leader(target); - // mode_object.set_Mode("LAND"); + + target.lon = config["route1"]["x"].as()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 - 3000; + leader_object.leader(target); + + + target.lon = config["route1"]["x"].as()*1e7 ; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + 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 + } + } + #endif + ros::waitForShutdown(); return 0; -} \ No newline at end of file +} + diff --git a/class_model/src/bio_main5.cpp b/class_model/src/bio_main5.cpp index 989072f..67b967e 100644 --- a/class_model/src/bio_main5.cpp +++ b/class_model/src/bio_main5.cpp @@ -3,6 +3,9 @@ #include "class_model/follower.h" #include "yaml-cpp/yaml.h" +#define changeLeader +// #define NoChangeLeader + int main(int argc, char **argv) { // Init ROS node @@ -21,7 +24,7 @@ int main(int argc, char **argv) { ReceiverClass receiver_object; - global_location target,self,M1,M2,M3,M4; + global_location target,self,M1,M2,M3,M4,leader; int ref_ID,checkLeader,leaderID,leaderIndex,flag; int routeNum,routeIndex,OnTarget; float errorX,errorY; @@ -44,6 +47,8 @@ int main(int argc, char **argv) { global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; global_location* member[] = {&M1,&M2,&M3,&M4}; + // global_location* pos[] = {&target,&self,&M1,&M2}; + // global_location* member[] = {&M1,&M2}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -66,11 +71,13 @@ 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); - } + } + + #ifdef changeLeader while(ros::ok()){ - // type = receiver_object.check_command(); + type = receiver_object.check_command(); target.lon = config[route[routeIndex]]["x"].as()*1e7 ; target.lat = config[route[routeIndex]]["y"].as()*1e7 ; @@ -95,21 +102,9 @@ int main(int argc, char **argv) { 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); + std::cout << "Drone5 is Leader" << std::endl; + leader_object.leader(target); }else{ ref_ID = follower_object.find_ref_drone(pos,index,leaderID); @@ -123,22 +118,66 @@ int main(int argc, char **argv) { // std::cout <<"y: "<< errorY << std::endl; } - if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ - routeIndex++; + // while(type != "n"){ + // type = receiver_object.check_command(); + if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + routeIndex++; + checkLeader = 0; + std::cout <<"routeIndex: "<= routeNum){ - mode_object.set_Mode("LAND"); - std::cout <<"LAND"<= routeNum){ + // // mode_object.set_Mode("LAND"); + // // std::cout <<"LAND"<()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 + 1000; + leader_object.leader(target); + + target.lon = config["route1"]["x"].as()*1e7 - 4000; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + leader_object.leader(target); + + + target.lon = config["route1"]["x"].as()*1e7 - 2000; + target.lat = config["route1"]["y"].as()*1e7 - 3000; + leader_object.leader(target); + + + target.lon = config["route1"]["x"].as()*1e7 ; + target.lat = config["route1"]["y"].as()*1e7 - 1000; + 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 + } + } + #endif + ros::waitForShutdown(); return 0; -} \ No newline at end of file +} + diff --git a/class_model/src/config.yaml b/class_model/src/config.yaml index e4dc906..df5c1f2 100644 --- a/class_model/src/config.yaml +++ b/class_model/src/config.yaml @@ -2,15 +2,15 @@ DroneParam: ID: 1 route1: x: 120.6743161 #(24.1218859, 120.6743161) - y: 24.1219860 + y: 24.1220860 z: 0.0 route2: - x: 120.6744161 #(24.1218859, 120.6743161) - y: 24.1219860 + x: 120.6746461 #(24.1218859, 120.6743161) + y: 24.1218860 z: 0.0 route3: - x: 120.6744161 #(24.1218859, 120.6743161) - y: 24.1218859 + x: 120.6743161 #(24.1218859, 120.6743161) + y: 24.1217360 z: 0.0 route4: x: 120.6743161 #(24.1218859, 120.6743161) diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index 92580aa..dbdb2b7 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -62,22 +62,28 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob ref_point.lon = 5000*sin(phi*3.14/180) + leader->lon + 1; ref_point.lat = 5000*cos(phi*3.14/180) + leader->lat; - + #ifdef NoChangeLeader + slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon); // Leader-Ref + c = leader->lat - slope*leader->lon; + #endif + + #ifdef changeLeader + if((target->lat != pre_target.lat) || (target->lon != pre_target.lon)){ // Leader-Target + face2target(target,leader); + // slope = (target->lat-leader->lat)/(target->lon-leader->lon+1); + // c = leader->lat - slope*leader->lon; + pre_target.lat = target->lat; + pre_target.lon = target->lon; + ROS_INFO("plane"); + } 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; - // } + #endif y = slope * follower->lon + c; - 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" <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; - 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; + #ifdef NoChangeLeader + /**************Leader-Ref*****************/ 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| + + k = (vector_LR.lon*vector_SM.lon)+(vector_LR.lat*vector_SM.lat); //k = vector_LR dot vector_SM + m = sqrt(pow(vector_LR.lon,2)+pow(vector_LR.lat,2)); //m = |vector_LR| + n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2)); //n = |vector_SM| + /*******************************************/ + #endif + #ifdef changeLeader + /**********Leader-Target***********/ + vector_LT.lon = target->lon - leader->lon; //vector_LT (leader->target) + vector_LT.lat = target->lat - leader->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| 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: "<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: "<lat; - // pre_tar.lon = target->lon; + }else{ + pre_LT = vector_LT; + pre_tar.lat = target->lat; + pre_tar.lon = target->lon; - // } + } + /***************************************/ + #endif cosTheta = k / (m*n); // std::cout <<"drone: "<ID){ - self = drone4; - }else{ - memberDrone[j] = drone4; - j+=1; - // std::cout << "drone4" <ID){ - self = drone5; - }else{ - memberDrone[j] = drone5; - j+=1; - // std::cout << "drone4" <ID){ + // self = drone4; + // }else{ + // memberDrone[j] = drone4; + // j+=1; + // // std::cout << "drone4" <ID){ + // self = drone5; + // }else{ + // memberDrone[j] = drone5; + // j+=1; + // // std::cout << "drone4" <lon/100) - (leader->lon/100); + y = (target->lat/100) - (leader->lat/100); + target_heading = ConvertDeg(x,y); + + error_heading = target_heading - heading; + error_yaw = check_direction(error_heading)*error_yaw*0.5; + + if(error_heading < -355 || error_heading > 355){ + error_heading = 0; + } + + // ROS_INFO("x:%f,y:%f,target_heading:%f,error_heading:%f",x,y,target_heading,error_heading); + + while(heading_status != 1){ + heading = GPS_object.get_heading(); + error_heading = target_heading - heading; + if (error_heading < 3 && error_heading > -3 ){ + error_yaw = 0; + heading_status = 1; + } + command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); + // ROS_INFO("error_yaw:%f",error_yaw); + } + +} \ No newline at end of file diff --git a/class_model/src/receiver.cpp b/class_model/src/receiver.cpp index a802a44..691f50a 100755 --- a/class_model/src/receiver.cpp +++ b/class_model/src/receiver.cpp @@ -8,7 +8,7 @@ ReceiverClass::ReceiverClass() : node_handle_("~"){ }else{ node_handle_.getParam("namespace", ros_namespace); } - mqtt_sub = node_handle_.subscribe(ros_namespace+"/cmd_receiver",10,&ReceiverClass::cmd_receiver, this); + mqtt_sub = node_handle_.subscribe("/cmd_receiver",10,&ReceiverClass::cmd_receiver, this); } ReceiverClass::~ReceiverClass() { ros::shutdown(); } diff --git a/iq_sim/worlds/three_drone.world b/iq_sim/worlds/three_drone.world index 9b30757..23f162c 100644 --- a/iq_sim/worlds/three_drone.world +++ b/iq_sim/worlds/three_drone.world @@ -89,14 +89,14 @@ - 0 -5 0 0 0 0 + -1 4 0 0 0 0 model://drone2 - 0 5 0 0 0 0 + -1 -4 0 0 0 0 model://drone3