Xuan0319
/
DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

test bio change leader success

Browse Source
protobuf
Xuan0319 3 years ago
parent 0bf03420d1
commit bf8f017f95
12 changed files with 463 additions and 206 deletions
Show Stats Download Patch File Download Diff File

2
class_model/include/class_model/convert_degree.h
Unescape Escape View File

@ -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;

7
class_model/include/class_model/follower.h
Unescape Escape View File

@ -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 <ros/ros.h>
#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);

4
class_model/src/Leader.cpp
Unescape Escape View File

@ -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;
@ -67,6 +65,8 @@ void LeaderClass::go2target(float target_lon,float target_lat){
y = (target_lat/100) - (current_location.lat*1e5);
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;
// error_yaw = check_direction(error_heading)*error_yaw; //check yaw direction

91
class_model/src/bio_main.cpp
Unescape Escape View File

@ -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]);
@ -68,9 +73,11 @@ int main(int argc, char **argv) {
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<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
@ -79,6 +86,8 @@ int main(int argc, char **argv) {
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;
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
@ -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<float>()*1e7 ;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
// leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
// 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);
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){
sleep(3);
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
routeIndex++;
std::cout <<"routeIndex: "<<routeIndex<<std::endl;
checkLeader = 0;
std::cout <<"Drone1routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
}
// }
// type = "x";
// while (routeIndex >= routeNum){
// // mode_object.set_Mode("LAND");
// // std::cout <<"LAND"<<std::endl;
// }
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
#endif
#ifdef NoChangeLeader
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["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 ;
target.lat = config["route1"]["y"].as<float>()*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;
}

91
class_model/src/bio_main2.cpp
Unescape Escape View File

@ -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]);
@ -68,9 +73,11 @@ int main(int argc, char **argv) {
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<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
@ -79,6 +86,8 @@ int main(int argc, char **argv) {
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;
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
@ -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<float>()*1e7 ;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
// leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
// 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);
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){
sleep(3);
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"Drone2routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
}
// }
// type = "x";
// while (routeIndex >= routeNum){
// // mode_object.set_Mode("LAND");
// // std::cout <<"LAND"<<std::endl;
// }
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
#endif
#ifdef NoChangeLeader
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["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 ;
target.lat = config["route1"]["y"].as<float>()*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
}
// mode_object.set_Mode("LAND");
}
#endif
ros::waitForShutdown();
return 0;
}

91
class_model/src/bio_main3.cpp
Unescape Escape View File

@ -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]);
@ -68,9 +73,11 @@ int main(int argc, char **argv) {
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<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*1e7 ;
@ -79,6 +86,8 @@ int main(int argc, char **argv) {
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;
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
@ -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<float>()*1e7 ;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
// leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
// 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);
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){
sleep(3);
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"Drone3routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
}
// }
// type = "x";
// while (routeIndex >= routeNum){
// // mode_object.set_Mode("LAND");
// // std::cout <<"LAND"<<std::endl;
// }
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
#endif
#ifdef NoChangeLeader
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["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 ;
target.lat = config["route1"]["y"].as<float>()*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
}
// mode_object.set_Mode("LAND");
}
#endif
ros::waitForShutdown();
return 0;
}

82
class_model/src/bio_main4.cpp
Unescape Escape View File

@ -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]);
@ -68,9 +73,11 @@ int main(int argc, char **argv) {
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<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*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<float>()*1e7 ;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
// leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
// 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);
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){
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
}
// }
// type = "x";
// while (routeIndex >= routeNum){
// // mode_object.set_Mode("LAND");
// // std::cout <<"LAND"<<std::endl;
// }
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
#endif
#ifdef NoChangeLeader
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["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 ;
target.lat = config["route1"]["y"].as<float>()*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
}
// mode_object.set_Mode("LAND");
}
#endif
ros::waitForShutdown();
return 0;
}

83
class_model/src/bio_main5.cpp
Unescape Escape View File

