You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
124 lines
3.8 KiB
C++
124 lines
3.8 KiB
C++
#include "class_model/Leader.h"
|
|
#include "class_model/convert_degree.h"
|
|
|
|
LeaderClass::LeaderClass() : node_handle_(""){
|
|
|
|
}
|
|
|
|
LeaderClass::~LeaderClass() { ros::shutdown(); }
|
|
|
|
void LeaderClass::leader(global_location target){
|
|
|
|
std::string command = "",pre_command = receiver_object.check_command();;
|
|
std_msgs::String message;
|
|
|
|
flag = 0;
|
|
// heading_status = 0;
|
|
while(true){
|
|
if(flag==0){
|
|
go2target(target.lon,target.lat);
|
|
}else{
|
|
// next_command.publish(message);
|
|
ROS_INFO("break");
|
|
break;
|
|
}
|
|
|
|
command = receiver_object.check_command();
|
|
while(command =="stop" || command == "land"){
|
|
// command_object.set_velocity(0,0,0,0,1);
|
|
// command = receiver_object.check_command();
|
|
|
|
if(command == "land"){
|
|
mode_object.set_Mode("LAND");
|
|
}else if(command != "stop"){
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
if(command != pre_command){
|
|
// ROS_INFO("change formation");
|
|
break;
|
|
}
|
|
pre_command = command;
|
|
}
|
|
}
|
|
|
|
void LeaderClass::go2target(float target_lon,float target_lat){
|
|
|
|
current_location=GPS_object.gps_position();
|
|
|
|
float target_heading,error_heading;
|
|
float heading = GPS_object.get_heading();
|
|
|
|
float x,y;
|
|
x = (target_lon/100) - (current_location.lon*1e5);
|
|
y = (target_lat/100) - (current_location.lat*1e5);
|
|
target_heading = ConvertDeg(x,y);
|
|
|
|
// float error_yaw = 0.2;
|
|
// error_heading = target_heading - heading;
|
|
// error_yaw = check_direction(error_heading)*error_yaw; //check yaw direction
|
|
|
|
while(heading_status != 1){
|
|
face2target(target_heading);
|
|
}
|
|
|
|
drone_speed.x = PID_x.update(current_location.lon*1e5 , target_lon/100 ,leader_pid); //leader_pid defined in header file
|
|
drone_speed.y = PID_y.update(current_location.lat*1e5 , target_lat/100 ,leader_pid);
|
|
|
|
// float error_x = PID_x.update1(current_location.lon*1e5 , target_location.lon );
|
|
// float error_y = PID_y.update1(current_location.lat*1e5 , target_location.lat );
|
|
|
|
if (drone_speed.x < ignore_small && drone_speed.x > -ignore_small){ //ignore small errors
|
|
drone_speed.x = 0;
|
|
}
|
|
if (drone_speed.y < ignore_small && drone_speed.y > -ignore_small){
|
|
drone_speed.y = 0;
|
|
}
|
|
if (drone_speed.x < -1){ //max velociy
|
|
drone_speed.x = -1;
|
|
}
|
|
if (drone_speed.x > 1){
|
|
drone_speed.x = 1;
|
|
}
|
|
if (drone_speed.y < -1){
|
|
drone_speed.y = -1;
|
|
}
|
|
if (drone_speed.y > 1){
|
|
drone_speed.y = 1;
|
|
}
|
|
|
|
if (drone_speed.x == 0 && drone_speed.y == 0){
|
|
flag = 1;
|
|
}
|
|
|
|
// PubData_object.publishData(drone_speed);
|
|
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("target:%f,%f",target_location.lon ,target_location.lat);
|
|
// ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5);
|
|
// ROS_INFO("************************************");
|
|
|
|
|
|
|
|
}
|
|
|
|
void LeaderClass::face2target(float target_heading){
|
|
|
|
float error_heading;
|
|
float heading = GPS_object.get_heading();
|
|
float error_yaw = 0.2;
|
|
error_heading = target_heading - heading;
|
|
error_yaw = check_direction(error_heading)*error_yaw;
|
|
|
|
if (error_heading < 5 && error_heading > -5 ){
|
|
error_yaw = 0;
|
|
heading_status = 1;
|
|
}
|
|
command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
|
|
// ROS_INFO("face2target...%f",target_heading);
|
|
} |