无人机开发分享——机载ROS2系统通过DDS实现对开源飞控PX4在线规划与控制

最新推荐文章于 2025-09-02 15:39:00 发布

原创最新推荐文章于 2025-09-02 15:39:00 发布 · 1.6k 阅读

22 ·

CC 4.0 BY-SA版权

文章标签：

#无人机 #c++ #linux

该文章已生成可运行项目，

在无人机的机载智能系统的开发中，由于ROS2系统相较于ROS1的分布式通讯等优点，现在机载开发也已逐渐由ROS1逐渐过渡到ROS2系统开发上来。本文的开发分享主要分享如何在机载ROS2系统上开发机载程序对开源飞控PX4实现规划与控制。

STEP1 ROS2与PX4通讯

以前做过无人机机载开发的都知道，ROS1与px4使用了mavros与mavlink进行通讯，机载端将mavlink消息与mavros消息进行实时转换。到了ROS2，这一通讯体质已进行了改变（mavros仍然可用，但不是主流），ROS2采用XRCE-DDS模块进行通讯。
在这里插入图片描述
PX4端内部使用UORB进行通讯，对外采用uXRCE-DDS的客户端进行消息转换对外发布（串口、网口），ROS2平台运行uXRCE-DDS代理端（服务端）将收到的信息转换为ROS2的topic/service进行发布。

STEP2 ROS2开发软件对PX4进行规划控制

本部分可以通过硬件在环仿真的形式完成，即PX4硬件飞控、机载3588/nvidia板硬件、gazebo软件（之前的文章有介绍过如何仿真）。
首先设置PX4飞控为HITL硬件在环仿真模式，在机载板安装ROS2系统，XRCE-dds的客户端在PX4软件自动运行，仅需在ROS2端下载代理端即可。

git clone -b v2.4.2 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
cd Micro-XRCE-DDS-Agent
mkdir build
cd build
cmake ..
make
sudo make install
sudo ldconfig /usr/local/lib/
MicroXRCEAgent udp4 -p 8888

开启udp本地网口通讯，进行uorb消息与ros2消息的转换。

搞定试验基础后，我们可以先实现一个基础控制，起飞悬停操作。
首先，我们需要下载一个px4适配ROS2的msg包，里面是通用的PX4控制消息包。

git clone https://github.com/PX4/px4_msgs.git

在这里插入图片描述
然后在ROS2程序中建立一个PX4-ROS2的控制功能包


class OffboardControl : public rclcpp::Node
{
public:
	OffboardControl() : Node("offboard_control")
	{
		offboard_control_mode_publisher_ = this->create_publisher<OffboardControlMode>("fmu/in/offboard_control_mode", 10);
		trajectory_setpoint_publisher_ = this->create_publisher<TrajectorySetpoint>("fmu/in/trajectory_setpoint", 10);
		vehicle_command_publisher_ = this->create_publisher<VehicleCommand>("fmu/in/vehicle_command", 10);
		offboard_setpoint_counter_ = 0;
		auto timer_callback = [this]() -> void {
			if (offboard_setpoint_counter_ == 10) {
				// Change to Offboard mode after 10 setpoints
				this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
				// Arm the vehicle
				this->arm();
			}
			// offboard_control_mode needs to be paired with trajectory_setpoint
			publish_offboard_control_mode();
			publish_trajectory_setpoint();
			// stop the counter after reaching 11
			if (offboard_setpoint_counter_ < 11) {
				offboard_setpoint_counter_++;
			}
		};
		timer_ = this->create_wall_timer(100ms, timer_callback);
	}
	void arm();
	void disarm();
private:
	rclcpp::TimerBase::SharedPtr timer_;
	rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
	rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
	rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_;
	std::atomic<uint64_t> timestamp_;   //!< common synced timestamped
	uint64_t offboard_setpoint_counter_;   //!< counter for the number of setpoints sent
	void publish_offboard_control_mode();
	void publish_trajectory_setpoint();
	void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0);
};

先建立一个继承于rclcpp node的控制类，建立三个话题发布，offboard_control_mode_publisher_，trajectory_setpoint_publisher_，vehicle_command_publisher_分别为设置offboard模式的，轨迹跟踪目标点，无人机控制指令的消息。
然后声明一个lamda（大家有兴趣可以搜索学习下）回调函数timer_callback，里面实现对消息内容的填充与发布。

/**
 * @brief Send a command to Arm the vehicle
 */
void OffboardControl::arm()
{
	publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
	RCLCPP_INFO(this->get_logger(), "Arm command send");
}
/**
 * @brief Send a command to Disarm the vehicle
 */
void OffboardControl::disarm()
{
	publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
	RCLCPP_INFO(this->get_logger(), "Disarm command send");
}
/**
 * @brief Publish the offboard control mode.
 *        For this example, only position and altitude controls are active.
 */
void OffboardControl::publish_offboard_control_mode()
{
	OffboardControlMode msg{};
	msg.position = true;
	msg.velocity = false;
	msg.acceleration = false;
	msg.attitude = false;
	msg.body_rate = false;
	msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	offboard_control_mode_publisher_->publish(msg);
}
/**
 * @brief Publish a trajectory setpoint
 *        For this example, it sends a trajectory setpoint to make the
 *        vehicle hover at 5 meters with a yaw angle of 180 degrees.
 */
void OffboardControl::publish_trajectory_setpoint()
{
	TrajectorySetpoint msg{};
	msg.position = {0.0, 0.0, -5.0};
	msg.yaw = -3.14; // [-PI:PI]
	msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	trajectory_setpoint_publisher_->publish(msg);
}
/**
 * @brief Publish vehicle commands
 * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
 * @param param1    Command parameter 1
 * @param param2    Command parameter 2
 */
void OffboardControl::publish_vehicle_command(uint16_t command, float param1, float param2)
{
	VehicleCommand msg{};
	msg.param1 = param1;
	msg.param2 = param2;
	msg.command = command;
	msg.target_system = 0;
	msg.target_component = 1;
	msg.source_system = 1;
	msg.source_component = 1;
	msg.from_external = true;
	msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	vehicle_command_publisher_->publish(msg);
}

以解锁为例，会调用publish_vehicle_command函数，将解锁指令填充到控制指令里，然后publish_vehicle_command该函数会对VehicleComman的msg消息进行数据填充，然后发布vehicle_command_publisher_->publish(msg)。发布offboard模式转换和轨迹跟踪点同理。
最后建立main函数，生成节点对象并运行

int main(int argc, char *argv[])
{
	std::cout << "Starting offboard control node..." << std::endl;
	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<OffboardControl>());
	rclcpp::shutdown();
	return 0;
}

对这段ros2的功能包进行编译后，ROS2终端启动下该功能包节点即可。
在这里插入图片描述

STEP3 机载自定义更多管理规划软件

在实现基础控制后，可以在机载端自己设置管理软件，如通过区域设计侦察航线发送到轨迹跟踪点，通过状态机设计何时解锁、起飞、RTL，根据具体需求，如区域遍历，实现机载的智能控制。
在这里插入图片描述
智能化是未来无人机发展的重要方向，因此无人机的机载智能软件的开发十分重要，大家可以着手对该方向进行研究，同时可以共同沟通分享开发经验。