Ros2+Gazebo+PX4+QGC+DDS+Offboard控制示例
本文摘要介绍了无人机开发环境的搭建流程。主要包括：1）QGroundControl地面站的安装与使用；2）Gazebo仿真环境的配置（支持Harmonic和Classic版本）；3）PX4飞控系统的源码编译与仿真运行；4）uXRCE-DDS代理的部署用于ROS2通信；5）ROS2离板控制示例的实现。文档详细记录了各组件安装命令、常见问题解决方法以及联合测试步骤，为无人机开发提供了完整的软件环境配置
·
Ubuntu 22.04 ROS2 Humble的安装请自行搜索其他资料或参考我前面的文章。
1. QGC
QGroundControl为PX4或ArduPilot动力车辆提供完整的飞行控制和车辆配置。它为初学者提供了简单直接的使用体验，同时仍为有经验的用户提供高端功能支持。
# Ubuntu QGC安装
sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl -y
sudo apt install libfuse2 -y
sudo apt install libxcb-xinerama0 libxkbcommon-x11-0 libxcb-cursor-dev -y
chmod +x QGroundControl-<arch>.AppImage
./QGroundControl-<arch>.AppImage参考：https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html
2. Gazebo
Ubuntu22.04支持Gazebo Harmonic与Gazebo Classic(supported through 2025)，如已安装其他版本可卸载后重新安装Gazebo，参考：https://gazebosim.org/docs/harmonic/install_ubuntu/。
# Gazebo卸载
dpkg -l | grep gazebo
sudo apt-get remove xxx xxx xxx# Gazebo Harmonic安装
sudo apt-get update
sudo apt-get install curl lsb-release gnupg
sudo curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] https://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
sudo apt-get update
sudo apt-get install gz-harmonic
gz sim
sudo apt remove gz-harmonic && sudo apt autoremove #卸载命令运行结果：
Gazebo Classic：Gazebo 的功能正接近与 PX4 上的 Gazebo Classic 相当，并将很快取代它。
https://docs.px4.io/main/zh/sim_gazebo_classic/
Gazebo: 它取代了较早的 Gazebo Classic 模拟器，并且是 Ubuntu 22.04 及以后版本中唯一支持的 Gazebo 版本。
https://docs.px4.io/main/zh/sim_gazebo_gz/
建议先自行安装好Gazebo后，再去安装编译PX4。
PX4脚本安装文件的Gazebo代码如下，不知道为什么本人直接用脚本文件安装好，运行时会报错（本人遇到的问题）。
3. PX4
无论对于模拟器还是硬件目标设备，PX4固件可以在控制台或者IDE中从源码构建。若要使用模拟器，或需要修改PX4并创建自定义构建，则必须自行构建PX4。如遇报错大概率为网络问题，可多拉取几次进行尝试。
https://docs.px4.io/main/zh/dev_setup/dev_env_linux_ubuntu
https://docs.px4.io/main/zh/dev_setup/building_px4
# 安装部署PX4
git clone https://github.com/PX4/PX4-Autopilot.git --recursive # --recursive 参数会同时克隆所有子模块
cd PX4-Autopilot
git submodule update --init –recursive # 更新并初始化所有子模块，确保所有依赖项都已获取
bash ./Tools/setup/ubuntu.sh # 安装所有必要的依赖项和工具
git describe --tags # 版本信息4. Gazebo+QGC中Mavlink控制台控制飞行器
# 仿真运行
cd PX4-Autopilot
make px4_sitl gz_x500
commander takeoff # 起飞
commander land shutdown # 降落
5. uXRCE-DDS代理
PX4 上运行的 uXRCE-DDS 客户端，连接至机载电脑上运行的micro XRCE-DDS 代理，两者之间通过串口或 UDP 链路实现双向数据交换，允许 uORB 消息像 ROS 2 主题一样在配套计算机上发布和订阅。
参考：https://docs.px4.io/main/zh/middleware/uxrce_dds#xrce-dds-agent
# 源码安装
git clone -b v2.4.3 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 # 连接模拟器，PX4模拟器通过UDP在8888端口运行uXRCE-DDS客户端结果：
make报错：
解决：https://github.com/PX4/PX4-Autopilot/issues/24477
版本参考：https://github.com/eProsima/Fast-DDS/tags
6. ROS 2 Offboard控制示例
以下C++示例展示了如何从ROS 2节点在离板模式下进行多旋翼位置控制。示例开始发送设定点，进入离板模式，起步，上升到5米，然后等待。虽然简单，但它展示了如何使用离机控制和发送载具指令的主要原理。
参考：https://docs.px4.io/main/zh/ros2/offboard_control
# Offboard模式示例
mkdir -p ~/ws_offboard_control/src/
cd ~/ws_offboard_control/src/
git clone https://github.com/PX4/px4_msgs.git # 用于将ROS 2节点与PX4内部接口连接
git clone https://github.com/PX4/px4_ros_com.git # 用于在 ROS2 和 PX4 之间交换数据和命令的示例节点，直接依赖px4_msgs
cd ..
source /opt/ros/humble/setup.bash
colcon build
source install/local_setup.bash联合测试，打开QGC并开启四个终端对应位置下分别执行如面的命令：
# 测试命令
./QGroundControl-x86_64.AppImage # 打开QGC
make px4_sitl gz_x500 # 运行仿真
MicroXRCEAgent udp4 -p 8888 # 连接模拟器
ros2 run px4_ros_com offboard_control # 启动示例
ros2 topic list # 查看话题信息，echo对应话题要在px4_msgs工作空间下
输出结果：无人机会飞至5米悬停空中，此时可以看到接收与输出的topic信息，后期进行二次开发需参考这些话题信息，需进行了解，参考https://docs.px4.io/main/zh/middleware/dds_topics。
上述内容基本都参考于PX4文档，笔者为巩固理解，用笔记的形式再重复一遍。
7. px4_ros_com示例
px4_ros_com（https://github.com/PX4/px4_ros_com）这个包提供了PX4与ROS2之间的通讯示例（依赖px4_msgs），包括发布者advertisers、订阅者listeners、离板控制offboard control三种模式，如下图。
7.1. advertisers
功能：每500ms向PX4发送调试向量数据。
代码：相对简单，不过多讲解，主要代码如下。
# DebugVectAdvertiser
class DebugVectAdvertiser : public rclcpp::Node
{
public:
DebugVectAdvertiser() : Node("debug_vect_advertiser")
{
// 创建发布者，发布px4_msgs::msg::DebugVect类型消息，发布的话题为/fmu/in/debug_vect，队列长度为10
publisher_ = this->create_publisher<px4_msgs::msg::DebugVect>("/fmu/in/debug_vect", 10);
// 定义定时器回调函数
auto timer_callback = [this]()->void {
auto debug_vect = px4_msgs::msg::DebugVect();
debug_vect.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now()).time_since_epoch().count(); // 设置消息时间戳，使用std::chrono::steady_clock获取当前时间
std::string name = "test";
std::copy(name.begin(), name.end(), debug_vect.name.begin());
debug_vect.x = 1.0;
debug_vect.y = 2.0;
debug_vect.z = 3.0;
RCLCPP_INFO(this->get_logger(), "\033[97m Publishing debug_vect: time: %lu x: %f y: %f z: %f \033[0m",
debug_vect.timestamp, debug_vect.x, debug_vect.y, debug_vect.z);
this->publisher_->publish(debug_vect);
};
// 创建定时器
timer_ = this->create_wall_timer(500ms, timer_callback);
}
private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<px4_msgs::msg::DebugVect>::SharedPtr publisher_;
};测试：参考第6部分，运行仿真、连接模拟器、运行下面命令、并echo查看话题信息。
# 发布者测试命令
ros2 run px4_ros_com debug_vect_advertiser
ros2 topic echo /fmu/in/debug_vect结果：
7.2. listeners
7.2.1. sensor_combined_listener.cpp
功能：订阅并显示 PX4 传感器融合的数据
代码：如下，加了部分注释
# SensorCombinedListener
class SensorCombinedListener : public rclcpp::Node
{
public:
explicit SensorCombinedListener() : Node("sensor_combined_listener")
{
rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
// 创建订阅者，订阅/fmu/out/sensor_combined话题，使用Qos策略
subscription_ = this->create_subscription<px4_msgs::msg::SensorCombined>("/fmu/out/sensor_combined", qos,
[this](const px4_msgs::msg::SensorCombined::UniquePtr msg) {
std::cout << "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n";
std::cout << "RECEIVED SENSOR COMBINED DATA" << std::endl;
std::cout << "=============================" << std::endl;
std::cout << "ts: " << msg->timestamp << std::endl;
// 上一陀螺仪采样周期内，在 FRD 机体坐标系 XYZ 轴上测得的平均角速度，单位 rad/s
// gyro_rad[0]: X轴角速度
// gyro_rad[1]: Y轴角速度
// gyro_rad[2]: Z轴角速度
std::cout << "gyro_rad[0]: " << msg->gyro_rad[0] << std::endl;
std::cout << "gyro_rad[1]: " << msg->gyro_rad[1] << std::endl;
std::cout << "gyro_rad[2]: " << msg->gyro_rad[2] << std::endl;
// 陀螺仪采样周期，单位微秒
std::cout << "gyro_integral_dt: " << msg->gyro_integral_dt << std::endl;
// 时间戳 + 加速度计相对时间戳 = 加速度计时间戳
std::cout << "accelerometer_timestamp_relative: " << msg->accelerometer_timestamp_relative << std::endl;
// 上一加速度计采样周期内，在 FRD 机体坐标系 XYZ 轴上测得的平均加速度值，单位为 m/s²
// accelerometer_m_s2[0]: X轴加速度
// accelerometer_m_s2[1]: Y轴加速度
// accelerometer_m_s2[2]: Z轴加速度（含重力）
std::cout << "accelerometer_m_s2[0]: " << msg->accelerometer_m_s2[0] << std::endl;
std::cout << "accelerometer_m_s2[1]: " << msg->accelerometer_m_s2[1] << std::endl;
std::cout << "accelerometer_m_s2[2]: " << msg->accelerometer_m_s2[2] << std::endl;
// 加速度计采样周期，单位微秒
std::cout << "accelerometer_integral_dt: " << msg->accelerometer_integral_dt << std::endl;
});
}
private:
rclcpp::Subscription<px4_msgs::msg::SensorCombined>::SharedPtr subscription_;
};
代码中话题/fmu/out/sensor_combined内容：
测试：参考第6部分，运行仿真、连接模拟器、运行下面命令。
# 订阅者测试
ros2 run px4_ros_com sensor_combined_listener结果：终端打印信息。
7.2.2. vehicle_gps_position_listener.cpp
功能：订阅并显示 GPS 数据。
代码：如下，加了部分注释。
# VehicleGpsPositionListener
class VehicleGpsPositionListener : public rclcpp::Node
{
public:
explicit VehicleGpsPositionListener() : Node("vehicle_global_position_listener")
{
rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
subscription_ = this->create_subscription<px4_msgs::msg::SensorGps>("/fmu/out/vehicle_gps_position", qos,
[this](const px4_msgs::msg::SensorGps::UniquePtr msg) {
std::cout << "\n\n\n\n\n\n\n\n\n\n";
std::cout << "RECEIVED VEHICLE GPS POSITION DATA" << std::endl;
std::cout << "==================================" << std::endl;
std::cout << "ts: " << msg->timestamp << std::endl;
// 纬度，单位为度，支持厘米级 RTK 定位精度
// 经度，单位为度，支持厘米级 RTK 定位精度
// 海拔高度（基于平均海平面），单位为米
std::cout << "lat: " << msg->latitude_deg << std::endl;
std::cout << "lon: " << msg->longitude_deg << std::endl;
std::cout << "alt: " << msg->altitude_msl_m << std::endl;
// 椭球高度，单位为米
std::cout << "alt_ellipsoid: " << msg->altitude_ellipsoid_m << std::endl;
// GPS 速度精度估计值（米 / 秒）
// GPS 航向精度估计值（弧度）
std::cout << "s_variance_m_s: " << msg->s_variance_m_s << std::endl;
std::cout << "c_variance_rad: " << msg->c_variance_rad << std::endl;
// 部分应用仅在该值不小于 2 时才会使用，需确保准确保填定位类型
std::cout << "fix_type: " << msg->fix_type << std::endl;
// GPS 水平位置精度（米）
// GPS 垂直位置精度（米）
// 水平精度因子
// 垂直精度因子
std::cout << "eph: " << msg->eph << std::endl;
std::cout << "epv: " << msg->epv << std::endl;
std::cout << "hdop: " << msg->hdop << std::endl;
std::cout << "vdop: " << msg->vdop << std::endl;
// 陀螺仪每毫秒噪声值
std::cout << "noise_per_ms: " << msg->noise_per_ms << std::endl;
// GPS 地面速度（米 / 秒）
// GPS 北向速度（米 / 秒）
// GPS 东向速度（米 / 秒）
// GPS 天向速度（米 / 秒，向下为正）
std::cout << "vel_m_s: " << msg->vel_m_s << std::endl;
std::cout << "vel_n_m_s: " << msg->vel_n_m_s << std::endl;
std::cout << "vel_e_m_s: " << msg->vel_e_m_s << std::endl;
std::cout << "vel_d_m_s: " << msg->vel_d_m_s << std::endl;
// 地面航向（非载体朝向，为实际运动方向），取值范围 -π~π（弧度）
std::cout << "cog_rad: " << msg->cog_rad << std::endl;
// 北向 - 东向 - 天向（NED）速度是否有效，有效值为 True
std::cout << "vel_ned_valid: " << msg->vel_ned_valid << std::endl;
// 世界协调时间相对偏移量，时间戳 + 该值 = 系统启动后对应的 UTC 时间戳（微秒）
std::cout << "timestamp_time_relative: " << msg->timestamp_time_relative << std::endl;
// UTC 时间戳（微秒），由 GPS 模块提供；冷启动后可能暂不可用，此时值为 0
std::cout << "time_utc_usec: " << msg->time_utc_usec << std::endl;
// 参与定位的卫星数量
std::cout << "satellites_used: " << msg->satellites_used << std::endl;
// 载体 XYZ 坐标系相对 NED 坐标系的航向角（仅双天线 GPS 有效），不可用时设为 NaN，单位弧度，取值范围 [-π, π]
std::cout << "heading: " << msg->heading << std::endl;
// 双天线阵列在载体坐标系中的航向偏移量（非双天线设为 NaN），单位弧度，取值范围 [-π, π]
std::cout << "heading_offset: " << msg->heading_offset << std::endl;
});
}
private:
rclcpp::Subscription<px4_msgs::msg::SensorGps>::SharedPtr subscription_;
};
代码中话题/fmu/out/vehicle_gps_position内容：
测试：参考第6部分，运行仿真、连接模拟器、运行下面命令。
ros2 run px4_ros_com vehicle_gps_position_listener结果：终端打印信息。
7.3. offboard control
7.3.1. offboard_control.cpp
功能：通过发布命令实现离板控制。
代码：如下，加了部分注释
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 {
// 当计数器达到10时（发送10次设定点后，约1秒）
if (offboard_setpoint_counter_ == 10) {
// 切换到离板模式
// VehicleCommand::VEHICLE_CMD_DO_SET_MODE = 176
// param1 = 1（MAV_MODE_FLAG_CUSTOM_MODE_ENABLED）
// param2 = 6（PX4的离板模式编号）
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);
};
// 三个主要方法
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);
}
void OffboardControl::publish_trajectory_setpoint()
{
TrajectorySetpoint msg{};
msg.position = {0.0, 0.0, -5.0}; // X, Y, Z坐标（NED坐标系）
msg.yaw = -3.14; // [-PI:PI]
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher_->publish(msg);
}
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 = 1;
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);
}测试：
ros2 run px4_ros_com offboard_control7.3.2. offboard_control_srv.cpp
功能：通过ROS 2服务调用Service实现离板控制而不是话题来发送命令控制。
代码：代码核心实现了一个五状态的状态机，依次为初始化init、离板模式请求offboard_requested、等待稳定wait_for_stable_offboard_mode、解锁请求arm_requested和已解锁状态armed，通过100毫秒定时器驱动状态流转。与前面话题发布进行控制相比，异步服务调用与响应回调函数，能够根据PX4返回的命令执行结果码（接受、拒绝、失败等）进行精确的状态管理和错误处理，在命令失败时自动关闭节点保障安全。
class OffboardControl : public rclcpp::Node
{
public:
OffboardControl(std::string px4_namespace) : Node("offboard_control_srv"),
state_{State::init},
service_result_{0},
service_done_{false},
// 初始化发布器
offboard_control_mode_publisher_{this->create_publisher<OffboardControlMode>(px4_namespace+"in/offboard_control_mode", 10)},
trajectory_setpoint_publisher_{this->create_publisher<TrajectorySetpoint>(px4_namespace+"in/trajectory_setpoint", 10)},
// 初始化服务客户端
vehicle_command_client_{this->create_client<px4_msgs::srv::VehicleCommand>(px4_namespace+"vehicle_command")}
{
RCLCPP_INFO(this->get_logger(), "Starting Offboard Control example with PX4 services");
RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for " << px4_namespace << "vehicle_command service");
// 等待服务可用（阻塞等待）
while (!vehicle_command_client_->wait_for_service(1s)) {
// 如果ROS2被中断
if (!rclcpp::ok()) {
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
timer_ = this->create_wall_timer(100ms, std::bind(&OffboardControl::timer_callback, this));
}
void switch_to_offboard_mode(); // 切换到离板模式
void arm();
void disarm();
private:
// 状态机枚举定义
enum class State{
init, // 初始状态
offboard_requested, // 已请求切换到离板模式
wait_for_stable_offboard_mode, // 等待稳定的离板模式
arm_requested, // 已请求解锁
armed // 已解锁
} state_;
uint8_t service_result_; // 服务调用结果
bool service_done_; // 服务是否完成
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedPtr vehicle_command_client_;
void publish_offboard_control_mode(); // 发布离板控制模式
void publish_trajectory_setpoint(); // 发布轨迹设定点
void request_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0); // 请求车辆命令（使用服务）
void response_callback(rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedFuture future); // 服务响应回调函数
void timer_callback(void); // 定时器回调函数
};
void OffboardControl::request_vehicle_command(uint16_t command, float param1, float param2)
{
// 创建服务请求
auto request = std::make_shared<px4_msgs::srv::VehicleCommand::Request>();
VehicleCommand msg{};
msg.param1 = param1;
msg.param2 = param2;
msg.command = command;
msg.target_system = 1;
msg.target_component = 1;
msg.source_system = 1;
msg.source_component = 1;
msg.from_external = true;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
// 将消息赋值给请求
request->request = msg;
// 重置服务状态
service_done_ = false;
// 异步发送服务请求
// std::bind绑定回调函数，std::placeholders::_1是占位符
auto result = vehicle_command_client_->async_send_request(request, std::bind(&OffboardControl::response_callback, this,
std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "Command send");
}
// 定时器回调函数
void OffboardControl::timer_callback(void){
static uint8_t num_of_steps = 0;
// offboard_control_mode needs to be paired with trajectory_setpoint
publish_offboard_control_mode();
publish_trajectory_setpoint();
// 状态机处理
switch (state_)
{
// 初始状态，第一步：请求切换到离板模式
case State::init :
switch_to_offboard_mode();
state_ = State::offboard_requested;
break;
// 已请求离板模式
case State::offboard_requested :
if(service_done_){ // 等待服务响应完成
if (service_result_==0){
RCLCPP_INFO(this->get_logger(), "Entered offboard mode");
state_ = State::wait_for_stable_offboard_mode; // 进入等待状态
}
else{
RCLCPP_ERROR(this->get_logger(), "Failed to enter offboard mode, exiting");
rclcpp::shutdown(); // 关闭节点
}
}
break;
// 等待稳定的离板模式
case State::wait_for_stable_offboard_mode :
// 等待10个周期（1秒）确保模式稳定
if (++num_of_steps>10){
arm();
state_ = State::arm_requested; // 进入解锁请求状态
}
break;
// 已请求解锁
case State::arm_requested :
if(service_done_){ // 等待服务响应
if (service_result_==0){
RCLCPP_INFO(this->get_logger(), "vehicle is armed");
state_ = State::armed; // 进入已解锁状态
}
else{
RCLCPP_ERROR(this->get_logger(), "Failed to arm, exiting");
rclcpp::shutdown();
}
}
break;
default:
break;
}
}
// 服务响应回调函数
void OffboardControl::response_callback(
rclcpp::Client<px4_msgs::srv::VehicleCommand>::SharedFuture future) {
auto status = future.wait_for(1s);
// 响应就绪
if (status == std::future_status::ready) {
auto reply = future.get()->reply; // 获取响应
service_result_ = reply.result; // 保存结果
// 根据结果输出相应信息
switch (service_result_)
{
case reply.VEHICLE_CMD_RESULT_ACCEPTED:
RCLCPP_INFO(this->get_logger(), "command accepted");
break;
case reply.VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
RCLCPP_WARN(this->get_logger(), "command temporarily rejected");
break;
case reply.VEHICLE_CMD_RESULT_DENIED:
RCLCPP_WARN(this->get_logger(), "command denied");
break;
case reply.VEHICLE_CMD_RESULT_UNSUPPORTED:
RCLCPP_WARN(this->get_logger(), "command unsupported");
break;
case reply.VEHICLE_CMD_RESULT_FAILED:
RCLCPP_WARN(this->get_logger(), "command failed");
break;
case reply.VEHICLE_CMD_RESULT_IN_PROGRESS:
RCLCPP_WARN(this->get_logger(), "command in progress");
break;
case reply.VEHICLE_CMD_RESULT_CANCELLED:
RCLCPP_WARN(this->get_logger(), "command cancelled");
break;
default:
RCLCPP_WARN(this->get_logger(), "command reply unknown");
break;
}
service_done_ = true;
} else {
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
}
}测试：
ros2 run px4_ros_com offboard_control_srv两种Offboard模式运行的结果都是相同的，在第6部分已展示。
立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。
更多推荐
30
0- 0
已为社区贡献1条内容
所有评论(0)