用遥控器控制无人机来进行egoplanner规划代码

代码随便写的，很简陋简洁

演示效果视频链接：https://www.bilibili.com/video/BV144421F71W/?spm_id_from=333.999.0.0&vd_source=0bdc57fc5be4d99023aec119511a7cc8整体代码如下

/*****************************************************************************************
 * 遥控器手动控制跟踪egoplanner轨迹
 * 本代码需要在自己写的ego轨迹跟踪控制下弄，因为PX4CTRL这个代码里的offboard模式下遥控器摇杆会和本代码影响冲突
 * 编译成功后运行即可
 *操作方法，pitch杆前推就会往相对机头正前方contorl_dist距离的点飞去，往后推则是回到起飞点（原点，相当于返航）
 yaw杆往左推则往相对机头方向左侧45度角度，距离为contorl_dist的航点飞去，
 yaw杆往右边推则往相对机头方向右侧45度角度，距离为contorl_dist的航点飞去。
 目前就写了这两个杆的功能，有需求的话自己该代码就行了，我也是随便写的，代码也不难
 ******************************************************************************************/

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandLong.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include<mavros_msgs/RCIn.h>
#include<nav_msgs/Odometry.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

mavros_msgs::RCIn rc;
nav_msgs::Odometry position_msg;
geometry_msgs::PoseStamped target_pos, joy_point;
mavros_msgs::State current_state;
float position_x, position_y, position_z,  current_yaw, targetpos_x, targetpos_y;
float pi = 3.14159265;
double rc1, rc2, rc3, rc4;

double control_dist = 3;//这个代表着控制点与无人机的距离
void rc_cb(const mavros_msgs::RCIn::ConstPtr&msg)
{
  rc = *msg;
  rc1 = rc.channels[0];//roll
  rc2 = rc.channels[1];//pitch[908,2090]
  rc3 = rc.channels[2];//thrust
  rc4 = rc.channels[3];//yaw[1003,1995]
}

void state_cb(const mavros_msgs::State::ConstPtr& msg){
	current_state = *msg;
}

//read vehicle odometry
void position_cb(const nav_msgs::Odometry::ConstPtr&msg)
{
  position_msg=*msg;
  position_x = position_msg.pose.pose.position.x;
  position_y = position_msg.pose.pose.position.y;
  position_z = position_msg.pose.pose.position.z;
	tf2::Quaternion quat;
	tf2::convert(msg->pose.pose.orientation, quat); //把mavros/local_position/pose里的四元数转给tf2::Quaternion quat
	double roll, pitch, yaw;
	tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
	current_yaw = yaw;
}


int main(int argc, char **argv)
{
	ros::init(argc, argv, "ego_joy_plan_node");
	setlocale(LC_ALL,"");
	ros::NodeHandle nh;
	ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
	("/mavros/state", 10, state_cb);//读取飞控状态的话题
  ros::Publisher joy_point_pub = nh.advertise<geometry_msgs::PoseStamped>
	("move_base_simple/goal", 1);
  ros::Subscriber rc_sub=nh.subscribe<mavros_msgs::RCIn>
  ("/mavros/rc/in",10,rc_cb);//读取遥控器通道的话题
	ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
	("/mavros/cmd/arming");//控制无人机解锁的服务端，不需要用
	ros::ServiceClient command_client = nh.serviceClient<mavros_msgs::CommandLong>
	("/mavros/cmd/command");
	ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
	("/mavros/set_mode");//设置飞机飞行模式的服务端，也不需要用
	ros::Subscriber position_sub=nh.subscribe<nav_msgs::Odometry>
  ("/mavros/local_position/odom",10,position_cb);

    ros::Rate rate(3); //

  while(ros::ok())
   { 
    if(rc2<2100 && rc2>1700)
     {
      joy_point.pose.position.x = control_dist*cos(current_yaw) + position_x;
	    joy_point.pose.position.y = control_dist*sin(current_yaw) + position_y;
	    joy_point.pose.position.z = position_z;
      joy_point_pub.publish(joy_point);
     }

    if(rc2<1300 && rc2>900)
     {
      joy_point.pose.position.x = 0;
	    joy_point.pose.position.y = 0;
	    joy_point.pose.position.z = 1;
      joy_point_pub.publish(joy_point);
      ROS_INFO("返航");
     }

    if(rc4<2000 && rc4>1700)
     {
      joy_point.pose.position.x = control_dist*cos(current_yaw-pi/4) + position_x;
	    joy_point.pose.position.y = control_dist*sin(current_yaw-pi/4) + position_y;
	    joy_point.pose.position.z = position_z;
      joy_point_pub.publish(joy_point);
     }
    if(rc4<1300 && rc4>900)
     {
      joy_point.pose.position.x = control_dist*cos(current_yaw+pi/4) + position_x;
	    joy_point.pose.position.y = control_dist*sin(current_yaw+pi/4) + position_y;
	    joy_point.pose.position.z = position_z;
      joy_point_pub.publish(joy_point);
     }
    ros::spinOnce();
    rate.sleep();
   }

	return 0;
}

关于egoplanner自定义控制器的写法思路有两种，一种位置控制（这种最简单）一种速度控制（也还行）

这里说一下位置控制，只需要订阅ego的/position_cmd这个话题，然后把里面的位置信息传给mavros的位置控制话题就行了，例如/mavros/setpoint_position/local。