From c6197c5650b7c998adc4e2425534c0b2dff0b453 Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Fri, 24 Mar 2023 23:26:55 +0800 Subject: [PATCH] test bio theory loop caculate --- class_model/src/bio_main.cpp | 19 ++++++++++++++----- class_model/src/bio_main2.cpp | 19 ++++++++++++++----- class_model/src/bio_main3.cpp | 19 ++++++++++++++----- class_model/src/bio_main4.cpp | 19 ++++++++++++++----- class_model/src/bio_main5.cpp | 19 ++++++++++++++----- class_model/src/follower.cpp | 14 +++++++------- class_model/src/test_lib.cpp | 11 +++++++---- 7 files changed, 84 insertions(+), 36 deletions(-) diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index 4613a87..3f618c4 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -37,8 +37,8 @@ int main(int argc, char **argv) { // global_location pos[] = {target,self,M1,M2}; // global_location member[] = {M1,M2}; - 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}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -46,17 +46,26 @@ int main(int argc, char **argv) { control_object.arm(); control_object.takeoff(2); - checkLeader = check_leader(pos,index).is_leader; + checkLeader = check_leader(*pos,index).is_leader; 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); + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ + + // 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(); + // pos[] = {target,self,M1,M2,M3,M4}; + // member[] = {M1,M2,M3,M4}; if(checkLeader == 1){ // leader_object.leader(target); }else{ - follower_object.follower(member,m_index,ref_ID); //follow reference drone position + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + follower_object.follower(*member,m_index,ref_ID); //follow reference drone position } } ros::waitForShutdown(); diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp index e05ce83..17dedc9 100644 --- a/class_model/src/bio_main2.cpp +++ b/class_model/src/bio_main2.cpp @@ -37,8 +37,8 @@ int main(int argc, char **argv) { // global_location pos[] = {target,self,M1,M2}; // global_location member[] = {M1,M2}; - 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}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -46,17 +46,26 @@ int main(int argc, char **argv) { control_object.arm(); control_object.takeoff(2); - checkLeader = check_leader(pos,index).is_leader; + checkLeader = check_leader(*pos,index).is_leader; 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); + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ + + // 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(); + // pos[] = {target,self,M1,M2,M3,M4}; + // member[] = {M1,M2,M3,M4}; if(checkLeader == 1){ // leader_object.leader(target); }else{ - follower_object.follower(member,m_index,ref_ID); //follow reference drone position + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + follower_object.follower(*member,m_index,ref_ID); //follow reference drone position } } ros::waitForShutdown(); diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp index 5e35e2b..99d099c 100644 --- a/class_model/src/bio_main3.cpp +++ b/class_model/src/bio_main3.cpp @@ -37,8 +37,8 @@ int main(int argc, char **argv) { // global_location pos[] = {target,self,M1,M2}; // global_location member[] = {M1,M2}; - 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}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -46,17 +46,26 @@ int main(int argc, char **argv) { control_object.arm(); control_object.takeoff(2); - checkLeader = check_leader(pos,index).is_leader; + checkLeader = check_leader(*pos,index).is_leader; 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); + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ + + // 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(); + // pos[] = {target,self,M1,M2,M3,M4}; + // member[] = {M1,M2,M3,M4}; if(checkLeader == 1){ // leader_object.leader(target); }else{ - follower_object.follower(member,m_index,ref_ID); //follow reference drone position + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + follower_object.follower(*member,m_index,ref_ID); //follow reference drone position } } ros::waitForShutdown(); diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp index de85900..3789550 100644 --- a/class_model/src/bio_main4.cpp +++ b/class_model/src/bio_main4.cpp @@ -37,8 +37,8 @@ int main(int argc, char **argv) { // global_location pos[] = {target,self,M1,M2}; // global_location member[] = {M1,M2}; - 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}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -46,17 +46,26 @@ int main(int argc, char **argv) { control_object.arm(); control_object.takeoff(2); - checkLeader = check_leader(pos,index).is_leader; + checkLeader = check_leader(*pos,index).is_leader; 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); + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ + + // 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(); + // pos[] = {target,self,M1,M2,M3,M4}; + // member[] = {M1,M2,M3,M4}; if(checkLeader == 1){ // leader_object.leader(target); }else{ - follower_object.follower(member,m_index,ref_ID); //follow reference drone position + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + follower_object.follower(*member,m_index,ref_ID); //follow reference drone position } } ros::waitForShutdown(); diff --git a/class_model/src/bio_main5.cpp b/class_model/src/bio_main5.cpp index d1daa95..e8da062 100644 --- a/class_model/src/bio_main5.cpp +++ b/class_model/src/bio_main5.cpp @@ -37,8 +37,8 @@ int main(int argc, char **argv) { // global_location pos[] = {target,self,M1,M2}; // global_location member[] = {M1,M2}; - 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}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -46,17 +46,26 @@ int main(int argc, char **argv) { control_object.arm(); control_object.takeoff(2); - checkLeader = check_leader(pos,index).is_leader; + checkLeader = check_leader(*pos,index).is_leader; 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); + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ + + // 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(); + // pos[] = {target,self,M1,M2,M3,M4}; + // member[] = {M1,M2,M3,M4}; if(checkLeader == 1){ // leader_object.leader(target); }else{ - follower_object.follower(member,m_index,ref_ID); //follow reference drone position + ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + follower_object.follower(*member,m_index,ref_ID); //follow reference drone position } } ros::waitForShutdown(); diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index ef34269..378f804 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -30,17 +30,17 @@ void FollowerClass::follower(global_location* member_data, size_t index, int ref std::string command = ""; std::string pre_command = receiver_object.check_command(); - while(true){ + // while(true){ calculate_position(distance,angle,self.plane); - command = receiver_object.check_command(); + // command = receiver_object.check_command(); - if(command != pre_command ){ - ROS_INFO("change formation"); - break; - } + // if(command != pre_command ){ + // ROS_INFO("change formation"); + // break; + // } - } + //} } void FollowerClass::plane(global_location target, global_location leader, global_location follower){ diff --git a/class_model/src/test_lib.cpp b/class_model/src/test_lib.cpp index ca47d36..3d8c35b 100644 --- a/class_model/src/test_lib.cpp +++ b/class_model/src/test_lib.cpp @@ -241,13 +241,13 @@ int main(int argc,char** argv){ global_location M2; global_location target; - target.lon = 3; - target.lat = -3; + target.lon = 0; + target.lat = 3; target.heading = 0; self.lon = -2; self.lat = -2; - self.ID = 3; + self.ID = 1; M1.lon = 2; M1.lat = -3; @@ -255,10 +255,13 @@ int main(int argc,char** argv){ M2.lon = 0; M2.lat = 0; - M2.ID = 1; + M2.ID = 3; global_location data[]={target,self,M1,M2}; size_t index = sizeof(data)/sizeof(data[0]); + int x = 0; + int y = 0; + int* arr[] = {&x,&y}; find_ref_drone(data,index,check_leader(data,index).leader_ID);