前言
- 最近 VLA（Vision-Language-Action，视觉-语言-动作）具身智能 火得不行，从谷歌的
RT-2、OpenVLA到π0，大模型开始直接输出机器人动作，而这一切的物理载体，正是机械臂 - 所以接下来这个系列，我们将逐步上手最近大火的 VLA 具身智能机械臂 ，计划从 机械臂基础 →
MoveIt2+Gazebo仿真 →Pinocchio刚体动力学 → VLA 入门一路推进 - 往期内容：
- 回到本期：上一期我们把
MoveIt2的骨架搭了起来------move_group、MoveItConfigsBuilder、panda的 SRDF 与规划管线，并在 RViz 里验证了运动规划 - 但上一期动的其实是"假"机械臂：轨迹只是在 RViz 里回放动画，关节背后没有真实的物理在推动
- 本期我们将要：在
Gazebo里用ros2_control真正驱动 panda，给它装上腕部深度相机，把点云喂给MoveIt的octomap，在抓取场景里绕障规划并到达目标点
文章目录
-
- 前言
- [1 Gazebo与机械臂](#1 Gazebo与机械臂)
-
-
- [1-1 介绍](#1-1 介绍)
- [1-2 panda 的 urdf 转 sdf](#1-2 panda 的 urdf 转 sdf)
- [1-3 包目录结构](#1-3 包目录结构)
- [1-4 关键文件源码](#1-4 关键文件源码)
-
- [1-4-1 主 URDF](#1-4-1 主 URDF)
- [1-4-2 主臂 ros2_control 宏](#1-4-2 主臂 ros2_control 宏)
- [1-4-3 夹爪 ros2_control 宏](#1-4-3 夹爪 ros2_control 宏)
- [1-4-4 控制器配置](#1-4-4 控制器配置)
- [1-4-5 初始关节角](#1-4-5 初始关节角)
- [1-4-6 构建配置 CMakeLists](#1-4-6 构建配置 CMakeLists)
- [1-5 launch](#1-5 launch)
- [1-6 一键启动](#1-6 一键启动)
-
- [2 编写自己的节点arm_control_demo.cpp](#2 编写自己的节点arm_control_demo.cpp)
-
-
- [2-1 流程](#2-1 流程)
- [2-2 功能包创建](#2-2 功能包创建)
- [2-3 思路](#2-3 思路)
- [2-4 源码](#2-4 源码)
- [2-5 启动与发点脚本](#2-5 启动与发点脚本)
-
- [3 深度相机插件与机械臂](#3 深度相机插件与机械臂)
-
-
- [3-1 末端执行器的深度相机](#3-1 末端执行器的深度相机)
- [3-2 手眼标定](#3-2 手眼标定)
- [3-3 插件配置以及注意事项](#3-3 插件配置以及注意事项)
- [3-4 对接moveit2的深度图像输入](#3-4 对接moveit2的深度图像输入)
-
- [3-4-1 QoS 对齐](#3-4-1 QoS 对齐)
- [3-4-2 最小测量范围](#3-4-2 最小测量范围)
- [3-5 launch启动文件](#3-5 launch启动文件)
- [3-6 添加桌子](#3-6 添加桌子)
- [3-7 测试](#3-7 测试)
-
- 总结
1 Gazebo与机械臂
1-1 介绍
- 先认识一下
Gazebo：一个开源物理仿真器，负责给机器人提供一个"真实世界"------重力、碰撞、接触、传感器噪声都在这层模拟 - 它和
MoveIt2是怎么对接的？拆开看就是三块各司其职：Gazebo：提供物理世界（重力、碰撞、接触），也是模型最终的执行环境gazebo_ros2_control：插件跑在 gzserver 进程内，把 URDF 里声明的 ros2_control 描述翻译成controller_manager，加载并激活控制器MoveIt：move_group规划出轨迹，通过控制器下发关节位置，再从/joint_states读回真实反馈
- 一句话总结：MoveIt 负责"想"，Gazebo 负责"动"，ros2_control 负责"传话"
说人话：Gazebo 当机械臂的"身体"、ros2_control 当"神经"、MoveIt 当"大脑"------上一期只有大脑在空想（RViz 回放动画），本期让三者连起来，机械臂才真正动起来
1-2 panda 的 urdf 转 sdf
URDF是 MoveIt 全家的语言(描述 link/joint/惯性/碰撞)，而 Gazebo classic 原生只认 SDF- 两个格式的关系：SDF 是"整个世界"的格式(除了机器人本体，还包含灯光、传感器、物理引擎参数)，URDF 是它的一个子集、偏机器人本体
- 所以 panda 想进 Gazebo，先得把 URDF "翻译"成 SDF，官方给了两条路：
- 在线转换：浏览器打开
gazebosim.org的 URDF 转换页面，传 URDF 下载 SDF - 命令行：
gz sdf -p 你的.urdf直接输出转换结果
- 在线转换：浏览器打开
- 拿本项目用的官方精简版
panda.urdf实测一下------注意它一个<inertial>都没有：
bash
# 确认一下：12 个 link 全部没有惯量
grep -c "<inertial" /opt/ros/humble/share/moveit_resources_panda_description/urdf/panda.urdf # 0
# 命令行转换，结果打印到终端
gz sdf -p /opt/ros/humble/share/moveit_resources_panda_description/urdf/panda.urdf
# 输出只有 <sdf><model name='panda'/></sdf> ------ 12 个 link 全被丢弃，一个不剩- 这就是坑 1 的现场：缺惯量的 link 在转换时被整体丢弃，一个不剩
- 本项目其实不手动转 ：
spawn_entity喂给 gzserver 的 URDF，会在加载时由sdformat自动转成 SDF - 但这条"自动翻译"路上埋了三个坑，正是后面
gazebo.launch.py里几段修复要处理的：- 没有
<inertial>的 link 会被整体丢弃(连带它的关节一起消失) - 网格路径用
package://时，Gazebo classic 不解析 - URDF 里的
.dae网格在 OGRE 渲染下是黑的(材质丢失)
- 没有
说人话：URDF 是机器人的"简历"，SDF 是"世界户口本"。Gazebo 只认户口本，简历虽然能自动翻译，但翻译老容易漏字，得人工兜底
1-3 包目录结构
- 本节我们将构建功能包
panda_gazebo_bringup，目录结构如下（编译产物__pycache__等已省略）：
bash
src/panda_gazebo_bringup/
├── CMakeLists.txt # 构建配置：把 config/launch 安装进 share 目录
├── package.xml # 包元信息与依赖声明
├── config/
│ ├── initial_positions.yaml # 机械臂初始关节角（一进 Gazebo 就摆好的姿势）
│ ├── panda_gazebo.urdf.xacro # 主 URDF：复用系统 panda.urdf + 自建 ros2_control
│ ├── panda.ros2_control.xacro # 主臂 7 关节的 ros2_control 描述（mock/isaac/gazebo 三分支）
│ ├── panda_hand.ros2_control.xacro # 夹爪 1 关节的 ros2_control 描述
│ └── ros2_controllers.yaml # controller_manager + 臂/夹爪/状态广播三个控制器
└── launch/
└── gazebo.launch.py # 基线启动：无相机、无 octomap（本章主角）- 对号入座，和前面章节对上：
- 1-2 挖的那三个坑（丢 link、
package://、渲染黑），就是 1-5 里_fix_*后处理要填的，逻辑都在gazebo.launch.py - 1-4 的
ros2_controllers.yaml是控制器的"总装配表"，spawner按它声明的名字加载并激活 initial_positions.yaml决定机械臂进 Gazebo 后摆什么姿势
- 1-2 挖的那三个坑（丢 link、
- 相机、场景相关的文件（
gazebo_camera.launch.py、pick_place.world、moveit_camera.rviz）和第三章绑定，等讲到深度相机时再逐个展开
说人话：这个包像一本菜谱------launch 是"做菜步骤"，xacro/yaml 是"食材配方"。本章先把"机械臂 + 控制器"这道基础菜端上来，相机和场景是第三章的加菜
1-4 关键文件源码
- 六份核心配置按依赖顺序拆成 1-4-1~1-4-6 逐份展开：先是主 URDF，再是它 include 的两个 ros2_control 宏，接着是插件和宏引用的两个 yaml，最后是构建规则 CMakeLists
1-4-1 主 URDF
- 文件：
config/panda_gazebo.urdf.xacro。它是整条链路的入口，1-5 的 launch 直接加载它，做三件事：复用系统panda.urdf（几何/惯量/关节/网格）、include 两个 ros2_control 宏并实例化、声明gazebo_ros2_control插件块 - 三个
xacro:arg参数含义：initial_positions_file：初始关节角文件路径，默认initial_positions.yaml（1-4-5）ros2_control_hardware_type：硬件后端类型，默认gazebo；宏里还预留mock_components/isaac，本章只用gazeboenable_camera：是否挂腕部 RGBD 相机，默认false；相机块整段是<xacro:if>包着的，和第三章 3-3 绑定，这里先留空位
- 完整源码：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
<xacro:arg name="ros2_control_hardware_type" default="gazebo" />
<xacro:arg name="enable_camera" default="false" />
<!-- 1) 复用系统包基础 URDF（几何/惯量/关节/网格），不修改系统文件 -->
<xacro:include filename="$(find moveit_resources_panda_description)/urdf/panda.urdf" />
<!-- 2) 自建 ros2_control 描述：在系统版本基础上新增 gazebo 硬件分支 -->
<xacro:include filename="panda.ros2_control.xacro" />
<xacro:include filename="panda_hand.ros2_control.xacro" />
<xacro:panda_ros2_control name="PandaGazeboSystem"
initial_positions_file="$(arg initial_positions_file)"
ros2_control_hardware_type="$(arg ros2_control_hardware_type)"/>
<xacro:panda_hand_ros2_control name="PandaHandGazeboSystem"
ros2_control_hardware_type="$(arg ros2_control_hardware_type)"/>
<!-- 3) gazebo_ros2_control 插件块：模型被 spawn 后插件加载，
controller_manager 直接运行在 gzserver 进程内（无需独立 ros2_control_node）。
Humble 的插件完整支持 parameters 标签（会注入为参数文件） -->
<gazebo>
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
<parameters>$(find panda_gazebo_bringup)/config/ros2_controllers.yaml</parameters>
</plugin>
</gazebo>
<!-- 4) 可选 RGBD 深度相机（眼在手）块：enable_camera=true 时挂腕部，
是第三章 3-3 的内容，讲到时再补全 -->
</robot>1-4-2 主臂 ros2_control 宏
- 文件：
config/panda.ros2_control.xacro。声明panda_ros2_control宏，1-4-1 include 后实例化为PandaGazeboSystem，描述主臂 7 个关节的硬件接口 - 宏参数含义：
name：<ros2_control name="${name}">的实例名，插件按它区分硬件块initial_positions_file：初始关节角文件，宏里用${xacro.load_yaml(initial_positions_file)['initial_positions']}读成 XML 属性，供各关节取initial_valueros2_control_hardware_type：选<hardware>分支------mock_components/isaac/gazebo，本章用gazebo对应gazebo_ros2_control/GazeboSystem
- 每个关节的接口含义：
command_interface name="position"：控制器下发的命令接口（位置模式）state_interface（position / velocity）：插件回传的状态反馈接口；position 的initial_value取 yaml 里的初始角，velocity 置 0
- 完整源码：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="panda_ros2_control" params="name initial_positions_file ros2_control_hardware_type">
<xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
<ros2_control name="${name}" type="system">
<hardware>
<xacro:if value="${ros2_control_hardware_type == 'mock_components'}">
<plugin>mock_components/GenericSystem</plugin>
</xacro:if>
<xacro:if value="${ros2_control_hardware_type == 'isaac'}">
<plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
<param name="joint_commands_topic">/isaac_joint_commands</param>
<param name="joint_states_topic">/isaac_joint_states</param>
</xacro:if>
<xacro:if value="${ros2_control_hardware_type == 'gazebo'}">
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</xacro:if>
</hardware>
<joint name="panda_joint1">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['panda_joint1']}</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
<joint name="panda_joint2">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['panda_joint2']}</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
<joint name="panda_joint3">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['panda_joint3']}</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
<joint name="panda_joint4">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['panda_joint4']}</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
<joint name="panda_joint5">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['panda_joint5']}</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
<joint name="panda_joint6">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['panda_joint6']}</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
<joint name="panda_joint7">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['panda_joint7']}</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
</ros2_control>
</xacro:macro>
</robot>1-4-3 夹爪 ros2_control 宏
- 文件：
config/panda_hand.ros2_control.xacro。声明panda_hand_ros2_control宏，实例化为PandaHandGazeboSystem，只描述夹爪的 1 个关节panda_finger_joint1 - 为什么只声明 1 个：系统包的
panda_finger_joint2带<param name="mimic">联动，会让插件以不存在的假关节名panda_finger_joint2_mimic上报、MoveIt 状态监视器每周期刷 "Joint not found"，所以去掉（代价是夹爪不再联动，本 demo 不用夹爪） - 完整源码：
xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="panda_hand_ros2_control" params="name ros2_control_hardware_type">
<ros2_control name="${name}" type="system">
<hardware>
<xacro:if value="${ros2_control_hardware_type == 'mock_components'}">
<plugin>mock_components/GenericSystem</plugin>
</xacro:if>
<xacro:if value="${ros2_control_hardware_type == 'isaac'}">
<plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
<param name="joint_commands_topic">/isaac_joint_commands</param>
<param name="joint_states_topic">/isaac_joint_states</param>
</xacro:if>
<xacro:if value="${ros2_control_hardware_type == 'gazebo'}">
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</xacro:if>
</hardware>
<joint name="panda_finger_joint1">
<command_interface name="position" />
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
<joint name="panda_finger_joint2">
<!-- 原系统包有 <param name="mimic">panda_finger_joint1</param>，会让
gazebo_ros2_control 以 panda_finger_joint2_mimic 这个名字发布
接口 -> /joint_states 里出现模型里不存在的假关节 -> MoveIt 状态
监视器每周期刷 "Joint not found"。去掉后它用自己的本名上报，
与模型一致、无噪音（代价：夹爪不再联动，本 demo 不用夹爪）。 -->
<command_interface name="position" />
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0.0</param>
</state_interface>
</joint>
</ros2_control>
</xacro:macro>
</robot>1-4-4 控制器配置
- 文件：
config/ros2_controllers.yaml。控制器的"总装配表"：插件启动 controller_manager 后按它加载控制器，spawner再按名字激活 - 参数含义：
update_rate: 100：controller_manager 的更新频率（Hz）panda_arm_controller：主臂控制器，类型joint_trajectory_controller/JointTrajectoryController，joints列出 7 个关节，命令接口 position、状态接口 position+velocitypanda_hand_controller：夹爪控制器，类型position_controllers/GripperActionController，joint指定panda_finger_joint1joint_state_broadcaster：关节状态广播器，把状态发布到/joint_states
- 完整源码：
yaml
# This config file is used by ros2_control
controller_manager:
ros__parameters:
update_rate: 100 # Hz
panda_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
panda_hand_controller:
type: position_controllers/GripperActionController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
panda_arm_controller:
ros__parameters:
command_interfaces:
- position
state_interfaces:
- position
- velocity
joints:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
panda_hand_controller:
ros__parameters:
joint: panda_finger_joint11-4-5 初始关节角
- 文件：
config/initial_positions.yaml。机械臂一进 Gazebo 就摆好的姿势，被 1-4-2 宏的${xacro.load_yaml}读取 - 数值选在 ready 位姿附近：末端大致朝下、桌面落在相机视野里，方便第三章把相机照向桌面
- 完整源码：
yaml
# Default initial positions for the panda arm's ros2_control fake system
initial_positions:
panda_joint1: 0.0
panda_joint2: -0.785
panda_joint3: 0.0
panda_joint4: -2.356
panda_joint5: 0.0
panda_joint6: 1.571
panda_joint7: 0.7851-4-6 构建配置 CMakeLists
- 文件：
CMakeLists.txt。纯数据包：不编译任何代码，只把config/和launch/两个目录安装进share/${PROJECT_NAME}------运行时 launch 里的$(find panda_gazebo_bringup)就是从这里定位文件的 - 不能省略、也不能只当
ros2 pkg create的默认模板用：默认模板只声明ament_cmake，不会安装子目录 ，缺了下面这段install(DIRECTORY ...)，xacro/yaml/launch 根本装不进 share，ros2 launch直接报找不到包内容 - 完整源码：
cmake
cmake_minimum_required(VERSION 3.8)
project(panda_gazebo_bringup)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
# 纯数据包：只安装目录，全部用相对路径（规避中文路径风险）
install(DIRECTORY config launch
DESTINATION share/${PROJECT_NAME}
)
ament_package()- 六份文件加上 1-5 的
gazebo.launch.py，链路就齐了：launch 加载主 xacro → xacro include 两个宏、读initial_positions.yaml→ 插件按ros2_controllers.yaml在 gzserver 内拉起controller_manager
说人话：这六份是"食材配方"，1-5 的 launch 是"做菜步骤"。先把配方逐份抄好，下一步看步骤怎么做菜
1-5 launch
- 上面说了一堆坑，现在看怎么在 launch 里一次性兜底。整个文件其实就做三件事：
- 用
MoveItConfigsBuilder在进程内 eager 生成robot_description(URDF 字符串)，再经过一串_fix_*后处理把坑填平 - 起
move_group、robot_state_publisher、RViz，并把 panda 用spawn_entity插进 Gazebo - 等 spawn 成功退出(=controller_manager 已建立)，再用
spawner加载三个控制器
- 用
- 完整代码：
python
import os
import re
import shutil
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def _fix_package_mesh_uris(urdf: str) -> str:
"""Gazebo classic 不解析 package:// 网格 URI（会当成 model:// 找），
替换成 file:// 绝对路径，RViz / MoveIt 同样可用。"""
panda_desc = get_package_share_directory('moveit_resources_panda_description')
return urdf.replace('package://moveit_resources_panda_description', 'file://' + panda_desc)
# 真实 panda/FR3 惯量（kg*m^2，主对角线 + 质心偏移，取 7 自由度臂同架构量级）。
# 之前注入扁平默认值 mass=1.0 / inertia=0.01 比真实值轻太多，位置控制器（默认
# Kp=100）在轻惯量下形成极限环振荡 -> 关节速度发散 -> 位置变 NaN -> Gazebo 里
# 机械臂消失（RViz 用模型显示所以还在）。换成真实量级后物理稳定。
REAL_INERTIA = {
'panda_link0': (2.3966, '0 0 0', '0.0090', '0.0115', '0.0085'),
'panda_link1': (2.9275, '0 -0.0181 -0.0386', '0.0239', '0.0225', '0.0064'),
'panda_link2': (2.9355, '0.0032 -0.0743 0.0088', '0.0419', '0.0251', '0.0617'),
'panda_link3': (2.2449, '0.0407 -0.0048 -0.0290', '0.0241', '0.0197', '0.0190'),
'panda_link4': (2.6156, '-0.0459 0.0630 -0.0085', '0.0345', '0.0289', '0.0413'),
'panda_link5': (2.3271, '-0.0016 0.0293 -0.0973', '0.0516', '0.0479', '0.0164'),
'panda_link6': (1.8170, '0.0597 -0.0410 -0.0102', '0.0054', '0.0141', '0.0161'),
'panda_link7': (0.6271, '0.0045 0.0086 -0.0162', '0.0002', '0.0002', '0.0001'),
'panda_link8': (0.0724, '0 0 0', '0.0001', '0.0001', '0.0001'),
'panda_hand': (0.73, '0 0 0', '0.001', '0.001', '0.001'),
'panda_leftfinger': (0.03, '0 0 0', '0.0001', '0.0001', '0.0001'),
'panda_rightfinger': (0.03, '0 0 0', '0.0001', '0.0001', '0.0001'),
}
def _add_inertials(urdf: str) -> str:
"""moveit_resources 的 panda.urdf 是 MoveIt 精简版，link 没有 <inertial>。
sdformat 的 URDF->SDF 转换会把无 inertial 的 link 整个丢弃（连带其 joint），
导致 spawn 出来的模型只有 gazebo 插件块、没有任何 link/joint ------ 表现为
gazebo_ros2_control 日志里所有关节 "Skipping joint ... not in the gazebo model"。
给每个缺 inertial 的 link 注入 REAL_INERTIA 表里的真实惯量（比扁平默认值重，
与控制器默认增益匹配，物理稳定不发散）。"""
fmt = ('<inertial><mass value="{m}"/><origin xyz="{o}" rpy="0 0 0"/>'
'<inertia ixx="{ixx}" ixy="0" ixz="0" iyy="{iyy}" iyz="0" '
'izz="{izz}"/></inertial>')
def _add(m):
tag = m.group(0)
nm = re.search(r'name="([^"]+)"', tag)
m_, o, ixx, iyy, izz = REAL_INERTIA.get(nm.group(1),
(1.0, '0 0 0', '0.01', '0.01', '0.01'))
block = fmt.format(m=m_, o=o, ixx=ixx, iyy=iyy, izz=izz)
# 自闭合 link（如 panda_link8）：<link name="x"/> -> <link name="x">...</link>
if tag.rstrip().endswith('/>'):
return tag[:-2] + '>' + block + '</link>'
return tag + block
return re.sub(r'<link\b[^>]*>', _add, urdf)
def _strip_mimic(urdf: str) -> str:
"""URDF 里 panda_finger_joint2 的 <mimic> 让 MoveIt robot model 在状态更新时
反复找内部名 'panda_finger_joint2_mimic' -> 每 10ms 刷一条 "Joint not found in model"。
去掉 <mimic>：MoveIt 把它当普通被动关节（SRDF 已声明 passive_joint）；
夹爪联动改由 ros2_control 的 mimic 参数负责（Gazebo 端不受影响）。"""
return re.sub(r'<mimic\b[^>]*/>', '', urdf)
def _swap_visual_meshes(urdf: str) -> str:
"""moveit_resources 的 panda .dae 网格在 Gazebo classic 的 OGRE 下渲染成
白色盒子/黑色（RViz 用同一文件能渲染，是 Gazebo 特有的问题）。换成官方
franka_description 的 FR3 网格（同为 7 自由度、文件名一致、OGRE 兼容），
Gazebo 与 RViz 都能正确显示。碰撞网格（STL）不换。"""
pnd = 'file:///opt/ros/humble/share/moveit_resources_panda_description/meshes/visual'
fr3 = 'file:///opt/ros/humble/share/franka_description/meshes/robot_arms/fr3/visual'
hand = 'file:///opt/ros/humble/share/franka_description/meshes/robot_ee/franka_hand_white/visual'
urdf = urdf.replace(pnd, fr3)
urdf = urdf.replace(fr3 + '/hand.dae', hand + '/hand.dae')
urdf = urdf.replace(fr3 + '/finger.dae', hand + '/finger.dae')
return urdf
def _fix_base(urdf: str) -> str:
"""panda.urdf 根 link 是 panda_link0、没有任何 world 固定关节（MoveIt 用 SRDF
虚拟关节代替）。但在 Gazebo 里这意味着底座是自由体------只能靠碰撞网格搁在
地面上，位置控制器在漂移的底座上打极限环 -> 关节抖动/偶发 NaN -> 机械臂
抽搐/消失。官方 Gazebo 配置都会给底座加 world 固定关节，这里补上。"""
base = ('<link name="world"/>'
'<joint name="panda_link0_joint" type="fixed">'
'<parent link="world"/><child link="panda_link0"/></joint>')
# 插在 <robot ...> 之后（要在 _add_inertials/_add_materials 之后调用，
# 这样 world link 不会被注入惯量/材质）
return re.sub(r'(<robot[^>]*>)', r'\1' + base, urdf, count=1)
def _add_materials(urdf: str) -> str:
"""panda.urdf 的 <visual> 只有 mesh、没有 <material> 颜色，Gazebo classic
会 fallback 到 .dae 自带材质，而这些 COLLADA 文件在 OGRE 下渲染成黑色 ->
机械臂看起来黑/看不见。给每个 visual 注入浅灰材质即可（MoveIt/RViz 同样可用）。"""
material = ('<material name="panda_light_grey">'
'<color rgba="0.83 0.83 0.83 1"/></material>')
def _add(m):
tag = m.group(0)
if tag.rstrip().endswith('/>'):
return tag[:-2] + '>' + material + '</visual>'
return tag + material
return re.sub(r'<visual\b[^>]*>', _add, urdf)
def _fix_controller_yaml_path(urdf: str) -> str:
"""gazebo_ros2_control 把 <parameters> 当作 --params-file 交给 rcl 解析，
workspace 路径含中文（非 ASCII）时 rcl_parse_arguments 会失败 -> 插件 Load
失败 -> gzserver 崩溃。把 ros2_controllers.yaml 复制到 ASCII 路径 /tmp 并替换。"""
src = os.path.join(
get_package_share_directory('panda_gazebo_bringup'),
'config', 'ros2_controllers.yaml')
dst = '/tmp/panda_ros2_controllers.yaml'
shutil.copy2(src, dst)
return urdf.replace(src, dst)
def generate_launch_description():
panda_bringup_dir = get_package_share_directory('panda_gazebo_bringup')
# ---- 1) robot_description：自建 gazebo xacro（eager 生成 URDF 字符串）----
# mappings 全为 str -> 进程内立即 xacro.process_file，返回可修改的 URDF 字符串
moveit_config = (
MoveItConfigsBuilder('moveit_resources_panda')
.robot_description(
file_path=os.path.join(panda_bringup_dir, 'config', 'panda_gazebo.urdf.xacro'),
mappings={'ros2_control_hardware_type': 'gazebo'},
)
.robot_description_semantic(file_path='config/panda.srdf')
.trajectory_execution(file_path='config/gripper_moveit_controllers.yaml')
.planning_pipelines(pipelines=['ompl', 'chomp', 'pilz_industrial_motion_planner'])
.to_moveit_configs()
)
# 关键修复（缺一不可，否则 gzserver 崩溃/模型不可见）：
# 1) 网格 URI package:// -> file:// 绝对路径（Gazebo classic 不解析 package://）
# 2) 参数文件复制到 ASCII 路径（中文 workspace 路径会让 rcl 解析 --params-file 失败）
# 3) 给无 inertial 的 link 注入默认惯性（sdformat 会丢弃无 inertial 的 link 连带关节）
# 4) 给 visual 注入浅灰材质（.dae 自带材质在 OGRE 下渲染成黑色，机械臂看不见）
# 5) spawn 直接从文件读完整 URDF：-topic 走 /robot_description 时，10KB+ 的大消息
# 在 FastDDS 下偶发只投递末尾片段 -> 模型无关节（关节全被插件跳过）。
fixed_urdf = _add_materials(
_fix_base(
_add_inertials(
_fix_controller_yaml_path(
_swap_visual_meshes(
_fix_package_mesh_uris(
_strip_mimic(
moveit_config.robot_description['robot_description'])))))))
moveit_config.robot_description = {'robot_description': fixed_urdf}
robot_urdf_file = '/tmp/panda_full.urdf'
with open(robot_urdf_file, 'w') as f:
f.write(fixed_urdf)
# ---- 2) Gazebo 空世界（gazebo_ros 默认空 world，带 GUI）----
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('gazebo_ros'),
'launch', 'gazebo.launch.py')
),
)
# ---- 3) 发布 /robot_description + 关节 TF（spawn_entity 从这里取 URDF）----
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[moveit_config.robot_description, {'use_sim_time': True}],
output='screen',
)
# ---- 4) world -> panda_link0 静态 TF（MoveIt 虚拟关节规划帧是 world）----
static_tf = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='static_transform_publisher',
arguments=['0', '0', '0', '0', '0', '0', 'world', 'panda_link0'],
output='log',
)
# ---- 5) move_group ----
move_group_node = Node(
package='moveit_ros_move_group',
executable='move_group',
output='screen',
parameters=[moveit_config.to_dict(), {'use_sim_time': True}],
arguments=['--ros-args', '--log-level', 'info'],
)
# ---- 6) RViz（复用系统 moveit.rviz，Fixed Frame = panda_link0）----
rviz_config = os.path.join(
get_package_share_directory('moveit_resources_panda_moveit_config'),
'launch', 'moveit.rviz',
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config],
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.planning_pipelines,
moveit_config.robot_description_kinematics,
{'use_sim_time': True},
],
)
# ---- 7) 把 panda spawn 进 Gazebo ----
# spawn_entity.py 默认只等 30s 服务就绪，Gazebo 首次启动/DDS 发现偏慢时不够，
# 显式传 -timeout 60 放宽。
# 模型插入 -> gazebo_ros2_control 插件加载 -> controller_manager 起在 gzserver 内
# 从文件读 URDF（而非 -topic）：见上方 fixed_urdf 的注释。
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-file', robot_urdf_file, '-entity', 'panda',
'-timeout', '60'],
output='screen',
)
# ---- 8) spawn 成功退出 = 模型已插入 = controller_manager 已建立 ----
# 用 spawner（带 controller-manager-timeout）加载并激活三个控制器
load_joint_state_broadcaster = Node(
package='controller_manager',
executable='spawner',
arguments=['joint_state_broadcaster', '-c', '/controller_manager',
'--controller-manager-timeout', '30'],
output='screen',
)
load_arm_controller = Node(
package='controller_manager',
executable='spawner',
arguments=['panda_arm_controller', '-c', '/controller_manager',
'--controller-manager-timeout', '30'],
output='screen',
)
load_hand_controller = Node(
package='controller_manager',
executable='spawner',
arguments=['panda_hand_controller', '-c', '/controller_manager',
'--controller-manager-timeout', '30'],
output='screen',
)
return LaunchDescription([
gazebo,
robot_state_publisher,
static_tf,
move_group_node,
rviz_node,
spawn_entity,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_broadcaster, load_arm_controller,
load_hand_controller],
)
),
])- 这串
_fix_*后处理，每一步都对应一个真实的坑，缺一个都可能让 gzserver 崩溃或模型不可见：
| 修复函数 | 解决的坑 |
|---|---|
_fix_package_mesh_uris |
Gazebo 不解析 package:// 网格，换成 file:// 绝对路径 |
_add_inertials |
无 <inertial> 的 link 被 sdformat 丢弃(连带关节)，注入真实惯量 |
_fix_base |
panda 根 link 没固定 world 关节、底座是自由体，补 world 固定关节 |
_add_materials |
.dae 网格在 OGRE 下渲染成黑色，注入浅灰材质 |
_fix_controller_yaml_path |
中文 workspace 路径让 rcl 解析 --params-file 失败，复制到 /tmp |
_swap_visual_meshes |
moveit_resources 的 .dae 在 OGRE 下渲染成白盒/黑盒，换成 FR3 网格 |
1-6 一键启动
- 上面的 launch 是
gazebo.launch.py------无相机、无 octomap 的基线版 ，一键脚本1_gazebo_baseline.sh把它包一层：先杀干净残留进程，再 source 环境、启动（第三章的相机增强版是另一个脚本3_depth_gazebo.sh，3-5 再展开） - 那段进程清理很关键------
ros2 launch的 Ctrl+C 在部分环境会卡死进程(信号 fd 被提前关闭)，脚本用pkill+ps精确匹配兜底kill -9
bash
#!/bin/bash
# 基线启动（无相机、无 octomap）：gazebo.launch.py 原版
# 主流程用 ./3_depth_gazebo.sh（= 本脚本的超集，还带腕部 RGBD 相机 + octomap）
# 本脚本仅在需要无相机基线做对照/排障时用；之后仍用 ./2_arm_control.sh 跑 C++ 控制节点
set -e
# ---- 启动前清理：杀掉所有残留进程，确保从干净状态启动 ----
echo "[cleanup] 清理残留进程 (gzserver / gzclient / move_group / rviz2 / controllers ...)"
for p in gzserver gzclient gazebo; do
pkill -x "$p" 2>/dev/null || true
done
# 进程名超过 15 字符时 pkill -x 匹配不到（如 robot_state_publisher），改用 ps 精确匹配；
# 过滤掉 grep 自身和 "bash -c"，避免误杀当前 shell / 上层包装（历史教训：曾把自己杀掉）
ps aux | grep -E "ros2 launch|spawn_entity|spawner|move_group|robot_state_publisher|static_transform_publisher|controller_manager|rviz2|arm_control_demo" \
| grep -v grep | grep -v "bash -c" \
| awk '{print $2}' | xargs -r kill -9 2>/dev/null || true
sleep 2
source /opt/ros/humble/setup.bash
source "$(dirname "$0")/install/setup.bash"
export LANG=C.UTF-8 LC_ALL=C.UTF-8
ros2 launch panda_gazebo_bringup gazebo.launch.py- 启动后就能看到 rviz2 和 Gazebo 两个窗口
2 编写自己的节点arm_control_demo.cpp
2-1 流程
- 节点要做的事非常聚焦：订阅一个"目标位姿"，每收到一次就规划一次、执行一次，然后回到等待
- 完整流程：
- 启动：launch 把
robot_description等参数注入，节点init()里建MoveGroupInterface(构造函数会阻塞等move_group的 action server 就绪) - 等待：主循环
cv_.wait()挂起，直到收到/joint_states和第一个/target_pose - 触发：订阅回调把目标位姿缓存进共享变量，
notify_one()唤醒主循环 - 规划：用
/joint_states最新值显式构造起点，setPoseTarget设目标，plan()出轨迹 - 执行：
execute()把轨迹发给move_group，后者经 ros2_control 下发给 Gazebo 物理驱动 - 反馈：
/joint_states订阅一直刷新，为下一次规划提供真实起点
- 启动：launch 把
2-2 功能包创建
- 节点放在独立包
arm_control_demo里，创建命令：
bash
cd src
ros2 pkg create arm_control_demo --build-type ament_cmake \
--dependencies rclcpp moveit_ros_planning_interface \
geometry_msgs sensor_msgs moveit_msgs- 创建完补上
launch/目录、把源码替换成 2-4 的完整实现后，最终包结构如下（编译产物已省略）：
bash
arm_control_demo/
├── CMakeLists.txt # 构建配置：编译 src/arm_control_demo.cpp，安装 launch
├── package.xml # 包元信息与依赖声明
├── launch/
│ └── arm_control.launch.py # 包装 launch：注入 robot_description 等参数
└── src/
└── arm_control_demo.cpp # 主节点：订阅 /target_pose，规划+执行（源码见 2-4）- 先看
CMakeLists.txt------不能只当pkg create的模板用：默认模板不编译src/里的源码、也不安装launch/目录。缺了add_executable+ament_target_dependencies+install(TARGETS ...)+install(DIRECTORY launch ...)这几段，ros2 run和ros2 launch都找不到东西。pkg create的--dependencies只是写进package.xml，CMake 里还必须find_package才能链接：
cmake
cmake_minimum_required(VERSION 3.8)
project(arm_control_demo)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
add_executable(arm_control_demo src/arm_control_demo.cpp)
ament_target_dependencies(arm_control_demo
rclcpp
sensor_msgs
geometry_msgs
moveit_msgs
moveit_ros_planning_interface
)
install(TARGETS arm_control_demo
DESTINATION lib/${PROJECT_NAME})
# 全部用相对路径（规避中文路径风险）
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME})
ament_package()- 因为
MoveGroupInterface需要robot_description、规划管线等参数(Humble 没有 topic 回退)，所以源码编译成可执行目标后，用arm_control.launch.py包一层：
python
import os
import shutil
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def _fix_package_mesh_uris(urdf: str) -> str:
panda_desc = get_package_share_directory('moveit_resources_panda_description')
return urdf.replace('package://moveit_resources_panda_description', 'file://' + panda_desc)
def _fix_controller_yaml_path(urdf: str) -> str:
src = os.path.join(
get_package_share_directory('panda_gazebo_bringup'),
'config', 'ros2_controllers.yaml')
dst = '/tmp/panda_ros2_controllers.yaml'
shutil.copy2(src, dst)
return urdf.replace(src, dst)
def generate_launch_description():
panda_bringup_dir = get_package_share_directory('panda_gazebo_bringup')
# 与 gazebo.launch.py 完全一致的 MoveIt 配置构造（URDF 用自建 gazebo xacro）
moveit_config = (
MoveItConfigsBuilder('moveit_resources_panda')
.robot_description(
file_path=os.path.join(panda_bringup_dir, 'config', 'panda_gazebo.urdf.xacro'),
mappings={'ros2_control_hardware_type': 'gazebo'},
)
.robot_description_semantic(file_path='config/panda.srdf')
.trajectory_execution(file_path='config/gripper_moveit_controllers.yaml')
.planning_pipelines(pipelines=['ompl', 'chomp', 'pilz_industrial_motion_planner'])
.to_moveit_configs()
)
moveit_config.robot_description = {
'robot_description': _fix_controller_yaml_path(
_fix_package_mesh_uris(
moveit_config.robot_description['robot_description']))
}
# C++ 节点必须由 launch 提供 robot_description 等参数（Humble 无 topic 回退）
return LaunchDescription([
Node(
package='arm_control_demo',
executable='arm_control_demo',
parameters=[moveit_config.to_dict(), {'use_sim_time': True}],
output='screen',
)
])- launch 里用和
gazebo.launch.py完全一致的MoveItConfigsBuilder构造，保证规划参数两边一致，并注入use_sim_time；启动命令由 2-5 的./2_arm_control.sh包一层 - 这里声明了 OMPL / CHOMP / Pilz 三条规划管线，规划时默认走 OMPL（每种算法的默认参数，即
planner_configs里那几百行range/goal_bias等，在moveit_resources_panda_moveit_config/config/ompl_planning.yaml，由MoveItConfigsBuilder自动加载，无需手写）------暂不展开，规划器对比与调参留到后面专题
2-3 思路
- 整体思路就是：订阅
/target_pose，收到一个目标就规划+执行一次，流程上就是"订阅 -> Plan -> Execute"循环
#mermaid-svg-Slgu1PQexEr5Tvcp{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-Slgu1PQexEr5Tvcp .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-Slgu1PQexEr5Tvcp .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-Slgu1PQexEr5Tvcp .error-icon{fill:#552222;}#mermaid-svg-Slgu1PQexEr5Tvcp .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-Slgu1PQexEr5Tvcp .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-Slgu1PQexEr5Tvcp .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-Slgu1PQexEr5Tvcp .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-Slgu1PQexEr5Tvcp .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-Slgu1PQexEr5Tvcp .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-Slgu1PQexEr5Tvcp .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-Slgu1PQexEr5Tvcp .marker{fill:#333333;stroke:#333333;}#mermaid-svg-Slgu1PQexEr5Tvcp .marker.cross{stroke:#333333;}#mermaid-svg-Slgu1PQexEr5Tvcp svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-Slgu1PQexEr5Tvcp p{margin:0;}#mermaid-svg-Slgu1PQexEr5Tvcp .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-Slgu1PQexEr5Tvcp .cluster-label text{fill:#333;}#mermaid-svg-Slgu1PQexEr5Tvcp .cluster-label span{color:#333;}#mermaid-svg-Slgu1PQexEr5Tvcp .cluster-label span p{background-color:transparent;}#mermaid-svg-Slgu1PQexEr5Tvcp .label text,#mermaid-svg-Slgu1PQexEr5Tvcp span{fill:#333;color:#333;}#mermaid-svg-Slgu1PQexEr5Tvcp .node rect,#mermaid-svg-Slgu1PQexEr5Tvcp .node circle,#mermaid-svg-Slgu1PQexEr5Tvcp .node ellipse,#mermaid-svg-Slgu1PQexEr5Tvcp .node polygon,#mermaid-svg-Slgu1PQexEr5Tvcp .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-Slgu1PQexEr5Tvcp .rough-node .label text,#mermaid-svg-Slgu1PQexEr5Tvcp .node .label text,#mermaid-svg-Slgu1PQexEr5Tvcp .image-shape .label,#mermaid-svg-Slgu1PQexEr5Tvcp .icon-shape .label{text-anchor:middle;}#mermaid-svg-Slgu1PQexEr5Tvcp .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-Slgu1PQexEr5Tvcp .rough-node .label,#mermaid-svg-Slgu1PQexEr5Tvcp .node .label,#mermaid-svg-Slgu1PQexEr5Tvcp .image-shape .label,#mermaid-svg-Slgu1PQexEr5Tvcp .icon-shape .label{text-align:center;}#mermaid-svg-Slgu1PQexEr5Tvcp .node.clickable{cursor:pointer;}#mermaid-svg-Slgu1PQexEr5Tvcp .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-Slgu1PQexEr5Tvcp .arrowheadPath{fill:#333333;}#mermaid-svg-Slgu1PQexEr5Tvcp .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-Slgu1PQexEr5Tvcp .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-Slgu1PQexEr5Tvcp .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-Slgu1PQexEr5Tvcp .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-Slgu1PQexEr5Tvcp .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-Slgu1PQexEr5Tvcp .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-Slgu1PQexEr5Tvcp .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-Slgu1PQexEr5Tvcp .cluster text{fill:#333;}#mermaid-svg-Slgu1PQexEr5Tvcp .cluster span{color:#333;}#mermaid-svg-Slgu1PQexEr5Tvcp div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-Slgu1PQexEr5Tvcp .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-Slgu1PQexEr5Tvcp rect.text{fill:none;stroke-width:0;}#mermaid-svg-Slgu1PQexEr5Tvcp .icon-shape,#mermaid-svg-Slgu1PQexEr5Tvcp .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-Slgu1PQexEr5Tvcp .icon-shape p,#mermaid-svg-Slgu1PQexEr5Tvcp .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-Slgu1PQexEr5Tvcp .icon-shape .label rect,#mermaid-svg-Slgu1PQexEr5Tvcp .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-Slgu1PQexEr5Tvcp .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-Slgu1PQexEr5Tvcp .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-Slgu1PQexEr5Tvcp :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 否
是
否
是
等待 /joint_states 到位
等待 /target_pose
收到目标?
用 /joint_states 显式构造规划起点
setPoseTarget 设目标
plan 规划轨迹
规划成功?
打印 plan failed
execute 执行
- 这里我们使用默认的
ompl规划管线，只需配好速度/加速度缩放和规划时间上限，Gazebo 物理下才跑得稳：
yaml
# 对应 arm_control_demo.cpp 里的常量(为对齐 yaml 写法列出)
velocity_scaling_factor: 0.3 # kVelocityScale,轨迹速度放慢到 30%
acceleration_scaling_factor: 0.3 # kAccelerationScale,加速度放慢到 30%
planning_time: 10.0 # kPlanningTime,单次规划时间上限(秒)2-4 源码
- 三个地方值得注意，都是实际踩过的坑：
MoveGroupInterface不能在构造函数里创建------shared_from_this()依赖make_shared初始化，构造函数里调用会抛std::bad_weak_ptr，所以挪到init()，由 main 建完节点后调用- 规划起点每次用
/joint_states最新值显式构造：MoveGroupInterface客户端缓存的起点在execute后不刷新，直接规划会用旧起点，导致轨迹起点和实际关节状态不一致 - 起点只取 7 个真实臂关节，排除
/joint_states里的 SRDF 假关节panda_finger_joint2_mimic，否则 move_group 应用起点会抛 "Catastrophic failure"
- 完整源码：
cpp
// arm_control_demo.cpp
// 第三期：目标位姿驱动的控制节点
// 订阅 /target_pose (geometry_msgs/Pose)：每收到一个目标点，就规划+执行过去。
// 不内置固定轨迹、不归位 ------ 节点启动后挂起等待，发点即动。
// 结构：ArmControlDemo 继承 rclcpp::Node，main() 只负责建节点、起后台 executor、调用 run()。
#include <algorithm>
#include <chrono>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/utils/moveit_error_code.h>
#include <moveit_msgs/msg/robot_state.hpp>
class ArmControlDemo : public rclcpp::Node
{
public:
explicit ArmControlDemo(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("arm_control_demo", options)
{
// 反馈：订阅 Gazebo/ros2_control 发布的真实关节状态（规划起点的真实来源）
joint_states_sub_ = create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", rclcpp::SensorDataQoS(),
[this](const sensor_msgs::msg::JointState::ConstSharedPtr msg) {
std::lock_guard<std::mutex> lock(mutex_);
joint_states_ = *msg;
got_joint_states_ = true;
});
// 指令：订阅目标位姿，收到一个就驱动一次（回调只缓存，处理放 run() 主循环）
target_pose_sub_ = create_subscription<geometry_msgs::msg::Pose>(
"/target_pose", rclcpp::QoS(10),
[this](const geometry_msgs::msg::Pose::ConstSharedPtr target) {
{
std::lock_guard<std::mutex> lock(mutex_);
target_pose_ = *target;
has_target_ = true;
}
cv_.notify_one();
});
// 关键：MoveGroupInterface 不能在这里创建！
// shared_from_this() 依赖 make_shared 初始化的 enable_shared_from_this
// 的 weak_this_，本环境 libstdc++ 要到 make_shared 返回后才初始化它，
// 在构造函数里调用会抛 std::bad_weak_ptr。挪到 init()，由 main 建完节点后调用。
// 也不要在构造函数里调 getCurrentPose()：此时后台 executor 还没起，
// /joint_states 订阅回调没跑过，current_state_monitor 是空的，会取不到状态。
// 启动日志放到 run()（executor 已转起来）再打。
}
// 建完节点后调用：创建 MoveGroupInterface（构造即阻塞等待 move_group action server）
void init()
{
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
shared_from_this(), kPlanningGroup);
// Gazebo 物理下放慢一点，更稳
move_group_->setMaxVelocityScalingFactor(kVelocityScale);
move_group_->setMaxAccelerationScalingFactor(kAccelerationScale);
move_group_->setPlanningTime(kPlanningTime);
}
// 主循环：等目标点 -> 规划 -> 执行 -> 再等
void run()
{
// 等第一个 /joint_states 到位（后台 executor 此刻已在转），再取当前末端位姿
std::unique_lock<std::mutex> lock(mutex_);
cv_.wait_for(lock, std::chrono::seconds(10), [this] { return got_joint_states_; });
lock.unlock();
geometry_msgs::msg::Pose current = currentEndEffectorPose();
RCLCPP_INFO(get_logger(), "current EE pose: [%.3f, %.3f, %.3f], waiting for /target_pose ...",
current.position.x, current.position.y, current.position.z);
while (rclcpp::ok()) {
std::unique_lock<std::mutex> lock(mutex_);
cv_.wait(lock, [this] { return has_target_ || !rclcpp::ok(); });
if (!rclcpp::ok()) {
break;
}
geometry_msgs::msg::Pose target = target_pose_;
has_target_ = false;
lock.unlock();
moveToTarget(target);
}
}
private:
using Plan = moveit::planning_interface::MoveGroupInterface::Plan;
using ErrorCode = moveit::core::MoveItErrorCode;
static constexpr char kPlanningGroup[] = "panda_arm";
static constexpr double kVelocityScale = 0.3;
static constexpr double kAccelerationScale = 0.3;
static constexpr double kPlanningTime = 10.0;
std::mutex mutex_;
std::condition_variable cv_;
sensor_msgs::msg::JointState joint_states_;
bool got_joint_states_ = false;
geometry_msgs::msg::Pose target_pose_;
bool has_target_ = false;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_sub_;
rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr target_pose_sub_;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
geometry_msgs::msg::Pose currentEndEffectorPose()
{
return move_group_->getCurrentPose("panda_link8").pose;
}
// 规划到目标位姿并执行
void moveToTarget(const geometry_msgs::msg::Pose & target)
{
RCLCPP_INFO(get_logger(), "target: [%.3f, %.3f, %.3f]",
target.position.x, target.position.y, target.position.z);
setStartFromJointStates();
Plan plan;
move_group_->setPoseTarget(target, "panda_link8");
ErrorCode plan_ok = move_group_->plan(plan);
if (plan_ok) {
ErrorCode exec_ok = move_group_->execute(plan);
RCLCPP_INFO(get_logger(), "execute result: %s",
moveit::core::error_code_to_string(exec_ok).c_str());
} else {
RCLCPP_ERROR(get_logger(), "plan failed: %s",
moveit::core::error_code_to_string(plan_ok).c_str());
}
}
// 每次规划前用 /joint_states 最新值显式构造起点：
// MoveGroupInterface 客户端缓存的起点状态在 execute 后不刷新，setStartStateToCurrentState()
// 拿到的还是旧值，导致规划轨迹起点与实际关节状态不一致。只取 7 个真实臂关节，
// 排除 /joint_states 里 SRDF 假关节 panda_finger_joint2_mimic（不在 URDF 模型里，
// 塞进起点会让 move_group 应用起点时抛 "Catastrophic failure"）。
void setStartFromJointStates()
{
std::lock_guard<std::mutex> lock(mutex_);
if (!got_joint_states_) {
move_group_->setStartStateToCurrentState();
return;
}
const std::vector<std::string> arm_joints = {
"panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4",
"panda_joint5", "panda_joint6", "panda_joint7"};
moveit_msgs::msg::RobotState start;
for (const auto & name : arm_joints) {
auto it = std::find(joint_states_.name.begin(), joint_states_.name.end(), name);
if (it != joint_states_.name.end()) {
size_t idx = std::distance(joint_states_.name.begin(), it);
start.joint_state.name.push_back(name);
start.joint_state.position.push_back(joint_states_.position[idx]);
}
}
move_group_->setStartState(start);
}
};
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
// robot_description / planning_pipelines 等参数必须由 launch 传入
auto node = std::make_shared<ArmControlDemo>(
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
// make_shared 已返回，shared_from_this() 可用；再建 MoveGroupInterface
node->init();
// 后台 executor：驱动两个订阅回调（use_sim_time 由 launch 注入）
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
std::thread spin_thread([&executor]() { executor.spin(); });
node->run();
rclcpp::shutdown();
spin_thread.join();
return 0;
}2-5 启动与发点脚本
- 节点编译完（
colcon build），先用2_arm_control.sh把它跑起来------它就是ros2 launch arm_control_demo arm_control.launch.py的包装，前置条件：1_gazebo_baseline.sh或3_depth_gazebo.sh已在跑（gazebo + move_group 都在）：
bash
#!/bin/bash
# 第三期：运行 C++ 控制节点（arm_control_demo 订阅 /target_pose，收点即规划+执行）
# 前提：先跑 1_gazebo_baseline.sh 或 3_depth_gazebo.sh（gazebo + move_group 都在跑）
source /opt/ros/humble/setup.bash
source "$(dirname "$0")/install/setup.bash"
export LANG=C.UTF-8 LC_ALL=C.UTF-8
ros2 launch arm_control_demo arm_control.launch.py- 再写一个发点脚本
4_send_target.sh，配合上面的节点：支持命令行单发(可选四元数)和交互模式，不发姿态时自动沿用 TF 里的当前末端姿态(只动位置、不转姿态)
bash
#!/bin/bash
# 发布目标位姿给 ./2_arm_control.sh（arm_control_demo 订阅 /target_pose，收点即规划+执行）
# 前提：./3_depth_gazebo.sh 在跑（提供 /tf 和 move_group），然后 ./2_arm_control.sh 也在跑
#
# 用法：
# ./4_send_target.sh # 交互模式：逐行输入 x y z，回车即发；q 退出
# ./4_send_target.sh <x> <y> <z> # 单发：保持当前末端姿态，只改位置
# ./4_send_target.sh <x> <y> <z> <qx> <qy> <qz> <qw> # 单发：完整指定姿态（四元数）
#
# 注意：坐标是 move_group 规划帧（world）下的 panda_link8 末端位姿；
# 不发姿态时自动沿用 tf 里当前的末端姿态（只动位置、不转姿态）。
set -e
# 与 ./1 ./2 ./3 相同的环境准备：退出 conda，避免 anaconda python 劫持 rclpy
source /home/lzh/anaconda3/etc/profile.d/conda.sh 2>/dev/null || true
conda deactivate 2>/dev/null || true
source /opt/ros/humble/setup.bash
source "$(dirname "$0")/install/setup.bash"
export LANG=C.UTF-8 LC_ALL=C.UTF-8
# 参数原样转发给内嵌 python（"/target_pose" 发布 + world->panda_link8 姿态查询）
python3 - "$@" <<'PY'
import sys
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from rclpy.parameter import Parameter
from geometry_msgs.msg import Pose
from tf2_ros import Buffer, TransformListener, TransformException
PARENT, CHILD = 'world', 'panda_link8'
def wait_transform(buf, node):
"""等 world->panda_link8 的 TF（move_group 规划帧下的末端姿态）。"""
for _ in range(100): # 最多 ~20s
try:
return buf.lookup_transform(PARENT, CHILD, rclpy.time.Time())
except TransformException:
rclpy.spin_once(node, timeout_sec=0.2)
raise RuntimeError(f'{PARENT} -> {CHILD} TF 20s 内不可用')
def current_orientation(buf, node):
t = wait_transform(buf, node)
q = t.transform.rotation
return q.x, q.y, q.z, q.w
def build_pose(args, buf, node):
vals = [float(a) for a in args]
p = Pose()
p.position.x, p.position.y, p.position.z = vals[0], vals[1], vals[2]
if len(vals) >= 7:
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w = vals[3:7]
else:
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w = \
current_orientation(buf, node)
return p
def send(pub, node, p, tag=''):
pub.publish(p)
rclpy.spin_once(node, timeout_sec=0.1)
node.get_logger().info(
f'sent{tag}: pos [{p.position.x:.3f}, {p.position.y:.3f}, {p.position.z:.3f}] '
f'quat [{p.orientation.x:.3f}, {p.orientation.y:.3f}, {p.orientation.z:.3f}, '
f'{p.orientation.w:.3f}]')
def main():
rclpy.init()
# use_sim_time 与整套仿真一致，TF 缓存判定才可靠
node = Node('target_pose_sender',
parameter_overrides=[Parameter('use_sim_time', value=True)])
buf = Buffer()
TransformListener(buf, node)
# arm_control_demo 用 rclcpp::QoS(10) 订阅（reliable），这里发布端也 reliable
pub = node.create_publisher(Pose, '/target_pose', QoSProfile(depth=10))
args = sys.argv[1:]
if not args:
# 交互模式：先打出当前末端位姿，再逐行收 x y z
qx, qy, qz, qw = current_orientation(buf, node)
node.get_logger().info(
f'当前末端姿态（world 下 panda_link8）：'
f'quat [{qx:.3f}, {qy:.3f}, {qz:.3f}, {qw:.3f}]，'
f'输入 q 退出')
try:
while True:
line = input('xyz> ').strip()
if not line:
continue
if line.lower() in ('q', 'quit', 'exit'):
break
try:
vals = [float(v) for v in line.split()]
if len(vals) != 3:
raise ValueError('需要 3 个数')
except ValueError as e:
node.get_logger().error(f'输入解析失败（{e}），需要 "x y z"，q 退出')
continue
p = build_pose(vals, buf, node)
send(pub, node, p)
except (EOFError, KeyboardInterrupt):
pass
else:
try:
p = build_pose(args, buf, node)
except (ValueError, IndexError):
sys.exit('参数错误：需要 <x> <y> <z> [qx qy qz qw]，四元数可省略')
send(pub, node, p)
rclpy.shutdown()
if __name__ == '__main__':
main()
PY- 使用起来很简单，只需一条命令：
bash
./4_send_target.sh 0.28 -0.2 0.5- 在
rviz2里就能看到效果：机械臂接收到目标后，plan()先规划出一段轨迹残影，紧接着实体机械臂跟着残影一起动起来
-
同时打开
Gazebo，会发现它的动作和rviz2里规划的一致
-
需要注意的是，当你发布的目标点不可达 （超出机械臂工作空间、与给定姿态组合起来 IK 无解，或目标位姿和机械臂自身/环境发生碰撞）时，
plan()会直接失败，机械臂原地不动，终端里会看到这样的报错：
bash
[arm_control_demo-1] [INFO] [1787986315.394702429] [move_group_interface]: MoveGroup action client/server ready
[arm_control_demo-1] [INFO] [1787986315.395037482] [move_group_interface]: Planning request accepted
[arm_control_demo-1] [INFO] [1787986325.592517226] [move_group_interface]: Planning request aborted
[arm_control_demo-1] [ERROR] [1787986325.592662114] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached
[arm_control_demo-1] [ERROR] [1787986325.592708880] [arm_control_demo]: plan failed: GOAL_STATE_INVALID- 关键在最后一行：
GOAL_STATE_INVALID表示 MoveIt 在规划前就把目标位姿判定为非法，根本没进规划器。换一个可达的目标点再发即可，比如./4_send_target.sh 0.4 0.0 0.5（落在桌面工作区正上方）
3 深度相机插件与机械臂
- 上一节我们提到过PlanningScene的碰撞检测支持Octomap
- Octomap是什么可以参考【OctoMap ROS 3D建图】ROS Noetic 基于 octomap_server 实现三维地图构建、可视化与地图保存
3-1 末端执行器的深度相机
- 深度相机(RGBD，一图两用：RGB + 点云)装在哪，直接决定它能"看"到什么，主要有三种：
- 固定外部相机：相机固定在世界某处，视角不变、覆盖范围大，但机械臂动起来容易遮挡目标
- 眼外安装(eye-to-hand)：相机装在机械臂外的支架上，能看到整个工作空间，标定一次即可，但和目标之间有遮挡风险
- 眼在手上(eye-in-hand)：相机固定在末端执行器上，跟着手走
- 为什么眼在手最具普适性：抓取/操作任务里，目标往往离手很近，只有相机在手上才能保证"手伸到哪、目标就在视野中央"，而且越靠近目标测量越准、误差越小
- 本项目采用眼在手：相机挂在
panda_hand腕部，做视觉伺服、抓取前的目标定位都很自然
说人话：眼睛装在手上，才能"盯住"手里的活。VLA 里那句"看手在干嘛"也是这个道理
3-2 手眼标定
- 相机输出的是"目标在相机坐标系 里离我多远"，机械臂执行的是"往机械臂坐标系 的哪个位姿伸"------两套坐标系之间隔着一座桥，这座桥就是手眼标定要解的固定变换
- 标哪个变换，看相机装在哪（和 3-1 的两种装法一一对应）：
- 眼在手（eye-in-hand） ：相机随末端一起动，要解的是 相机与末端（gripper） 的固定变换
- 眼在手外（eye-to-hand） ：相机固定不动，要解的是 相机与机械臂底座（base） 的固定变换
- 数学本质是解 AX=XB ：机械臂摆两个不同姿态，末端位姿变化 A（正运动学直接给），相机对着同一块标定板，相机位姿变化 B（棋盘格角点估计给），未知数 X 就是手眼变换。摆十几组姿态堆起来就能解出 X，常用 Tsai-Lenz / Park-Martin / Daniilidis 等算法，OpenCV 的
cv::calibrateHandEye()直接封装好了- 实操流程：末端挂标定板（或相机对着地面棋盘格）→ 摆多组姿态、逐组记录末端位姿和棋盘格角点 → 解方程得 X → 把 X 写进 TF 或标定参数
- 但仿真可以整体跳过这一步 ：
camera_link和panda_hand之间的变换就写在 URDF 的camera_joint里（后文 3-3 相机块里那个rpy="3.14159 -1.5708 0"），仿真里这个值精确且已知------标定求的正是它，再标一遍只是自欺欺人
- 所以本章后续直接信任 URDF 里的安装变换；将来上真机，手眼标定是必做项，标定误差会 1:1 变成抓取误差
说人话：手眼标定就是给"眼睛"和"手"对表------相机说目标在它面前 30cm，机械臂得换算成自己该往哪个方向伸。真机装上去总有装配误差，必须标；仿真里这个"对表值"是我们亲手写进 URDF 的标准答案，所以直接跳过
3-3 插件配置以及注意事项
- Gazebo 的深度相机插件我们用
libgazebo_ros_camera：标准深度相机插件，一个sensor type="depth"同时出 RGB 图像、深度图、点云 三路话题（外加各自的camera_info），MoveIt 的 octomap 要吃的点云，就是其中/camera/depth/points这一路（3-4 会把它接进规划场景）- 但有个坑：
libgazebo_ros_camera输出的点云是 z-forward 光学约定 （深度沿 +z、+x 向右、+y 向下），而frame_name指向的camera_link是 x-forward 物理系（相机沿 +x 看）。两套轴系差 90°，深度被标在了与视轴垂直的方向上 → 转成world后，本应水平的桌面被画成一面竖直的墙（中心点落到了相机同高、前方约 0.41m 处，而不是正下方） - 所以需要多引入一个纯 TF 子帧
camera_optical_frame（无碰撞、无几何，就是多一个坐标系）：origin全 0、只转姿态rpy="-1.5708 0 -1.5708"，把光学 +z 转到与camera_link的 +x（视轴）重合、+x/+y 对齐画面右/下------点云标到这个帧上就是标准的 z-forward 光学系，最后frame_name指向它即可
- 但有个坑：
- 在
src/panda_gazebo_bringup/config/panda_gazebo.urdf.xacro里直接补上下面这一整块（放在</robot>之前）：
xml
<!-- 4) 可选 RGBD 深度相机（眼在手）：gazebo_camera.launch.py 用 enable_camera=true 打开。
纯被动固定 link（无碰撞），挂 panda_hand 腕部；
libgazebo_ros_camera.so 标准深度相机：RGB + 点云（+深度图）三路话题 -->
<xacro:if value="$(arg enable_camera)">
<link name="camera_link">
<visual><geometry><box size="0.03 0.025 0.02"/></geometry></visual>
</link>
<joint name="camera_joint" type="fixed">
<!-- 直接挂在 panda_hand(工具系)上 -> panda_hand->camera_link 为纯平移、yaw=0，
相机轴与末端执行器完全对齐(此前挂 link8 会因 hand 自带 yaw-45° 而相对手掌错开45°)。
位置取 hand 系 (0.078,0,0)：只沿 hand 的 x 侧挂、y=0，让开手掌包络(x±32mm)。
rpy="3.14159 -1.5708 0"：libgazebo_ros_camera 深度相机默认沿传感器系 +x 拍摄、
+z 朝上，pitch -90° 把 +x 转到工具轴 +z 方向、拍摄方向水平朝前；
roll 180° 绕拍摄轴旋转，修正画面左右颠倒 -->
<parent link="panda_hand"/>
<child link="camera_link"/>
<origin xyz="0.078 0 0" rpy="3.14159 -1.5708 0"/>
</joint>
<!-- 光学帧：libgazebo_ros_camera 输出的点云是 ROS 光学约定（z-forward：
深度沿 +z、+x 向右、+y 向下），frame_name 必须指向一个 z-forward 的帧。
camera_link 是 x-forward 的物理系（相机沿 +x 看），直接当光学帧用会导致
深度标在与视轴垂直的方向上 -> 桌面被渲染成竖直的墙。
加一个纯 TF 子帧，rpy="-1.5708 0 -1.5708" 使光学 +z 与 camera_link +x（视轴）重合，
同时光学 +x/+y 与画面右/下对齐，RGB 与点云方向一致。 -->
<link name="camera_optical_frame"/>
<joint name="camera_optical_joint" type="fixed">
<parent link="camera_link"/>
<child link="camera_optical_frame"/>
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
</joint>
<gazebo reference="camera_link">
<sensor type="depth" name="camera">
<update_rate>10</update_rate>
<camera>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>640</width><height>480</height><format>R8G8B8</format>
</image>
<clip><near>0.02</near><far>3</far></clip>
<depth_camera><output_type>points</output_type></depth_camera>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<!-- 插件最小测量距离（必须写 <plugin> 内，Load 的 _sdf 指向 plugin）：
min_depth 默认 0.4m，而 ready 位姿下相机垂直朝下、离桌面仅 0.39m
-> 桌面中心/物体全被裁成 -inf 丢点云，octomap 建不出场景。
缩到 0.02m 才能测近处。<clip><near> 只是 OGRE 渲染裁剪，
不参与点云过滤，也一并缩到 0.02 保持一致。 -->
<min_depth>0.02</min_depth>
<max_depth>3</max_depth>
<ros>
<!-- 不加 <namespace>：camera_name='camera' 已给话题加 camera/ 前缀，
作为顶层 /camera/；remap 键必须带前缀才匹配得上 -->
<!-- QoS：MoveIt 的 PointCloudOctomapUpdater(Humble) 用 SensorDataQoS()
订阅点云，即硬编码 BEST_EFFORT+KEEP_LAST(5)，参数不可配；
相机默认 RELIABLE，两边不兼容 -> octomap 永远收不到点云。
这里把相机全部话题统一降级成 best_effort 对齐。
副作用：RViz 的 RGB/点云显示默认订阅 RELIABLE 会收不到，
需在显示属性里把 QoS 改成 Best Effort（或加转发节点）。 -->
<qos>
<reliability>best_effort</reliability>
</qos>
<remapping>camera/image_raw:=/camera/color/image_raw</remapping>
<remapping>camera/camera_info:=/camera/color/camera_info</remapping>
<remapping>camera/depth/image_raw:=/camera/depth/image_raw</remapping>
<remapping>camera/depth/camera_info:=/camera/depth/camera_info</remapping>
<remapping>camera/points:=/camera/depth/points</remapping>
</ros>
<camera_name>camera</camera_name>
<!-- 输出标到光学帧(z-forward)上，与插件点云约定一致 -->
<frame_name>camera_optical_frame</frame_name>
</plugin>
</sensor>
</gazebo>
</xacro:if>说人话：
camera_link是"相机脸朝哪"（沿 +x 看），光学约定是"点云坐标怎么标"（深度沿 +z）。两者对不上，深度就会被画到与视线垂直的方向上------水平的桌面看起来就像一面竖直的墙。多插一个camera_optical_frame，就是专门给点云一个正确的坐标系
3-4 对接moveit2的深度图像输入
- 点云接进 MoveIt 走的是 octomap ：
move_group的PointCloudOctomapUpdater订阅点云，把障碍物更新进 planning scene，规划时自动避障
3-4-1 QoS 对齐
- 第一个坑在 QoS。更新器订阅点云用的是
rclcpp::SensorDataQoS()，也就是硬编码 BEST_EFFORT + KEEP_LAST(5) ，参数不可配;而相机插件默认RELIABLE------两边不兼容，octomap 永远收不到点云 - 解决办法：把相机所有话题统一降级成
best_effort对齐
xml
<!-- 相机 QoS 降级:对齐 moveit octomap 的 SensorDataQoS()(best_effort) -->
<qos>
<reliability>best_effort</reliability>
</qos>
<remapping>camera/points:=/camera/depth/points</remapping>- 然后在
move_group参数里把数据源挂到/camera/depth/points(这个路径就是上面 remap 出来的)：
python
# gazebo_camera.launch.py 里 move_group 的 octomap 参数
'octomap_frame': 'world',
'octomap_resolution': 0.02,
'sensors': ['camera'],
'camera': {
'sensor_plugin': 'occupancy_map_monitor/PointCloudOctomapUpdater',
'point_cloud_topic': '/camera/depth/points',
'max_range': 3.0,
'padding_offset': 0.01,
'max_update_rate': 5.0,
'filtered_cloud_topic': 'filtered_cloud',
},- 副作用：RViz 的 RGB/点云显示默认订阅
RELIABLE也收不到，所以moveit_camera.rviz里新增的两个显示都要把Reliability Policy改成Best Effort
3-4-2 最小测量范围
- 点云接上了，但很快会发现一个怪现象：桌面中心、物体总是建不出来，"越近越看不见"
- 原因藏在插件源码里------点云过滤用的是插件自己的
min_depth参数，默认 0.4m ，注意它不是<clip><near>(那只是 OGRE 渲染裁剪，不参与点云过滤) - 逻辑是：深度小于
min_depth的点，整个设成-inf丢弃
cpp
// libgazebo_ros_camera.cpp 里的逻辑
impl_->min_depth_ = _sdf->Get<double>("min_depth", 0.4); // 默认 0.4m!
if (depth > impl_->min_depth_ && depth < impl_->max_depth_) {
/* 有效点 */
} else if (depth <= impl_->min_depth_) {
/* 整点设 -inf 丢弃 */
}- 而我们 ready 位姿下相机垂直朝下：离桌面 0.39m 高，到桌面上的物体也只有约 0.38~0.41m------大片桌面和物体表面落在 0.4m 之内，于是"近处的桌面和物体被自己的相机裁掉了"
- 注意这个参数必须写在
<plugin>内 ：插件Load收到的_sdf指向<plugin>元素(camera_name、frame_name都在这一层)。
xml
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<min_depth>0.02</min_depth>
<max_depth>3</max_depth>
</plugin>3-5 launch启动文件
- 把相机、octomap、QoS、光学帧都串起来，就是相机增强版
gazebo_camera.launch.py。它和 1-5 的gazebo.launch.py只有两处不同：xacro 映射多了enable_camera: 'true'（3-3 的相机块才会被展开）、move_group参数里多了 octomap 那一段（3-4-1 的数据源）。完整源码如下（大部分和 1-5 相同，只有开头映射、move_group 参数、结尾 world 声明三处是新增的）：
python
# gazebo_camera.launch.py ------ gazebo.launch.py 的相机增强版（第三期补章）
# 仅两处与 gazebo.launch.py 不同：
# 1) xacro mapping enable_camera=true -> 腕部(panda_hand)挂 RGBD 深度相机（眼在手）
# 2) move_group 增加 octomap 参数（PointCloudOctomapUpdater 吃 /camera/depth/points）
# 老 gazebo.launch.py 保持无相机、无 octomap 的原版基线。
import os
import re
import shutil
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def _fix_package_mesh_uris(urdf: str) -> str:
"""Gazebo classic 不解析 package:// 网格 URI（会当成 model:// 找），
替换成 file:// 绝对路径，RViz / MoveIt 同样可用。"""
panda_desc = get_package_share_directory('moveit_resources_panda_description')
return urdf.replace('package://moveit_resources_panda_description', 'file://' + panda_desc)
# 真实 panda/FR3 惯量（kg*m^2，主对角线 + 质心偏移，取 7 自由度臂同架构量级）。
# 之前注入扁平默认值 mass=1.0 / inertia=0.01 比真实值轻太多，位置控制器（默认
# Kp=100）在轻惯量下形成极限环振荡 -> 关节速度发散 -> 位置变 NaN -> Gazebo 里
# 机械臂消失（RViz 用模型显示所以还在）。换成真实量级后物理稳定。
REAL_INERTIA = {
'panda_link0': (2.3966, '0 0 0', '0.0090', '0.0115', '0.0085'),
'panda_link1': (2.9275, '0 -0.0181 -0.0386', '0.0239', '0.0225', '0.0064'),
'panda_link2': (2.9355, '0.0032 -0.0743 0.0088', '0.0419', '0.0251', '0.0617'),
'panda_link3': (2.2449, '0.0407 -0.0048 -0.0290', '0.0241', '0.0197', '0.0190'),
'panda_link4': (2.6156, '-0.0459 0.0630 -0.0085', '0.0345', '0.0289', '0.0413'),
'panda_link5': (2.3271, '-0.0016 0.0293 -0.0973', '0.0516', '0.0479', '0.0164'),
'panda_link6': (1.8170, '0.0597 -0.0410 -0.0102', '0.0054', '0.0141', '0.0161'),
'panda_link7': (0.6271, '0.0045 0.0086 -0.0162', '0.0002', '0.0002', '0.0001'),
'panda_link8': (0.0724, '0 0 0', '0.0001', '0.0001', '0.0001'),
'panda_hand': (0.73, '0 0 0', '0.001', '0.001', '0.001'),
'panda_leftfinger': (0.03, '0 0 0', '0.0001', '0.0001', '0.0001'),
'panda_rightfinger': (0.03, '0 0 0', '0.0001', '0.0001', '0.0001'),
}
def _add_inertials(urdf: str) -> str:
"""moveit_resources 的 panda.urdf 是 MoveIt 精简版，link 没有 <inertial>。
sdformat 的 URDF->SDF 转换会把无 inertial 的 link 整个丢弃（连带其 joint），
导致 spawn 出来的模型只有 gazebo 插件块、没有任何 link/joint ------ 表现为
gazebo_ros2_control 日志里所有关节 "Skipping joint ... not in the gazebo model"。
给每个缺 inertial 的 link 注入 REAL_INERTIA 表里的真实惯量（比扁平默认值重，
与控制器默认增益匹配，物理稳定不发散）。"""
fmt = ('<inertial><mass value="{m}"/><origin xyz="{o}" rpy="0 0 0"/>'
'<inertia ixx="{ixx}" ixy="0" ixz="0" iyy="{iyy}" iyz="0" '
'izz="{izz}"/></inertial>')
def _add(m):
tag = m.group(0)
nm = re.search(r'name="([^"]+)"', tag)
m_, o, ixx, iyy, izz = REAL_INERTIA.get(nm.group(1),
(1.0, '0 0 0', '0.01', '0.01', '0.01'))
block = fmt.format(m=m_, o=o, ixx=ixx, iyy=iyy, izz=izz)
# 自闭合 link（如 panda_link8）：<link name="x"/> -> <link name="x">...</link>
if tag.rstrip().endswith('/>'):
return tag[:-2] + '>' + block + '</link>'
return tag + block
return re.sub(r'<link\b[^>]*>', _add, urdf)
def _strip_mimic(urdf: str) -> str:
"""URDF 里 panda_finger_joint2 的 <mimic> 让 MoveIt robot model 在状态更新时
反复找内部名 'panda_finger_joint2_mimic' -> 每 10ms 刷一条 "Joint not found in model"。
去掉 <mimic>：MoveIt 把它当普通被动关节（SRDF 已声明 passive_joint）；
夹爪联动改由 ros2_control 的 mimic 参数负责（Gazebo 端不受影响）。"""
return re.sub(r'<mimic\b[^>]*/>', '', urdf)
def _swap_visual_meshes(urdf: str) -> str:
"""moveit_resources 的 panda .dae 网格在 Gazebo classic 的 OGRE 下渲染成
白色盒子/黑色（RViz 用同一文件能渲染，是 Gazebo 特有的问题）。换成官方
franka_description 的 FR3 网格（同为 7 自由度、文件名一致、OGRE 兼容），
Gazebo 与 RViz 都能正确显示。碰撞网格（STL）不换。"""
pnd = 'file:///opt/ros/humble/share/moveit_resources_panda_description/meshes/visual'
fr3 = 'file:///opt/ros/humble/share/franka_description/meshes/robot_arms/fr3/visual'
hand = 'file:///opt/ros/humble/share/franka_description/meshes/robot_ee/franka_hand_white/visual'
urdf = urdf.replace(pnd, fr3)
urdf = urdf.replace(fr3 + '/hand.dae', hand + '/hand.dae')
urdf = urdf.replace(fr3 + '/finger.dae', hand + '/finger.dae')
return urdf
def _fix_base(urdf: str) -> str:
"""panda.urdf 根 link 是 panda_link0、没有任何 world 固定关节（MoveIt 用 SRDF
虚拟关节代替）。但在 Gazebo 里这意味着底座是自由体------只能靠碰撞网格搁在
地面上，位置控制器在漂移的底座上打极限环 -> 关节抖动/偶发 NaN -> 机械臂
抽搐/消失。官方 Gazebo 配置都会给底座加 world 固定关节，这里补上。"""
base = ('<link name="world"/>'
'<joint name="panda_link0_joint" type="fixed">'
'<parent link="world"/><child link="panda_link0"/></joint>')
# 插在 <robot ...> 之后（要在 _add_inertials/_add_materials 之后调用，
# 这样 world link 不会被注入惯量/材质）
return re.sub(r'(<robot[^>]*>)', r'\1' + base, urdf, count=1)
def _add_materials(urdf: str) -> str:
"""panda.urdf 的 <visual> 只有 mesh、没有 <material> 颜色，Gazebo classic
会 fallback 到 .dae 自带材质，而这些 COLLADA 文件在 OGRE 下渲染成黑色 ->
机械臂看起来黑/看不见。给每个 visual 注入浅灰材质即可（MoveIt/RViz 同样可用）。"""
material = ('<material name="panda_light_grey">'
'<color rgba="0.83 0.83 0.83 1"/></material>')
def _add(m):
tag = m.group(0)
if tag.rstrip().endswith('/>'):
return tag[:-2] + '>' + material + '</visual>'
return tag + material
return re.sub(r'<visual\b[^>]*>', _add, urdf)
def _fix_controller_yaml_path(urdf: str) -> str:
"""gazebo_ros2_control 把 <parameters> 当作 --params-file 交给 rcl 解析，
workspace 路径含中文（非 ASCII）时 rcl_parse_arguments 会失败 -> 插件 Load
失败 -> gzserver 崩溃。把 ros2_controllers.yaml 复制到 ASCII 路径 /tmp 并替换。"""
src = os.path.join(
get_package_share_directory('panda_gazebo_bringup'),
'config', 'ros2_controllers.yaml')
dst = '/tmp/panda_ros2_controllers.yaml'
shutil.copy2(src, dst)
return urdf.replace(src, dst)
def generate_launch_description():
panda_bringup_dir = get_package_share_directory('panda_gazebo_bringup')
# ---- 1) robot_description：自建 gazebo xacro（eager 生成 URDF 字符串）----
# mappings 全为 str -> 进程内立即 xacro.process_file，返回可修改的 URDF 字符串
moveit_config = (
MoveItConfigsBuilder('moveit_resources_panda')
.robot_description(
file_path=os.path.join(panda_bringup_dir, 'config', 'panda_gazebo.urdf.xacro'),
mappings={'ros2_control_hardware_type': 'gazebo', 'enable_camera': 'true'},
)
.robot_description_semantic(file_path='config/panda.srdf')
.trajectory_execution(file_path='config/gripper_moveit_controllers.yaml')
.planning_pipelines(pipelines=['ompl', 'chomp', 'pilz_industrial_motion_planner'])
.to_moveit_configs()
)
# 关键修复（缺一不可，否则 gzserver 崩溃/模型不可见）：
# 1) 网格 URI package:// -> file:// 绝对路径（Gazebo classic 不解析 package://）
# 2) 参数文件复制到 ASCII 路径（中文 workspace 路径会让 rcl 解析 --params-file 失败）
# 3) 给无 inertial 的 link 注入默认惯性（sdformat 会丢弃无 inertial 的 link 连带关节）
# 4) 给 visual 注入浅灰材质（.dae 自带材质在 OGRE 下渲染成黑色，机械臂看不见）
# 5) spawn 直接从文件读完整 URDF：-topic 走 /robot_description 时，10KB+ 的大消息
# 在 FastDDS 下偶发只投递末尾片段 -> 模型无关节（关节全被插件跳过）。
fixed_urdf = _add_materials(
_fix_base(
_add_inertials(
_fix_controller_yaml_path(
_swap_visual_meshes(
_fix_package_mesh_uris(
_strip_mimic(
moveit_config.robot_description['robot_description'])))))))
moveit_config.robot_description = {'robot_description': fixed_urdf}
robot_urdf_file = '/tmp/panda_full.urdf'
with open(robot_urdf_file, 'w') as f:
f.write(fixed_urdf)
# ---- 2) Gazebo 抓取场景（默认 pick_place.world，可 world:= 覆盖）----
# 默认桌高/摆放按 ready 位姿 FK 定（末端 (0.307,0,0.59)、相机垂直朝下）：
# 桌面顶 z=0.20 才能让相机照到整片桌面；基线 gazebo.launch.py 仍是空世界。
# 换场景：ros2 launch ... gazebo_camera.launch.py world:=/path/to/xxx.world
# （./3_depth_gazebo.sh 已支持透传该参数）
default_world = os.path.join(panda_bringup_dir, 'config', 'pick_place.world')
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('gazebo_ros'),
'launch', 'gazebo.launch.py')
),
launch_arguments={'world': LaunchConfiguration('world')}.items(),
)
# ---- 3) 发布 /robot_description + 关节 TF（spawn_entity 从这里取 URDF）----
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[moveit_config.robot_description, {'use_sim_time': True}],
output='screen',
)
# ---- 4) world -> panda_link0 静态 TF（MoveIt 虚拟关节规划帧是 world）----
static_tf = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='static_transform_publisher',
arguments=['0', '0', '0', '0', '0', '0', 'world', 'panda_link0'],
output='log',
)
# ---- 5) move_group ----
move_group_node = Node(
package='moveit_ros_move_group',
executable='move_group',
output='screen',
parameters=[
moveit_config.to_dict(),
{
'use_sim_time': True,
'octomap_frame': 'world',
'octomap_resolution': 0.02,
# MoveIt 2(Humble) octomap 是扁平结构：'sensors' 是传感器名列表，
# 每个传感器名作前缀给 PointCloudOctomapUpdater 用。
'sensors': ['camera'],
'camera': {
'sensor_plugin': 'occupancy_map_monitor/PointCloudOctomapUpdater',
'point_cloud_topic': '/camera/depth/points',
'max_range': 3.0,
'point_subsample': 1,
'padding_offset': 0.01,
'padding_scale': 1.0,
'max_update_rate': 5.0,
'filtered_cloud_topic': 'filtered_cloud',
},
},
],
arguments=['--ros-args', '--log-level', 'info'],
)
# ---- 6) RViz（本包 moveit_camera.rviz = 系统 moveit.rviz + RGB图像/点云显示）----
# 相机话题是 best_effort（为 moveit octomap 设的），新增两个显示都在
# Topic 里把 Reliability Policy 设成了 Best Effort，否则 RViz 默认 Reliable 收不到。
rviz_config = os.path.join(
get_package_share_directory('panda_gazebo_bringup'),
'config', 'moveit_camera.rviz',
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config],
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.planning_pipelines,
moveit_config.robot_description_kinematics,
{'use_sim_time': True},
],
)
# ---- 7) 把 panda spawn 进 Gazebo ----
# spawn_entity.py 默认只等 30s 服务就绪，Gazebo 首次启动/DDS 发现偏慢时不够，
# 显式传 -timeout 60 放宽。
# 模型插入 -> gazebo_ros2_control 插件加载 -> controller_manager 起在 gzserver 内
# 从文件读 URDF（而非 -topic）：见上方 fixed_urdf 的注释。
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-file', robot_urdf_file, '-entity', 'panda',
'-timeout', '60'],
output='screen',
)
# ---- 8) spawn 成功退出 = 模型已插入 = controller_manager 已建立 ----
# 用 spawner（带 controller-manager-timeout）加载并激活三个控制器
load_joint_state_broadcaster = Node(
package='controller_manager',
executable='spawner',
arguments=['joint_state_broadcaster', '-c', '/controller_manager',
'--controller-manager-timeout', '30'],
output='screen',
)
load_arm_controller = Node(
package='controller_manager',
executable='spawner',
arguments=['panda_arm_controller', '-c', '/controller_manager',
'--controller-manager-timeout', '30'],
output='screen',
)
load_hand_controller = Node(
package='controller_manager',
executable='spawner',
arguments=['panda_hand_controller', '-c', '/controller_manager',
'--controller-manager-timeout', '30'],
output='screen',
)
return LaunchDescription([
DeclareLaunchArgument(
'world', default_value=default_world,
description='Gazebo world 文件；默认 pick_place.world（抓取场景），'
'可用 world:= 覆盖'),
gazebo,
robot_state_publisher,
static_tf,
move_group_node,
rviz_node,
spawn_entity,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_broadcaster, load_arm_controller,
load_hand_controller],
)
),
])- 上面的 launch 和 1-5 的
gazebo.launch.py是同一副骨架，_fix_*后处理、spawn、spawner 那段逐字相同。读者可以直接拿 1-5 的基线 launch 改：只加三处即可（enable_camera映射、octomap 参数、world:=声明） - 显示配置
moveit_camera.rviz（本包config/内）：在系统moveit.rviz基础上新增两个显示------RGB 图像和深度点云。因为相机 QoS 已降级成best_effort（3-4-1），两个显示在 Topic 里都把 Reliability Policy 设成Best Effort才收得到。这里给精简版（网格 + 轨迹 + octomap 场景 + RGB/点云，够用；完整版在工作区config/moveit_camera.rviz）：
yaml
Panels:
- Class: rviz_common/Displays
Name: Displays
- Class: rviz_common/Views
Name: Views
Visualization Manager:
Class: ""
Displays:
- Class: rviz_default_plugins/Grid
Enabled: true
Name: Grid
Reference Frame: <Fixed Frame>
- Class: moveit_rviz_plugin/Trajectory
Enabled: true
Name: Trajectory
Robot Description: robot_description
Trajectory Topic: /display_planned_path
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
Robot Description: robot_description
- Class: rviz_default_plugins/Image
Enabled: true
Name: Camera RGB
Topic:
Reliability Policy: Best Effort
Value: /camera/color/image_raw
- Class: rviz_default_plugins/PointCloud2
Enabled: true
Name: Camera PointCloud
Topic:
Reliability Policy: Best Effort
Value: /camera/depth/points
Use Fixed Frame: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: panda_link0
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
Transformation:
Current:
Class: rviz_default_plugins/TF
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 1.6
Focal Point:
X: 0
Y: 0
Z: 0
Name: Current View
Pitch: 0.4
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.8
Saved: ~
Window Geometry:
Camera RGB:
collapsed: false
Displays:
collapsed: false
Height: 900
Width: 1400
X: 0
Y: 0- 一键脚本
3_depth_gazebo.sh把 launch 包一层：先杀干净残留进程、source 环境，还支持可选 world 参数透传：
bash
#!/bin/bash
# 第三期补章：一键启动 Gazebo + MoveIt + 腕部 RGBD 相机（gazebo_camera.launch.py）
# gazebo.launch.py 的相机增强版：眼在手（panda_hand 挂 RGBD），move_group 带 octomap
# 之后再用 ./2_arm_control.sh 跑 C++ 控制节点
# 用法：
# ./3_depth_gazebo.sh # 默认抓取场景（pick_place.world）
# ./3_depth_gazebo.sh 场景.world # 换自定义 world（相对/绝对路径均可）
set -e
# ---- 启动前清理：杀掉所有残留进程，确保从干净状态启动 ----
echo "[cleanup] 清理残留进程 (gzserver / gzclient / move_group / rviz2 / controllers ...)"
for p in gzserver gzclient gazebo; do
pkill -x "$p" 2>/dev/null || true
done
# 进程名超过 15 字符时 pkill -x 匹配不到（如 robot_state_publisher），改用 ps 精确匹配；
# 过滤掉 grep 自身和 "bash -c"，避免误杀当前 shell / 上层包装（历史教训：曾把自己杀掉）
ps aux | grep -E "ros2 launch|spawn_entity|spawner|move_group|robot_state_publisher|static_transform_publisher|controller_manager|rviz2|arm_control_demo" \
| grep -v grep | grep -v "bash -c" \
| awk '{print $2}' | xargs -r kill -9 2>/dev/null || true
sleep 2
# source /home/lzh/anaconda3/etc/profile.d/conda.sh
# conda deactivate
source /opt/ros/humble/setup.bash
source "$(dirname "$0")/install/setup.bash"
export LANG=C.UTF-8 LC_ALL=C.UTF-8
# 可选 world 参数透传给 launch 的 world:=（launch 默认 pick_place.world）
WORLD="${1:-}"
if [ -n "$WORLD" ]; then
WORLD="$(realpath -m "$WORLD")"
echo "[world] 使用自定义场景: $WORLD"
ros2 launch panda_gazebo_bringup gazebo_camera.launch.py "world:=$WORLD"
else
echo "[world] 使用默认抓取场景 (pick_place.world)"
ros2 launch panda_gazebo_bringup gazebo_camera.launch.py
fi- 启动后打开 rviz2，可以看到左下角是 RGB 图像、深度点云，以及 octomap 八叉树
3-6 添加桌子
- 空世界里只有地面，抓取无从谈起。我们自建一个
pick_place.world：一张矮桌 + 三个彩色目标物体，通过 launch 的world:=参数加载 - 桌高不是随便定的------ready 位姿下相机垂直朝下，离目标面越近视野越小(视野半径 = 距离 * tan(40 度))，标准桌高会让桌面落在相机视野之外，所以用顶面 z=0.20 的矮桌，相机离桌面 0.39m，视野半径约 0.33m，刚好覆盖桌子表面
world
<?xml version="1.0" ?>
<!-- pick_place.world ------ 抓取演示场景（./3_depth_gazebo.sh 用）
设计约束来自 ready 位姿 FK：
panda 底座在 (0,0,0)，末端 panda_link8 在 (0.307,0,0.590)，相机垂直朝下
相机在 (0.385,0,0.590)，垂直朝下时离目标面越高视野越小（半径 = 距离*tan(40°)）
-> 桌子必须很矮（顶面 z=0.20），否则相机照不到整片桌面、octomap 建不出场景。
桌子放 (0.5,0)：相机离桌面 0.39m，视野半径 ~0.33m，能覆盖桌子表面大部分。 -->
<sdf version="1.6">
<world name="default">
<!-- 太阳 + 地面（同 gazebo_ros 的 empty.world） -->
<include><uri>model://sun</uri></include>
<include><uri>model://ground_plane</uri></include>
<!-- 矮桌：0.55(x) x 0.40(y) x 0.20(z)，顶面 z=0.20，木色，static 防被撞动 -->
<model name="table">
<static>true</static>
<pose>0.5 0 0.10 0 0 0</pose>
<link name="table_link">
<collision name="collision">
<geometry><box><size>0.55 0.40 0.20</size></box></geometry>
<surface><friction><ode><mu>1.0</mu><mu2>1.0</mu2></ode></friction></surface>
</collision>
<visual name="visual">
<geometry><box><size>0.55 0.40 0.20</size></box></geometry>
<material>
<ambient>0.55 0.35 0.15 1</ambient>
<diffuse>0.55 0.35 0.15 1</diffuse>
</material>
</visual>
</link>
</model>
<!-- 目标 1：红色方块 4cm，底面略高于桌面(0.20)让它落稳 -->
<model name="red_cube">
<pose>0.46 -0.10 0.205 0 0 0</pose>
<link name="link">
<inertial>
<mass>0.05</mass>
<inertia ixx="1.3e-5" ixy="0" ixz="0" iyy="1.3e-5" iyz="0" izz="1.3e-5"/>
</inertial>
<collision name="collision">
<geometry><box><size>0.04 0.04 0.04</size></box></geometry>
<surface><friction><ode><mu>0.8</mu><mu2>0.8</mu2></ode></friction></surface>
</collision>
<visual name="visual">
<geometry><box><size>0.04 0.04 0.04</size></box></geometry>
<material>
<ambient>0.85 0.15 0.15 1</ambient>
<diffuse>0.85 0.15 0.15 1</diffuse>
</material>
</visual>
</link>
</model>
<!-- 目标 2：绿色圆柱 r=3cm h=8cm -->
<model name="green_cylinder">
<pose>0.52 0.09 0.245 0 0 0</pose>
<link name="link">
<inertial>
<mass>0.05</mass>
<inertia ixx="3.8e-5" ixy="0" ixz="0" iyy="2.2e-5" iyz="0" izz="3.8e-5"/>
</inertial>
<collision name="collision">
<geometry><cylinder><radius>0.03</radius><length>0.08</length></cylinder></geometry>
<surface><friction><ode><mu>0.8</mu><mu2>0.8</mu2></ode></friction></surface>
</collision>
<visual name="visual">
<geometry><cylinder><radius>0.03</radius><length>0.08</length></cylinder></geometry>
<material>
<ambient>0.2 0.7 0.2 1</ambient>
<diffuse>0.2 0.7 0.2 1</diffuse>
</material>
</visual>
</link>
</model>
<!-- 目标 3：蓝色方块 5cm -->
<model name="blue_cube">
<pose>0.56 -0.03 0.225 0 0 0</pose>
<link name="link">
<inertial>
<mass>0.08</mass>
<inertia ixx="3.3e-5" ixy="0" ixz="0" iyy="3.3e-5" iyz="0" izz="3.3e-5"/>
</inertial>
<collision name="collision">
<geometry><box><size>0.05 0.05 0.05</size></box></geometry>
<surface><friction><ode><mu>0.8</mu><mu2>0.8</mu2></ode></friction></surface>
</collision>
<visual name="visual">
<geometry><box><size>0.05 0.05 0.05</size></box></geometry>
<material>
<ambient>0.15 0.4 0.8 1</ambient>
<diffuse>0.15 0.4 0.8 1</diffuse>
</material>
</visual>
</link>
</model>
</world>
</sdf>
- 然后让 launch 支持
world:=参数(默认pick_place.world)，一键脚本把场景路径透传过去：
python
# gazebo_camera.launch.py 里 Gazebo 场景部分
default_world = os.path.join(panda_bringup_dir, 'config', 'pick_place.world')
DeclareLaunchArgument(
'world', default_value=default_world,
description='Gazebo world 文件;默认 pick_place.world,可用 world:= 覆盖')
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(...), # gazebo_ros/launch/gazebo.launch.py
launch_arguments={'world': LaunchConfiguration('world')}.items(),
)- 一键脚本就是 3-5 里的
3_depth_gazebo.sh：本身不写死场景，只把参数透传给 launch 的world:=。想换自定义场景就./3_depth_gazebo.sh 你的.world；想回空世界就传绝对路径./3_depth_gazebo.sh /opt/ros/humble/share/gazebo_ros/worlds/empty.world（脚本里realpath会先转成绝对路径，所以不能只写包内相对名gazebo_ros/worlds/empty.world------那会被拼成当前目录下并不存在的路径）
3-7 测试
- 场景就绪，依次执行三步，机械臂就会绕开桌子和物体规划过去，octomap 实时更新
bash
./3_depth_gazebo.sh
./2_arm_control.sh
./4_send_target.sh 0.46 -0.10 0.40 1 0 0 0 # 红方块上方- 移动机器人，发现避开障碍物，octomap 也在实时更新
总结
- 本文从 Gazebo 对接 MoveIt2 讲起，到 ros2_control 真正驱动机械臂、编写目标位姿控制节点，最后给腕部接上 RGBD 深度相机，把点云接进 octomap 并搭出抓取场景，整套"规划-避障-抓取"链路完整跑通
- 核心要点回顾：
- Gazebo 对接 MoveIt2 ：
gazebo_ros2_control插件 +controller_manager，MoveIt 规划、Gazebo 执行 - URDF 转 SDF 的三个坑 ：无惯量丢 link、
package://不解析、OGRE 渲染黑色，靠 launch 后处理兜底 - 眼在手深度相机 ：挂在
panda_hand腕部，补camera_optical_frame解决 z-forward 点云被标到 x-forward 帧上的"竖直墙"问题 - octomap 对接 ：相机 QoS 降级
best_effort对齐SensorDataQoS()，数据源指到/camera/depth/points - 最小测量距离 ：插件的
min_depth默认 0.4m，必须写在<plugin>内，缩到 0.02m 才能测到近处的桌面和物体 - 抓取场景 ：
pick_place.world矮桌 + 三个目标，world:=参数可随时换场景
- Gazebo 对接 MoveIt2 ：
- 如有错误，欢迎指出！
- 感谢观看！