使用ego-planner进行无人机控制时的数据流（自用）

本文将详细介绍如何使用ego-planner向无人机发送控制指令，实现自动控制的过程。我们将探讨ego-planner的运行方式以及控制指令的传输过程，涉及到ROS节点、MAVROS协议以及无人机的控制器等方面。

在实现自动控制时，控制指令的传输是至关重要的一环。本文将详细介绍ego-planner如何向无人机发送控制指令，从而实现自动控制的过程。

无人机控制流程：

1. 运行run_ctrl.launch实现自动起飞和悬停。
2. 可通过遥控器或机载电脑进行手动或自动控制。

本文主要讲解自动控制中的控制指令传输过程。
控制指令传输过程：
在ego-planner中，控制指令的传输是通过px4ctrl_node.cpp实现的，该节点利用MAVROS将控制指令发送给无人机。（realflight_modules/px4ctrl/src/px4ctrl_node.cpp）

ros::Subscriber cmd_sub = nh.subscribe<quadrotor_msgs::PositionCommand>(
    "cmd",
    100,
    boost::bind(&Command_Data_t::feed, &fsm.cmd_data, _1),
    ros::VoidConstPtr(),
    ros::TransportHints().tcpNoDelay());

fsm.ctrl_FCU_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);

在控制有限状态机中，当状态为CMD_CTRL时，通过get_cmd_des()函数获取控制指令的期望状态。

case CMD_CTRL:
{
   
    if (!rc_data.is_hover_mode || !odom_is_received(now_time))
    {
   
        state = MANUAL_CTRL;
        toggle_offboard_mode(false);

        ROS_WARN("[px4ctrl] From CMD_CTRL(L3) to MANUAL_CTRL(L1)!");
    }
    else if (!rc_data.is_command_mode || !cmd_is_received(now_time))
    {
   
        state = AUTO_HOVER;
        set_hov_with_odom();
        des = get_hover_des();
        ROS_INFO("[px4ctrl] From CMD_CTRL(L3) to AUTO_HOVER(L2)!");
    }
    else
    {
   
        des = get_cmd_des();
    }
    ...
    break;
}

控制指令获取过程：
在get_cmd_des()函数中，获取控制指令的期望状态des，包括位置、速度、加速度、加加速度、偏航角、偏航角速度。

Desired_State_t PX4CtrlFSM::get_cmd_des()
{
   
    Desired_State_t des;
    des.p = cmd_data.p;
    des.v = cmd_data.v;
    des.a = cmd_data.a;
    des.j = cmd_data.j;
    des.yaw = cmd_data.yaw;
    des.yaw_rate = cmd_data.yaw_rate;

    return des;
}

设定期望值des后，控制器负责根据获取的控制指令更新无人机的状态，其中包括推力模型的估计、控制指令的计算以及控制命令的发布。

// STEP2: estimate thrust model
	if (state == AUTO_HOVER || state == CMD_CTRL)
	{
   
		// controller.estimateThrustModel(imu_data.a, bat_data.volt, param);
		controller.estimateThrustModel(imu_data.a,param);

	}

	// STEP3: solve and update new control commands
	if (rotor_low_speed_during_land) // used at the start of auto takeoff
	{
   
		motors_idling(imu_data, u);
	}
	else
	{
   
		debug_msg = controller.calculateControl(des, odom_data, imu_data, u);
		debug_msg.header.stamp = now_time;
		debug_pub.publish(debug_msg);
	}

	// STEP4: publish control commands to mavros
	if (param.use_bodyrate_ctrl)
	{
   
		publish_bodyrate_ctrl(u, now_time);
	}
	else
	{
   
		publish_attitude_ctrl(u, now_time);
	}

cmd_data 通过px4ctrl中subscriber接收得到（见上面subscriber中定义）。

可以看到STEP4有两种情况：bodyrate_ctrl和attitude_ctrl。看一下函数定义：

void PX4CtrlFSM::publish_bodyrate_ctrl(const Controller_Output_t &u, const ros::Time &stamp)
{
   
	mavros_msgs::AttitudeTarget msg;

	msg.header.stamp = stamp;
	msg.header.frame_id = std::string("FCU");

	msg.type_mask = mavros_msgs::AttitudeTarget::IGNORE_ATTITUDE;

	msg.body_rate.x = u.bodyrates.x();
	msg.body_rate.y = u.bodyrates.y();
	msg.body_rate.z = u.bodyrates.z();

	msg.thrust = u