@ -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]);
@ -68,9 +73,11 @@ int main(int argc, char **argv) {
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<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*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<float>()*1e7 ;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
// leader_object.leader(target);
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
// 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);
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){
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
}
// }
// type = "x";
// while (routeIndex >= routeNum){
// // mode_object.set_Mode("LAND");
// // std::cout <<"LAND"<<std::endl;
// }
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
#endif
#ifdef NoChangeLeader
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["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 + 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 4000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 1000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 - 2000;
target.lat = config["route1"]["y"].as<float>()*1e7 - 3000;
leader_object.leader(target);
target.lon = config["route1"]["x"].as<float>()*1e7 ;
target.lat = config["route1"]["y"].as<float>()*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
}
// mode_object.set_Mode("LAND");
}
#endif
ros::waitForShutdown();
return 0;
}

10
class_model/src/config.yaml
Unescape Escape View File

@ -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)

144
class_model/src/follower.cpp
Unescape Escape View File

@ -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;
slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon);
#ifdef NoChangeLeader
slope = (ref_point.lat-leader->lat)/(ref_point.lon-leader->lon); // Leader-Ref
c = leader->lat - slope*leader->lon;
#endif
// if((target->lat != pre_target.lat) || (target->lon != pre_target.lon)){
// slope = (target->lat-leader->lat)/(target->lon-leader->lon);
#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;
#endif
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;
std::cout<< "slope: " << slope <<std::endl;
if((follower->lat <= y && phi < 180) || (follower->lat > y && phi >= 180)){
// std::cout <<"drone :"<<follower->ID <<", plane: 1" <<std::endl;
@ -162,41 +168,53 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
// 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;
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))){
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;
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;
}else{
pre_LT = vector_LT;
pre_tar.lat = target->lat;
pre_tar.lon = target->lon;
// }
}
/***************************************/
#endif
cosTheta = k / (m*n);
// std::cout <<"drone: "<<self.ID<<"memberID: "<<member.ID << " ,cosTheta:"<< cosTheta << std::endl;
@ -280,20 +298,20 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
j+=1;
// std::cout << "drone3" <<std::endl;
}
if(drone4.ID == data[1]->ID){
self = drone4;
}else{
memberDrone[j] = drone4;
j+=1;
// std::cout << "drone4" <<std::endl;
}
if(drone5.ID == data[1]->ID){
self = drone5;
}else{
memberDrone[j] = drone5;
j+=1;
// std::cout << "drone4" <<std::endl;
}
// if(drone4.ID == data[1]->ID){
// self = drone4;
// }else{
// memberDrone[j] = drone4;
// j+=1;
// // std::cout << "drone4" <<std::endl;
// }
// if(drone5.ID == data[1]->ID){
// self = drone5;
// }else{
// memberDrone[j] = drone5;
// j+=1;
// // std::cout << "drone4" <<std::endl;
// }
for(int i=0;i<index-2;i++){
@ -325,7 +343,7 @@ 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 << ",mem_index: "<< s_index << 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;
@ -361,15 +379,15 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid); //caculate error (PID)
error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
// error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid); //caculate error (PID)
// error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
// float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] ); //caculate error
// float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
// error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
// error_lat = Pf[1] - (follower_location.lat*1e+5);
error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
error_lat = Pf[1] - (follower_location.lat*1e+5);
float error_degree = deg_phi - heading;
@ -425,3 +443,35 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1);
// command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
}
void FollowerClass::face2target(global_location* target, global_location* leader){
float x,y,target_heading;
float heading = GPS_object.get_heading();
heading_status = 0;
error_yaw = 0.2;
x = (target->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);
}
}

2
class_model/src/receiver.cpp
Unescape Escape View File

@ -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(); }

4
iq_sim/worlds/three_drone.world
Unescape Escape View File

@ -89,14 +89,14 @@
</include>
</model>
<model name="drone2">
<pose> 0 -5 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 5 0 0 0 0</pose>
<pose> -1 -4 0 0 0 0</pose>
<include>
<uri>model://drone3</uri>
</include>

Write Preview
Loading…
Cancel
Save