Ubuntu22.04 + PX4 + ROS 2 + Gazebo Harmonic四轴无人机仿真环境中开发功能（1）

这份指南假设已经完成了基础环境安装（ROS 2 Humble, Gazebo, PX4 源码），现在我们专注于搭建工作区并实现键盘控制功能。

---

准备工作：启动仿真环境

在开始编写代码前，请先准备好以下环境（保持运行）：

1. 启动 QGroundControl (可选但推荐)

   • 双击运行 QGroundControl.AppImage，用于监视无人机状态（如是否解锁、GPS 是否正常）。

2. 终端 1：启动通信代理 (Agent)

   Bash

   MicroXRCEAgent udp4 -p 8888

3. 终端 2：启动 PX4 + Gazebo 仿真

   Bash

   cd ~/PX4-Autopilot/
   make px4_sitl gz_x500

   等待 Gazebo 窗口弹出，且 QGroundControl 显示 "Ready To Fly" (绿色)。

---

核心步骤：编写与运行控制节点

接下来的操作都在 终端 3 中进行。

第一步：创建工作区与依赖包

我们将创建一个包含 px4_msgs（消息定义）和你的控制代码的独立工作区。

Bash

# 1. 创建目录
mkdir -p ~/ws_test/src/
cd ~/ws_test/src/

# 2. 克隆 PX4 必备的消息定义包和桥接包
git clone https://github.com/PX4/px4_msgs.git
git clone https://github.com/PX4/px4_ros_com.git

# 3. 创建你的控制包 (Python)
ros2 pkg create --build-type ament_python drone_control_test --dependencies rclpy px4_msgs

第二步：编写键盘控制脚本

在包的源码目录下新建 Python 文件。

1. 进入目录：

   Bash

   cd ~/ws_test/src/drone_control_test/drone_control_test/

2. 新建并编辑文件 keyboard_control.py：

   Bash

   gedit keyboard_control.py
   # 使用 gedit 或 nano 编辑，例如：gedit keyboard_control.py

