无人机避障仿真系统：Gazebo+PX4+ROS2+Fast-LIO2+SUPER框架联调

上一篇系统介绍已实现Fast-LIO2接收Gazebo仿真数据并基本完成定位与建图，然后SUPER的规划控制节点fsm_node启动，等待目标指令。关键问题：如何将SUPER自定义的控制指令传给Gazebo中的PX4飞控。

如何将ROS2的控制指令消息发给PX4，用uXRCE-DDS模块。PX4已运行相应的client，需在上位机启动agent，并发布相应的消息。

实现消息发布

自定义ros2 node子类，

cpp 复制代码

// 注册
  rclcpp::QoS px4_qos(
      rclcpp::QoS(5).best_effort().keep_last(5).durability_volatile());

  // Standard QoS for other topics
  rclcpp::QoS qos(
      rclcpp::QoS(1).best_effort().keep_last(1).durability_volatile());

  // Reliable QoS for PX4 command topics
  rclcpp::QoS px4_cmd_qos(
      rclcpp::QoS(10).reliable().keep_last(10).durability_volatile());

  // ==================== Publishers ====================
  odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>(odom_topic_, qos);
  cmd_pub_ = this->create_publisher<mars_quadrotor_msgs::msg::PositionCommand>(
      cmd_topic_, qos);

  // PX4 DDS publishers
  trajectory_setpoint_pub_ =
      this->create_publisher<px4_msgs::msg::TrajectorySetpoint>(
          "/fmu/in/trajectory_setpoint", px4_cmd_qos);
  offboard_control_mode_pub_ =
      this->create_publisher<px4_msgs::msg::OffboardControlMode>(
          "/fmu/in/offboard_control_mode", px4_cmd_qos);
  vehicle_command_pub_ = this->create_publisher<px4_msgs::msg::VehicleCommand>(
      "/fmu/in/vehicle_command", px4_cmd_qos);

// 设置定时器
cmd_watchdog_timer_ = this->create_wall_timer(
      std::chrono::milliseconds(100),
      std::bind(&GzBridgeNode::commandPublishTimerCallback, this));

void GzBridgeNode::commandPublishTimerCallback() {
  // Check for command timeout
  double cmd_age = (this->now() - last_cmd_time_).seconds();

  if (cmd_received_ && cmd_age > CMD_TIMEOUT) {
    RCLCPP_WARN(this->get_logger(),
                "\033[33m[WARN] Command timeout (%.2f s). "
                "SUPER may have stopped publishing.\033[0m",
                cmd_age);
  }

  // If no commands received yet, publish a hover setpoint at origin
  if (!cmd_received_) {
    px4_msgs::msg::TrajectorySetpoint setpoint;
    setpoint.timestamp = microsecondsSinceEpoch();
    setpoint.position = {0.0f, 0.0f, -1.5f}; // 1.5m above origin in NED
    setpoint.velocity = {std::nanf(""), std::nanf(""), std::nanf("")};
    setpoint.acceleration = {std::nanf(""), std::nanf(""), std::nanf("")};
    setpoint.jerk = {std::nanf(""), std::nanf(""), std::nanf("")};
    setpoint.yaw = 0.0f;
    setpoint.yawspeed = std::nanf("");
    trajectory_setpoint_pub_->publish(setpoint);
  }
}

将 PX4 的飞行状态（位置、姿态、速度）转换成 ROS2 标准的 nav_msgs/Odometry 消息，发布给 SUPER 规划器（fsm_node）。

SUPER 规划器需要知道无人机当前在哪里、朝哪个方向飞，才能规划轨迹。它订阅 /lidar_slam/odom 获取这些信息。

数据流：

PX4 DDS (/fmu/out/vehicle_local_position + vehicle_attitude)

↓ NED→ENU 转换

odom_pub_ → /lidar_slam/odom (nav_msgs/Odometry)

↓

SUPER fsm_node（规划器根据此做轨迹规划）

cmd_pub_ --- 转发 SUPER 的控制指令

cmd_pub_ = this->create_publisher<mars_quadrotor_msgs::msg::PositionCommand>(cmd_topic_, qos);

// 发布到 /planning/pos_cmd

作用：将 SUPER 规划器发出的 PositionCommand 原样转发回 /planning/pos_cmd 话题。

为什么需要：这是一个兼容性保留。原来的架构中 fsm_node 发布到 /planning/pos_cmd，perfect_drone_sim 订阅它。现在 gz_bridge_node 也订阅同一个话题来接收指令，但可能还有其他节点（如日志记录、可视化）也在监听这个话题，所以 cmd_pub_ 做了一次"环回"转发，确保其他订阅者仍然能收到。

数据流：

SUPER fsm_node → /planning/pos_cmd (PositionCommand)

↓

gz_bridgeNode 订阅 → 转换成 PX4 TrajectorySetpoint → 发给 PX4

↓ 同时环回

cmd_pub_ → /planning/pos_cmd (原样转发)

↓

其他订阅者（日志、可视化等）

启动Agent

安装
下载源代码，标准的ROS2 colcon build。

bash 复制代码

source /opt/ros/humble/setup.bash && cd ~/ros2_ws && colcon build --packages-select micro_ros_agent --cmake-args -DCMAKE_BUILD_TYPE=Release

运行

bash 复制代码

# 与Gazebo仿真从0计时匹配需参数 -t sim
$HOME/ros2_ws/install/micro_ros_agent/lib/micro_ros_agent/micro_ros_agent udp4 -p 8888 -t sim