ArduPilot 与 ROS 2 集成实战：基于 AP_DDS 库的 DDS/micro-ROS 测试、控制与二次开发指南

ArduPilot 与 ROS 2 集成实战基于 AP_DDS 库的 DDS/micro-ROS 测试、控制与二次开发指南【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilotAP_DDS 是 ArduPilot 内置的 DDSData Distribution Service客户端库它让 ArduPlane、ArduCopter、ArduRover、ArduSub 等飞控固件能以原生 DDS 的方式接入 ROS 2 生态在 SITL 仿真中通过 UDP 或串口与 Micro XRCE Agent 通信将传感器数据、姿态、时钟等以标准 ROS 2 topic 发布并接收/ap/joy、/ap/cmd_gps_pose等指令 topic 与arm_motors、mode_switch等服务调用。读完本文你将掌握 DDS 测试环境搭建、SITL 参数配置、UDP/串口双通道联调、ROS 2 CLI 观测与控制、时钟同步以及如何为 AP_DDS 添加新的 IDL 消息与服务映射。一、系统架构从 ArduPilot SITL 到 ROS 2 的数据通路ArduPilot 仓库中包含了 DDS 客户端库位于libraries/AP_DDS/它可以作为 SITL 的一部分运行。在 DDS 应用侧则由一个 ROS 2 节点、一个 eProsima Integration Service早期版本需要用于把 DDS 的 request/reply topic 映射为 ROS 2 service现已不再必需以及一个 Micro XRCE Agent 组成。两侧系统通过串口或 UDP 进行通信。UDP 回环模式推荐用于 SITL的拓扑如下硬件串口回环模式常用于真机或串口联调的拓扑如下从源码结构看DDS 客户端的传输层被抽象为可插拔的两套实现libraries/AP_DDS/AP_DDS_UDP.cpp实现 UDP 传输仅 SITL 可用libraries/AP_DDS/AP_DDS_Serial.cpp实现串口传输。两者都通过uxr_set_custom_transport_callbacks注册 open/close/write/read 四个回调来对接 eProsima Micro XRCE DDS Clientuxr/client/client.h。UDP 传输是非分帧non-framed的直接使用SocketAPM建立 UDP socket 收发串口传输是分帧framed的读写依赖AP_SerialManager分配到的 UART 端口读操作以 100 微秒的轮询间隔等待数据到达见AP_DDS_Serial.cpp中的serial_transport_read注释也标明该轮询方式对速率有上限是后续可优化的点。在编译开关层面libraries/AP_DDS/AP_DDS_config.h通过AP_DDS_ENABLED统一控制整个库的编译默认 1并细分出AP_DDS_UDP_ENABLEDUDP 仅在 SITL 使能依赖AP_NETWORKING_ENABLED、AP_DDS_NAVSATFIX_PUB_ENABLED依赖AP_GPS_ENABLED、AP_DDS_STATIC_TF_PUB_ENABLED依赖 GPS、AP_DDS_CLOCK_SUB_ENABLED仅 SITL 使能等众多按功能拆分的开关各发布/订阅/服务模块可以独立裁剪。二、环境安装与前置准备虽然 ArduPilot 对 DDS 的支持大部分通过 git 子模块如 Micro-CDR、Micro-XRCE-DDS-Client、mavlink 等见仓库根目录modules/下的子模块目录提供但仍有以下两件事必须手动完成安装Micro XRCE DDS Gen用于从 IDL 生成代码创建 ROS 2 工作区workspace。具体的环境搭建步骤请参考 ArduPilot 官方 wiki 的 ROS 2 章节README 中指向 https://ardupilot.org/dev/docs/ros2.html 进行设置。仅串口场景为 SITL 创建虚拟串口在 Linux 上要让 SITL 使用串口进行 DDS 通信需要先创建虚拟串口对因此必须安装 socatsudo apt-get update sudo apt-get install socat三、配置 SITL 启用 DDS在配置 SITLTools/autotest/sim_vehicle.py驱动时核心参数如下表所示| 参数名 | 说明 | 默认值 | | - | - | - | | DDS_ENABLE | 设为 1 启用 DDS设为 0 禁用 | 1 | | DDS_USE_NS | 设为 1 时在 topic/service 名称中加入vMAV_SYSID段 | 0 | | SERIAL1_BAUD | DDS 使用的串口波特率 | 57 | | SERIAL1_PROTOCOL | 设为 45 以在串口上使用 DDS | 0 |启动 SITL 的命令如下UDP 场景下唯一必须设置的参数就是DDS_ENABLE# Wipe params till you see AP: ArduPilot Ready # Select your favorite vehicle type sim_vehicle.py -w -v ArduPlane --console -DG --enable-DDS # Only set this for Serial, which means 115200 baud param set SERIAL1_BAUD 115 # See libraries/AP_SerialManager/AP_SerialManager.h AP_SerialManager SerialProtocol_DDS_XRCE param set SERIAL1_PROTOCOL 45SERIAL1_PROTOCOL的取值 45 对应AP_SerialManager中的SerialProtocol_DDS_XRCE协议枚举SITL 侧通过sim_vehicle.py -A --serial1uart:...把该串口重定向到虚拟串口。DDS 目前在参与构建的固件中默认启用。如需关闭执行以下命令并重启模拟器param set DDS_ENABLE 0 REBOOT源码视角DDS 相关参数的完整集合除了 README 中列出的四个参数libraries/AP_DDS/AP_DDS_Client.cpp的var_info[]中还定义了更多可调参数均以DDS_前缀暴露便于理解整个 DDS 子系统的行为| 参数名 | 说明 | 默认值 | 取值范围 | | - | - | - | - | | DDS_ENABLE | 启用/禁用 DDS 子系统AP_PARAM_FLAG_ENABLE重启生效 | 1ENABLED_BY_DEFAULT | 0/1 | | DDS_UDP_PORT | DDS 使用的 UDP 端口号 | 2019 | 1–65535 | | DDS_IP | UDP 目标 IP子组映射到AP_Networking_IPV4 | — | — | | DDS_DOMAIN_ID | 设置 ROS_DOMAIN_ID | 0 | 0–232 | | DDS_TIMEOUT_MS | XRCE Agent ping 超时时间毫秒 | 1000 | 1–10000 | | DDS_MAX_RETRY | ping Agent 的最大重试次数0 表示无限重试 | 10 | 0–100 | | DDS_USE_NS | 话题/服务名是否包含vMAV_SYSID段 | 0 | 0/1 |这些参数与libraries/AP_DDS/AP_DDS_Client.h中的成员一一对应例如udp.port、domain_id、ping_timeout_ms、ping_max_retry、use_ns。命名空间开启后所有话题名会变成/ap/vMAV_SYSID/topic的形式例如时钟话题变为/ap/vN/clock。四、启动并连接UDP 与 Serial 两种传输方式环境就绪后的通用步骤如下Source ROS 2 安装环境source install/setup.bash然后按下述小节选择对应传输方式启动最后即可使用 ROS 2 CLI。UDPSITL 推荐# 启动 microROS agent cd ardupilot/libraries/AP_DDS ros2 run micro_ros_agent micro_ros_agent udp4 -p 2019# 启动 SITL务必先杀掉之前残留的 ardupilot SITL 终端进程 sim_vehicle.py -v ArduPlane -DG --console --enable-DDSSerial# 用 socat 创建虚拟串口对记录输出的两个 /dev/pts/* 端口若与你机器上的不同按需替换 socat -d -d pty,raw,echo0 pty,raw,echo0 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/1 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/2 2023/02/21 05:26:06 socat[334] N starting data transfer loop with FDs [5,5] and [7,7]# 启动 microROS agent假设 DDS 应用侧使用 /dev/pts/2 cd ardupilot/libraries/AP_DDS ros2 run micro_ros_agent micro_ros_agent serial -b 115200 -D /dev/pts/2# 启动 SITL假设 ArduPilot SITL 侧使用 /dev/pts/1同样先杀掉残留 SITL 进程 sim_vehicle.py -v ArduPlane -DG --console --enable-DDS -A --serial1uart:/dev/pts/1五、使用 ROS 2 CLI 观测 ArduPilot 数据连接建立后即可看到 DDS 节点并检查数据输出。$ ros2 node list /ardupilot_dds根据编译配置不同你将看到类似如下的 topic 列表$ ros2 topic list -v Published topics: * /ap/airspeed [ardupilot_msgs/msg/Airspeed] 1 publisher * /ap/battery [sensor_msgs/msg/BatteryState] 1 publisher * /ap/clock [rosgraph_msgs/msg/Clock] 1 publisher * /ap/geopose/filtered [geographic_msgs/msg/GeoPoseStamped] 1 publisher * /ap/gps_global_origin/filtered [geographic_msgs/msg/GeoPointStamped] 1 publisher * /ap/imu/experimental/data [sensor_msgs/msg/Imu] 1 publisher * /ap/navsat [sensor_msgs/msg/NavSatFix] 1 publisher * /ap/pose/filtered [geometry_msgs/msg/PoseStamped] 1 publisher * /ap/rc [ardupilot_msgs/msg/Rc] 1 publisher * /ap/status [ardupilot_msgs/msg/Status] 1 publisher * /ap/tf_static [tf2_msgs/msg/TFMessage] 1 publisher * /ap/time [builtin_interfaces/msg/Time] 1 publisher * /ap/twist/filtered [geometry_msgs/msg/TwistStamped] 1 publisher * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher * /rosout [rcl_interfaces/msg/Log] 1 publisher Subscribed topics: * /ap/cmd_gps_pose [ardupilot_msgs/msg/GlobalPosition] 1 subscriber * /ap/cmd_vel [geometry_msgs/msg/TwistStamped] 1 subscriber * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber * /clock [rosgraph_msgs/msg/Clock] 1 subscriber这些 topic 正是libraries/AP_DDS/AP_DDS_Topic_Table.h中AP_DDS_Client::topics[]表定义的集合每条记录都声明了 DataWriter/DataReader 方向、DDS 类型名与 QoS 策略。例如/ap/navsatsensor_msgs::msg::dds_::NavSatFix_与/ap/batterysensor_msgs::msg::dds_::BatteryState_使用BEST_EFFORT可靠性与 KEEP_LAST depth 5而/ap/time、/ap/clock使用RELIABLE可靠性、depth 20/ap/status、/ap/tf_static、/ap/goal_lla则使用TRANSIENT_LOCAL持久化策略并仅保留 1 条历史确保迟到的订阅者也能拿到最新值。查看发布频率与内容$ ros2 topic hz /ap/time average rate: 50.115 min: 0.012s max: 0.024s std dev: 0.00328s window: 52$ ros2 topic echo /ap/time sec: 1678668735 nanosec: 729410000查看可用服务$ ros2 service list /ap/arm_motors /ap/mode_switch /ap/prearm_check /ap/experimental/takeoff ---已启用传感器的静态坐标变换static transforms也会发布可用如下命令接收注意其 QoS 为 transient_local需要显式指定ros2 topic echo /ap/tf_static --qos-depth 1 --qos-history keep_last --qos-reliability reliable --qos-durability transient_local --once要消费这些变换强烈建议在 ROS 2 侧创建并运行一个 tf2 变换广播器transform broadcaster参考 ROS 2 官方 tf2 概念文档中的教程。六、通过 ROS 2 Services 控制飞控AP_DDS库暴露的服务会自动映射为 ROS 2 service并遵循 ROS 2 对 topic 与消息/服务类型的命名规范。早期版本需要借助 eProsima Integration Service 把 DDS 的 request/reply topic 映射到 ROS 2 service现在已不再需要。列出可用服务及其类型$ ros2 service list -t /ap/arm_motors [ardupilot_msgs/srv/ArmMotors] /ap/mode_switch [ardupilot_msgs/srv/ModeSwitch] /ap/prearm_check [std_srvs/srv/Trigger] /ap/experimental/takeoff [ardupilot_msgs/srv/Takeoff]调用解锁电机服务$ ros2 service call /ap/arm_motors ardupilot_msgs/srv/ArmMotors {arm: True} requester: making request: ardupilot_msgs.srv.ArmMotors_Request(armTrue) response: ardupilot_msgs.srv.ArmMotors_Response(resultTrue)调用模式切换服务mode 4 对应 GUIDED$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch {mode: 4} requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode4) response: ardupilot_msgs.srv.ModeSwitch_Response(statusTrue, curr_mode4)调用预解锁检查服务$ ros2 service call /ap/prearm_check std_srvs/srv/Trigger requester: making request: std_srvs.srv.Trigger_Request() response: std_srvs.srv.Trigger_Response(successFalse, messageVehicle is Not Armable) or std_srvs.srv.Trigger_Response(successTrue, messageVehicle is Armable)调用起飞服务$ ros2 service call /ap/experimental/takeoff ardupilot_msgs/srv/Takeoff {alt: 10.5} requester: making request: ardupilot_msgs.srv.Takeoff_Request(alt10.5) response: ardupilot_msgs.srv.Takeoff_Response(statusTrue)服务侧的实现同样位于libraries/AP_DDS/AP_DDS_Service_Table.h的AP_DDS_Client::services[]表中。除上述四个服务外表中还注册了参数服务set_parametersService、get_parameterService类型为rcl_interfaces::srv::dds_::SetParameters_/GetParameters_由AP_DDS_PARAMETER_SERVER_ENABLED编译开关控制允许通过 ROS 2 参数接口读写 ArduPilot 参数。七、通过 ROS 2 Topics 下发指令以下两个 topic 可用于控制飞行器/ap/joy类型sensor_msgs/msg/Joy最多覆盖 8 个 RC 通道至少需要发送 4 个轴。数值会被钳制在 -1.0 到 1.0 之间对某个通道发送NaN可禁用该通道的覆盖若 1 秒内未收到新指令通道会自动回落到 RC 输入。ros2 topic pub /ap/joy sensor_msgs/msg/Joy {axes: [0.0, 0.0, 0.0, 0.0]} publisher: beginning loop publishing #1: sensor_msgs.msg.Joy(headerstd_msgs.msg.Header(stampbuiltin_interfaces.msg.Time(sec0, nanosec0), frame_id), axes[0.0, 0.0, 0.0, 0.0], buttons[])/ap/cmd_gps_pose类型ardupilot_msgs/msg/GlobalPosition当飞行器处于 GUIDED 模式时发送一个目标航点让飞行器飞往。ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition {latitude: 34, longitude: 118, altitude: 1000} publisher: beginning loop publishing #1: ardupilot_msgs.msg.GlobalPosition(headerstd_msgs.msg.Header(stampbuiltin_interfaces.msg.Time(sec0, nanosec0), frame_id), coordinate_frame0, type_mask0, latitude34.0, longitude118.0, altitude1000.0, velocitygeometry_msgs.msg.Twist(lineargeometry_msgs.msg.Vector3(x0.0, y0.0, z0.0), angulargeometry_msgs.msg.Vector3(x0.0, y0.0, z0.0)), acceleration_or_forcegeometry_msgs.msg.Twist(lineargeometry_msgs.msg.Vector3(x0.0, y0.0, z0.0), angulargeometry_msgs.msg.Vector3(x0.0, y0.0, z0.0)), yaw0.0)从源码看这两个控制入口分别对应AP_DDS_Topic_Table.h中的VELOCITY_CONTROL_SUBtopiccmd_vel与GLOBAL_POSITION_SUBtopiccmd_gps_pose其中cmd_vel依赖AP_DDS_VEL_CTRL_ENABLEDcmd_gps_pose依赖AP_DDS_GLOBAL_POS_CTRL_ENABLED。此外话题表还包含JOY_SUB与DYNAMIC_TRANSFORMS_SUB接收/ap/tf订阅数据由AP_DDS_Client.cpp中对应的静态接收缓存如rx_joy_topic、rx_velocity_control_topic、rx_global_position_control_topic承载并最终经AP_DDS_ExternalControl等模块接入飞控控制链路。八、时钟同步Clock Synchronisation在仿真环境中主仿真时钟通常是/clock需要提供给 ArduPilot以确保 ArduPilot 发布的 topic 带有正确的时间戳这样 ROS 2 侧的传感器融合sensor fusion类节点才能融合到正确时序的数据。ArduPilot SITL 使用标准 ROS 2 参数--use-sim-time true|false。若设为trueArduPilot 会自动订阅/clocktopic 并以此作为 DDS topic 的时间戳来源如果订阅失败则回退使用自身内部时钟。无论哪种情况ArduPilot 都会把时钟发布到/ap/clock若开启命名空间则为/ap/vN/clock。源码层面时钟订阅由AP_DDS_CLOCK_SUB_ENABLED控制该开关仅在HAL_BOARD_SITL下默认使能对应AP_DDS_Topic_Table.h中的CLOCK_SUB条目类型为rosgraph_msgs::msg::dds_::Clock_而AP_DDS_Client.cpp中external_clock_time与has_received_clock静态成员记录了外部时钟的接收状态作为时间戳回退判定的依据。九、为 AP_DDS 库贡献新消息与服务添加 DDS 消息到 ArduPilot与使用 ROS 2 的.msg文件不同由于 ArduPilot 支持原生 DDS消息文件遵循 OMG IDL DDS v4.2 规范.idl文件。该库设计上兼容任何符合这些扩展的.idl文件未来这些限制有望逐步放开。从 ROS 2 引入一个新的 IDL 文件流程如下以下命令以 ROS 2 Jazzy 为例若使用 ROS 2 Humble将jazzy替换为humble即可cd ardupilot source /opt/ros/jazzy/setup.bash # 找到 IDL 文件 find /opt/ros/$ROS_DISTRO -type f -wholename \*builtin_interfaces/msg/Time.idl # 在源码树中创建与 ros 目录结构一致的目录 mkdir -p libraries/AP_DDS/Idl/builtin_interfaces/msg/ # 复制 IDL 文件 cp /opt/ros/jazzy/share/builtin_interfaces/msg/Time.idl libraries/AP_DDS/Idl/builtin_interfaces/msg/ # 按上文所述用 --enable-DDS 重新构建如果该消息是 ArduPilot 自定义消息则先在Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg中创建 ROS 消息当前仓库Tools/ros2/ardupilot_msgs/msg/下已有Airspeed.msg、GlobalPosition.msg、Rc.msg、Status.msg四个自定义消息然后用 colcon 构建ardupilot_msgs最后把 install 目录中的 IDL 文件夹复制到源码树中即libraries/AP_DDS/Idl/下对应的ardupilot_msgs/msg/、ardupilot_msgs/srv/等目录。仓库中现有的libraries/AP_DDS/Idl/目录结构ardupilot_msgs、builtin_interfaces、geographic_msgs、geometry_msgs、sensor_msgs、std_msgs、std_srvs、tf2_msgs、rosgraph_msgs、rcl_interfaces等就是该流程的直接产物。添加 topic 和 service 的命名规则AP_DDS暴露的 topic 与服务会自动映射到 ROS 2前提是遵循以下在 topic/service 表中定义条目时的规则。ROS 2 消息与服务接口类型的映射manglingROS 2 消息与接口定义在从 ROS 2 映射到 DDS 时会被rosidl_adapter进行名称重整mangle以避免 C/C 库中的命名冲突ROS 2 对象namespace::Struct会映射为 DDS 侧的namespace::dds_::Struct_。示例映射如下ROS 2DDSrosgraph_msgs::msg::Clockrosgraph_msgs::msg::dds_::Clock_sensor_msgs::msg::NavSatFixsensor_msgs::msg::dds_::NavSatFix_ardupilot_msgs::srv::ArmMotors_Requestardupilot_msgs::srv::dds_::ArmMotors_Request_ardupilot_msgs::srv::ArmMotors_Responseardupilot_msgs::srv::dds_::ArmMotors_Response_注意服务接口总是需要一个 Request / Response 配对。ROS 2 topic 与 service 名称映射到 DDSROS 2 的 topic 与服务名称映射到 DDS 时每个 ROS 2 子系统都会被分配一个前缀服务的 request/response 配对还需要额外的后缀ROS 2 子系统DDS 前缀DDS 后缀topicsrt/service requestrq/Requestservice responserr/Replyservicers/parameterrp/actionra/示例映射ROS 2DDSap/clockrt/ap/clockap/navsatrt/ap/navsatap/arm_motorsrq/ap/arm_motorsRequest, rr/ap/arm_motorsReply这些规则在libraries/AP_DDS/AP_DDS_Topic_Table.h与libraries/AP_DDS/AP_DDS_Service_Table.h中有完整的落地体现例如服务表中arm_motorsService的request_topic_name为arm_motorsRequest、reply_topic_name为arm_motorsReply类型为ardupilot_msgs::srv::dds_::ArmMotors_Request_/_Response_experimental/takeoffService则对应experimental/takeoffRequest与experimental/takeoffReply。新增条目时参照这两张表的既有模式即可。开发规范要求AP_DDS 中的 C 代码使用 Astyle 格式化这是 CI 通过构建的硬性要求。运行自动化格式化工具./Tools/scripts/run_astyle.pypre-commit 用于处理 Python、XML 等代码的格式化。安装后会在提交时自动运行若有改动将其加回暂存区再次提交即可。安装 pre-commit Python 包在仓库根目录安装 ArduPilot 的 hooks然后正常提交cd ardupilot pre-commit install git commit十、在真实硬件上测试 DDS串口方式测试 DDS 最简单的方式是使用像 Pixhawk 6X 这样通过 USB 提供两个串行接口的板卡。该信息记录在libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat中SERIAL_ORDER OTG1 UART7 UART5 USART1 UART8 USART2 UART4 USART3 OTG2例如构建、烧录并将 OTG2 配置为 DDS 端口./waf configure --board Pixhawk6X --enable-DDS ./waf plane --upload mavproxy.py --console param set DDS_ENABLE 1 # Check the hwdef file for which port is OTG2 param set SERIAL8_PROTOCOL 45 param set SERIAL8_BAUD 115 reboot然后运行 Micro ROS agentcd /path/to/ros2_ws source install/setup.bash cd src/ardupilot/libraries/AP_DDS ros2 run micro_ros_agent micro_ros_agent serial -b 115200 -D /dev/serial/by-id/usb-ArduPilot_Pixhawk6X_210028000151323131373139-if02如果连接失败不运行 Micro ROS agent而是先调试数据流python3 -m serial.tools.miniterm /dev/serial/by-id/usb-ArduPilot_Pixhawk6X_210028000151323131373139-if02 115200 --echo --encoding hexlify待上述 USB 串口链路验证通过后同样的步骤可以迁移到物理串口从而把软件与硬件问题隔离开来。十一、常见问题与调试建议端口占用切换传输方式或重复启动前务必先杀掉之前运行的 ardupilot SITL 进程否则会导致端口UDP 2019 或串口被占用而无法建立连接。时钟不同步导致数据乱序仿真场景请为 SITL 传入--use-sim-time true并确认/clock话题持续发布可用ros2 topic hz /clock验证否则 ArduPilot 会回退到内部时钟。看不到话题/服务检查固件是否以--enable-DDS编译且DDS_ENABLE为 1同时确认 ROS 2 与 Agent 的ROS_DOMAIN_ID对应DDS_DOMAIN_ID参数一致。串口联调不通用上文 miniterm 的 hexlify 回显方式直接查看串口字节流确认 Agent 与飞控两侧的波特率115200、协议SERIAL*_PROTOCOL45与端口指向是否匹配。通过本文的流程你可以在 SITL 或 Pixhawk 6X 等真实硬件上建立 ArduPilot 与 ROS 2 的完整数据链路既可以用 ROS 2 CLI 实时观测遥测与时钟也可以通过 topic 与服务下发解锁、模式切换、航点与起飞指令更可以参照命名规则向libraries/AP_DDS/添加自定义 IDL 消息把 ArduPilot 无缝接入自己的 ROS 2 机器人软件栈。【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考