3. 复制粘贴以下完整代码：

   Python

   #!/usr/bin/env python3
   import rclpy
   from rclpy.node import Node
   from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
   from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleStatus
   
   import sys
   import select
   import termios
   import tty
   
   # === 操作说明 ===
   msg = """
   ---------------------------
      Reading from the keyboard
   ---------------------------
      w
   a  s  d
   
   w/s: 前进 / 后退 (X轴, 北/南)
   a/d: 向左 / 向右 (Y轴, 西/东)
   q:   降落 (Land)
   CTRL-C: 退出
   """
   
   class KeyboardControl(Node):
   
       def __init__(self):
           super().__init__('keyboard_control_node')
   
           # === QoS 设置 (必须匹配 PX4 设置) ===
           qos_profile = QoSProfile(
               reliability=ReliabilityPolicy.BEST_EFFORT,
               durability=DurabilityPolicy.TRANSIENT_LOCAL,
               history=HistoryPolicy.KEEP_LAST,
               depth=1
           )
   
           # === 发布者 ===
           self.offboard_control_mode_publisher_ = self.create_publisher(
               OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
           self.trajectory_setpoint_publisher_ = self.create_publisher(
               TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
           self.vehicle_command_publisher_ = self.create_publisher(
               VehicleCommand, '/fmu/in/vehicle_command', qos_profile)
   
           self.vehicle_status_subscriber_ = self.create_subscription(
               VehicleStatus, '/fmu/out/vehicle_status', self.vehicle_status_callback, qos_profile)
   
           # === 状态变量 ===
           self.vehicle_status = VehicleStatus()
           self.offboard_setpoint_counter_ = 0
   
           # 初始目标位置 (NED坐标系: x北, y东, z下)
           self.x = 0.0
           self.y = 0.0
           self.z = -3.0 # 负数代表向上，即高度 3米
   
           # 步长 (每次按键移动 0.5 米)
           self.step = 0.5 
   
           # === 定时器 10Hz ===
           self.timer = self.create_timer(0.1, self.timer_callback)
   
       def vehicle_status_callback(self, msg):
           self.vehicle_status = msg
   
       def timer_callback(self):
           # 1. 发送心跳 (Offboard模式必须)
           self.publish_offboard_control_mode()
   
           # 2. 发送位置指令
           self.publish_trajectory_setpoint(self.x, self.y, self.z)
   
           # 3. 起飞逻辑 (发送满10次数据后尝试切模式+解锁)
           if self.offboard_setpoint_counter_ == 10:
               self.engage_offboard_mode()
               self.arm()
   
           if self.offboard_setpoint_counter_ < 11:
               self.offboard_setpoint_counter_ += 1
   
       # === 核心：更新位置的方法 ===
       def update_position(self, key):
           if key == 'w': # 前进 (North)
               self.x += self.step
               print(f"前进一步 (北): x={self.x:.1f}")
           elif key == 's': # 后退 (South)
               self.x -= self.step
               print(f"后退一步 (南): x={self.x:.1f}")
           elif key == 'a': # 向左 (West) -> NED坐标系中Y正为东
               self.y -= self.step
               print(f"向左一步 (西): y={self.y:.1f}")
           elif key == 'd': # 向右 (East)
               self.y += self.step
               print(f"向右一步 (东): y={self.y:.1f}")
           elif key == 'q': # 降落
               self.land()
               print("正在降落...")
   
       def publish_offboard_control_mode(self):
           msg = OffboardControlMode()
           msg.position = True
           msg.velocity = False
           msg.acceleration = False
           msg.attitude = False
           msg.body_rate = False
           msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
           self.offboard_control_mode_publisher_.publish(msg)
   
       def publish_trajectory_setpoint(self, x, y, z):
           msg = TrajectorySetpoint()
           msg.position = [x, y, z]
           msg.yaw = 0.0 # 机头朝北
           msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
           self.trajectory_setpoint_publisher_.publish(msg)
   
       def engage_offboard_mode(self):
           self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1., 6.)
           self.get_logger().info("切换到 Offboard 模式")
   
       def arm(self):
           self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
           self.get_logger().info("已解锁 (Armed)")
   
       def land(self):
           self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND)
           self.get_logger().info("发送降落指令")
   
       def publish_vehicle_command(self, command, param1=0.0, param2=0.0):
           msg = VehicleCommand()
           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 = int(self.get_clock().now().nanoseconds / 1000)
           self.vehicle_command_publisher_.publish(msg)
   
   # === 键盘监听辅助函数 (非阻塞) ===
   def getKey(settings):
       tty.setraw(sys.stdin.fileno())
       # select 检查输入，timeout=0 实现非阻塞
       rlist, _, _ = select.select([sys.stdin], [], [], 0)
       if rlist:
           key = sys.stdin.read(1)
       else:
           key = ''
       termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
       return key
   
   def main(args=None):
       rclpy.init(args=args)
       node = KeyboardControl()
   
       settings = termios.tcgetattr(sys.stdin)
   
       print(msg)
   
       try:
           while rclpy.ok():
               # 1. 快速处理一次 ROS 回调 (维持心跳)
               rclpy.spin_once(node, timeout_sec=0)
   
               # 2. 检查键盘输入
               key = getKey(settings)
               if key:
                   if key == '\x03': # CTRL+C
                       break
                   node.update_position(key)
   
       except Exception as e:
           print(e)
       finally:
           node.land()
           node.destroy_node()
           rclpy.shutdown()
           termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
   
   if __name__ == '__main__':
       main()

第三步：注册脚本到 setup.py

你需要告诉 ROS 2 这个脚本是一个可执行节点。

1. 编辑配置文件：

   Bash

   # 路径：~/ws_test/src/drone_control_test/setup.py
   cd ~/ws_test/src/drone_control_test/
   gedit setup.py

2. 找到 entry_points 部分，修改为：

   Python

   entry_points={
       'console_scripts': [
           'keyboard_control = drone_control_test.keyboard_control:main',
       ],
   },

第四步：编译工作区

Bash

cd ~/ws_test
colcon build

(注意：如果是第一次编译，时间可能会稍长，因为要编译 px4_msgs)

---

最后一步：运行起飞！

保持终端 1 和 终端 2 正常运行中，在 终端 3 执行：

Bash

# 1. 加载环境变量 (这步非常重要！)
source ~/ws_test/install/setup.bash

# 2. 运行节点
ros2 run drone_control_test keyboard_control

操作指南：

1. 程序运行后，你会看到 "Reading from the keyboard" 菜单。

2. 稍等 1-2 秒，终端日志会显示 "Armed" 和 "Switching to Offboard mode"。

3. Gazebo 画面中：无人机会自动起飞并悬停在 3 米高度。

4. 控制：

   • 按 W / S：无人机向前/向后飞。

   • 按 A / D：无人机向左/向右飞。

   • 按 Q：无人机执行降落。

   • 按 Ctrl+C：退出程序（无人机会自动触发降落保护）。