diff --git a/class_model/include/class_model/command.h b/class_model/include/class_model/command.h index d5f54b9..e67f08a 100755 --- a/class_model/include/class_model/command.h +++ b/class_model/include/class_model/command.h @@ -6,7 +6,7 @@ #include #include #include -#include +// #include class CommandClass { public: @@ -25,7 +25,7 @@ private: ros::NodeHandle node_handle_; geometry_msgs::TwistStamped msg; - mavros_msgs::GlobalPositionTarget goal_position; + // mavros_msgs::GlobalPositionTarget goal_position; global_location current_location; global_location target_location; global_location origin_location; diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index 300aaf8..539a948 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -65,7 +65,7 @@ private: global_location pre_tar,ref_point; // function plane()'s variable float slope,c,y; // function plane()'s variable global_location pre_target; // function plane()'s variable - int flag = 0; + int flag = 0,pre_plane = 0; 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 874d9e8..57d9dfc 100644 --- a/class_model/src/Leader.cpp +++ b/class_model/src/Leader.cpp @@ -15,8 +15,9 @@ void LeaderClass::leader(global_location target){ flag = 0; if(pre_target.lon != target.lon || pre_target.lat != target.lat){ heading_status = 0; + ROS_INFO("heading_flag"); } - + heading_status = 0; while(true){ if(flag==0){ go2target(target.lon,target.lat); @@ -103,10 +104,10 @@ void LeaderClass::go2target(float target_lon,float target_lat){ // 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("************************************"); @@ -116,11 +117,11 @@ void LeaderClass::face2target(float target_heading){ float error_heading; float heading = GPS_object.get_heading(); - float error_yaw = 0.1; + float error_yaw = 0.2; error_heading = target_heading - heading; error_yaw = check_direction(error_heading)*error_yaw*0.5; - if (error_heading < 5 && error_heading > -5 ){ + if (error_heading < 3 && error_heading > -3 ){ error_yaw = 0; heading_status = 1; } diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index 8cd3f61..132b9dd 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -74,8 +74,22 @@ int main(int argc, char **argv) { if(checkLeader == 1){ leader_object.leader(target); - target.lon = config["routh2"]["x"].as()*1e7; - target.lat = config["routh2"]["y"].as()*1e7; + // for(int i=0;i<=360;i++){ + // target.lon = config["routh1"]["x"].as()*1e7 + 300*sin(i*0.2); + // target.lat = config["routh1"]["y"].as()*1e7 + 300*cos(i*0.2); + // leader_object.leader(target); + // std::cout<< "radin: " << i*3.14/180 <()*1e7 + 500; + target.lat = config["routh1"]["y"].as()*1e7 ; + leader_object.leader(target); + 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 -500; + target.lat = config["routh1"]["y"].as()*1e7 ; + 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 diff --git a/class_model/src/command.cpp b/class_model/src/command.cpp index 18cdc2c..0e8b9cb 100644 --- a/class_model/src/command.cpp +++ b/class_model/src/command.cpp @@ -10,7 +10,9 @@ CommandClass::CommandClass() : node_handle_("~"){ } velocity_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",10); - gps_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_position/global",10); + // gps_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_position/global",10); + gps_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_raw/global",10); + } CommandClass::~CommandClass() { ros::shutdown(); } @@ -98,22 +100,29 @@ void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second void CommandClass::set_global_position(double lon,double lat,float alt,float yaw){ - // mavros_msgs::GlobalPositionTarget goal_position; - // goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_REL_ALT; - // goal_position.type_mask = goal_position.IGNORE_VZ | goal_position.IGNORE_YAW_RATE; - // goal_position.latitude = lat; - // goal_position.longitude = lon; - // goal_position.altitude = alt; - // goal_position.yaw = yaw; + mavros_msgs::GlobalPositionTarget goal_position; + goal_position.coordinate_frame = mavros_msgs::GlobalPositionTarget::FRAME_GLOBAL_REL_ALT; + goal_position.type_mask = mavros_msgs::GlobalPositionTarget::IGNORE_VX | + mavros_msgs::GlobalPositionTarget::IGNORE_VY | + mavros_msgs::GlobalPositionTarget::IGNORE_VZ | + mavros_msgs::GlobalPositionTarget::IGNORE_AFX | + mavros_msgs::GlobalPositionTarget::IGNORE_AFY | + mavros_msgs::GlobalPositionTarget::IGNORE_AFZ | + mavros_msgs::GlobalPositionTarget::FORCE | + mavros_msgs::GlobalPositionTarget::IGNORE_YAW_RATE; + goal_position.latitude = lat; + goal_position.longitude = lon; + goal_position.altitude = alt; + goal_position.yaw = yaw; // goal_position.header.frame_id = "map"; - // // goal_position.type_mask = 65535; + // goal_position.type_mask = 65535; - geographic_msgs::GeoPoseStamped goal_position; - goal_position.header.frame_id = "map"; - goal_position.pose.position.latitude = lat; - goal_position.pose.position.longitude = lon; - goal_position.pose.position.altitude = 67; + // geographic_msgs::GeoPoseStamped goal_position; + // goal_position.header.frame_id = "map"; + // goal_position.pose.position.latitude = lat; + // goal_position.pose.position.longitude = lon; + // goal_position.pose.position.altitude = 67; ROS_INFO("%f,%f",lat,lon); double begin = ros::Time::now().toSec(); diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index a37e6d5..bd300c2 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -48,15 +48,19 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob //calculate drone is locate on R/L plane float phi = leader->heading/100 ; - phi = phi*3.14/180; + // phi = phi*3.14/180; + + if (phi > 355 && phi < 5){ + phi = 0; + } // float ref_y = leader->lat + 5000; // float ref_x = leader->lon + 1; // ref_point.lon = (cos(-phi)*leader->lon + sin(-phi)*ref_y); // ref_point.lat = (-sin(-phi)*leader->lon + cos(-phi)*ref_y); - ref_point.lon = 5000*sin(phi) + leader->lon; - ref_point.lat = 5000*cos(phi) + leader->lat; + 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); @@ -75,8 +79,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob // std::cout<< follower.ID << ","<< follower.lat <<","<< y <lat <= y && slope>0) || (follower->lat > y && slope<0)){ - std::cout <<"drone :"<ID <<", plane: 1" <lat <= y && phi < 180) || (follower->lat > y && phi >= 180)){ + // std::cout <<"drone :"<ID <<", plane: 1" <ID){ case 1: drone1.init_location.lat = follower->lat; @@ -109,8 +113,8 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob drone5.plane = 1; break; } - }else if((follower->lat > y && slope>0) || (follower->lat <= y && slope<0)){ - std::cout <<"drone :"<ID <<", plane: -1" <lat > y && phi < 180) || (follower->lat <= y && phi >= 180)){ + // std::cout <<"drone :"<ID <<", plane: -1" <ID){ case 1: drone1.init_location.lat = follower->lat; @@ -195,7 +199,7 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron // } cosTheta = k / (m*n); - std::cout <<"drone: "<= 0){ @@ -313,7 +317,7 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) ); // std::cout << i<<"," < - 0 5 0 0 0 0 + 0 -5 0 0 0 0 model://drone2 - 0 -5 0 0 0 0 + 0 5 0 0 0 0 model://drone3