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

change leader when target change

Browse Source
protobuf
Xuan0319 3 years ago
parent ec4b10b723
commit b59265bc80
16 changed files with 381 additions and 123 deletions
Show Stats Download Patch File Download Diff File

8
class_model/include/class_model/FindLeader.h
Unescape Escape View File

@ -3,7 +3,7 @@
#include <iterator>
typedef struct result{
int leader_ID;
int leader_ID,leader_index;
bool is_leader;
}result;
@ -25,6 +25,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<index ;j++){
if(_result.leader_ID == data[j].ID){
_result.leader_index = j;
}
}
// std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
if (data[1].ID == _result.leader_ID){

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

@ -29,6 +29,7 @@ public:
void leader(global_location target);
void go2target(float x,float y);
int isOnTarget();
private:
// ROS NodeHandle
@ -41,6 +42,7 @@ private:
global_location current_location;
velocity drone_speed;
int flag = 0,heading_status = 0,buf = 0;
int OnTarget = 0;
float pre_alt,cur_alt;
float error_lon,error_lat;
float leader_pid[3] = {1 , 0.000000000001 ,0};

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

@ -1,7 +1,7 @@
/*Drone Control Command*/
#ifndef COMMAND_H
#define COMMAND_H
#define DEBUG
// #define DEBUG
#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/GlobalPositionTarget.h>

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

@ -1,7 +1,7 @@
/*Follow the leader in a fixed formation */
#ifndef FOLLOWER_H
#define FOLLOWER_H
#define swarmNum 5
#include <ros/ros.h>
#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");

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

@ -1,7 +1,7 @@
/*Follow the leader in a fixed formation */
#ifndef FORMATION_H
#define FORMATION_H
#define DEBUG
// #define DEBUG
#include <ros/ros.h>
#include "class_model/sensor.h"
#include "class_model/mode.h"

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

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

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

@ -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<float>()*1e7;
target.lat = config["routh1"]["y"].as<float>()*1e7;
target.lon = config["route1"]["x"].as<float>()*1e7;
target.lat = config["route1"]["y"].as<float>()*1e7;
routeNum = config["routeNum"]["num"].as<int>();
std::string route[4]={"route1","route2","route3","route4"};
sleep(5);
self = request_object.get_self_GPS();
@ -66,6 +71,23 @@ int main(int argc, char **argv) {
while(ros::ok()){
// type = receiver_object.check_command();
target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*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();
@ -73,32 +95,43 @@ 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;
// 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 + 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);
// target.lon = config["routh1"]["x"].as<float>()*1e7 ;
// target.lat = config["routh1"]["y"].as<float>()*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();

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

@ -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<float>()*1e7;
target.lat = config["routh1"]["y"].as<float>()*1e7;
target.lon = config["route1"]["x"].as<float>()*1e7;
target.lat = config["route1"]["y"].as<float>()*1e7;
routeNum = config["routeNum"]["num"].as<int>();
std::string route[4]={"route1","route2","route3","route4"};
sleep(5);
self = request_object.get_self_GPS();
@ -59,7 +64,6 @@ int main(int argc, char **argv) {
checkLeader = check_leader(*pos,index).is_leader;
leaderID = check_leader(*pos,index).leader_ID;
if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
@ -67,6 +71,23 @@ int main(int argc, char **argv) {
while(ros::ok()){
// type = receiver_object.check_command();
target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*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();
@ -74,17 +95,45 @@ 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);
}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();

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

@ -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<float>()*1e7;
target.lat = config["routh1"]["y"].as<float>()*1e7;
target.lon = config["route1"]["x"].as<float>()*1e7;
target.lat = config["route1"]["y"].as<float>()*1e7;
routeNum = config["routeNum"]["num"].as<int>();
std::string route[4]={"route1","route2","route3","route4"};
sleep(5);
self = request_object.get_self_GPS();
@ -66,6 +71,23 @@ int main(int argc, char **argv) {
while(ros::ok()){
// type = receiver_object.check_command();
target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*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();
@ -73,17 +95,45 @@ 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);
}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();

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

@ -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<float>()*1e7;
target.lat = config["routh1"]["y"].as<float>()*1e7;
target.lon = config["route1"]["x"].as<float>()*1e7;
target.lat = config["route1"]["y"].as<float>()*1e7;
routeNum = config["routeNum"]["num"].as<int>();
std::string route[4]={"route1","route2","route3","route4"};
sleep(5);
self = request_object.get_self_GPS();
@ -66,6 +71,23 @@ int main(int argc, char **argv) {
while(ros::ok()){
// type = receiver_object.check_command();
target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*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();
@ -73,17 +95,45 @@ 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);
}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();

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

@ -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<float>()*1e7;
target.lat = config["routh1"]["y"].as<float>()*1e7;
target.lon = config["route1"]["x"].as<float>()*1e7;
target.lat = config["route1"]["y"].as<float>()*1e7;
routeNum = config["routeNum"]["num"].as<int>();
std::string route[4]={"route1","route2","route3","route4"};
sleep(5);
self = request_object.get_self_GPS();
@ -66,6 +71,23 @@ int main(int argc, char **argv) {
while(ros::ok()){
// type = receiver_object.check_command();
target.lon = config[route[routeIndex]]["x"].as<float>()*1e7 ;
target.lat = config[route[routeIndex]]["y"].as<float>()*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();
@ -73,17 +95,45 @@ 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);
}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();

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

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

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

@ -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 <<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;
@ -325,8 +325,8 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
ref_ID = samePlane[check_index].ID;
std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << ",mem_index: "<< s_index << std::endl;
std::cout <<"\n"<< 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;
}
@ -422,5 +422,6 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
// ROS_INFO("drone%d,plane%d",self.ID,plane);
// command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1);
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);
}

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

@ -560,7 +560,7 @@ void FormationClass::calculate_position(float k,float theta,int direction){
#endif
#ifndef DEBUG
command_object.set_velocity(error_lon ,error_lat ,0,0,0.1);
command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
#endif
#ifdef DEBUG
command_object.set_velocity(error_lon ,error_lat ,0,0,0.1,get_data_t,cal_data_t);

Write Preview
Loading…
Cancel
Save