简介：项目基于ROS2 Humble框架，融合MoveIt2运动规划与C语言实时控制，为三维龙门式机器人提供一套完整的行为树架构控制方案，面向机器人开发者、自动化专业学生及工业应用工程师，可用于物料搬运、装配等场景的智能化改造。资源包共53个文件，以Python脚本、C++头文件与源文件、YAML配置、Xacro模型、XML描述为主，另有服务定义、SRDF、RViz配置和Markdown说明等，体积仅158KB。各类型文件分工明确：脚本与源码实现控制逻辑，配置与模型文件支撑MoveIt2规划，文档则梳理了从环境搭建到行为树管理的要点。目前已有28人学习。该资源体量小而结构完整，适合快速理解ROS2_Humble下MoveIt2与C语言结合的实现思路，并为二次开发提供基础模板。

1. 三维龙门式机器人用 ROS2_Humble 接 MoveIt2 与 C 语言：值不值得把控制权交给一个“机械臂框架”

一台三维龙门式机器人要接入 ROS2_Humble，用 MoveIt2 做运动规划，再用 C 语言写业务控制，最后用行为树_Behavior_Trees 管理整条任务流——这套组合把机械臂领域的成熟工具往直角坐标机器人上搬，很多人第一步就卡住：MoveIt2 默认被旋转关节的思维惯坏了，龙门三个平动关节怎么建模、怎么规划、怎么在 Gazebo 里不抖，全是细节。我按自己落地过的一套做法，把从 URDF 建模、move_group 启动、C/C++ 混编、行为树接线到仿真验证的链路拆开，直接给参数和命令。适合正在写龙门或直角坐标机器人控制代码的工程师和研究生，也适合想给传统 PLC 式顺序控制加一层“规划大脑”的人。

2. 在 ROS2_Humble 里把龙门机器人送进 MoveIt2：URDF、SRDF 与 move_group 三个必调参数

2.1 龙门机器人 URDF：三个平动关节的建模与惯性参数

先说一个最容易理解错的地方：龙门机器人不是“机器人臂”，但 MoveIt2 眼里它就是一根由三个 prismatic joint（平动关节）组成的机械臂。所以 URDF 里不要出现 revolute 关节，轴方向就是 X、Y、Z 三个向量。下面是一份可以跑通的最小 URDF，x 轴是横梁方向，y 轴是深度方向，z 轴是竖直升降方向。

<?xml version="1.0"?> <robot name="gantry_3d"> <link name="base"> <inertial> <mass value="50.0"/> <origin xyz="0 0 0" rpy="0 0 0"/> <inertia ixx="0.5" iyy="0.5" izz="0.5" ixy="0" ixz="0" iyz="0"/> </inertial> </link> <link name="x_carriage"/> <joint name="joint_x" type="prismatic"> <parent link="base"/> <child link="x_carriage"/> <origin xyz="0 0 0" rpy="0 0 0"/> <axis xyz="1 0 0"/> <limit lower="-0.8" upper="0.8" effort="200" velocity="0.5"/> </joint> <link name="y_carriage"/> <joint name="joint_y" type="prismatic"> <parent link="x_carriage"/> <child link="y_carriage"/> <origin xyz="0 0 0" rpy="0 0 0"/> <axis xyz="0 1 0"/> <limit lower="-0.6" upper="0.6" effort="200" velocity="0.5"/> </joint> <link name="z_carriage"/> <joint name="joint_z" type="prismatic"> <parent link="y_carriage"/> <child link="z_carriage"/> <origin xyz="0 0 0" rpy="0 0 0"/> <axis xyz="0 0 1"/> <limit lower="-0.5" upper="0.3" effort="150" velocity="0.3"/> </joint> <link name="tool0"/> <joint name="joint_tool" type="fixed"> <parent link="z_carriage"/> <child link="tool0"/> <origin xyz="0 0 -0.2" rpy="0 0 0"/> </joint> </robot>

这段 URDF 里我故意把 z 轴的 velocity 设成 0.3 而不是 0.5，因为竖直轴要对抗重力，升降速度太猛会在起停瞬间让横梁震颤。effort 参数在纯运动规划里不参与计算，但进入 ros2_control 或 Gazebo 后会作为电机力矩上限，龙门机器人大惯量结构对这个值很敏感，给太小会直接导致仿真里下降时掉不下去。

SRDF 是 MoveIt2 规划组的关键，描述哪个 joint 属于哪个规划组。注意规划组要包含的是关节，不是 link；end effector 组才放 link。用 MoveIt Setup Assistant 可以从 URDF 自动生成，也可以手写，内容本质是这个结构：

<robot name="gantry_3d"> <group name="gantry_arm"> <joint name="joint_x"/> <joint name="joint_y"/> <joint name="joint_z"/> </group> <group name="effector"> <link name="tool0"/> </group> </robot>

规划组名 gantry_arm 会出现在所有 move_group 接口调用里，建议一上来就固定好，后面改名字除了代码要跟着改，行为树 XML 里的 group 映射也要改，纯属给自己增加工作量。

控制器配置是另一个容易漏的环节。move_group 只做轨迹规划，轨迹执行要靠 ros2_control 的 follow_joint_trajectory 控制器。龙门机器人三个平动轴共用一个控制器即可，moveit_controllers.yaml 里这样写：

controller_names: - gantry_arm_controller gantry_arm_controller: action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true joints: - joint_x - joint_y - joint_z

action_ns 必须是 follow_joint_trajectory，因为 move_group 通过 action 接口把轨迹发给控制器。有些教程里写 trajectory_controller 让新手以为是任意名字，实际会被 move_group 忽略，最后出现“规划成功但机器不动”的怪现象。

2.2 用 launch.py 拉起 move_group 规划链

URDF、SRDF、控制器配置都齐了，接下来就是把 move_group 节点跑起来。常见做法是写一个 launch.py，把 robot_description 以字符串参数传给节点，注意这里传的是 URDF 文本内容，不是文件路径，这个区别是新手最容易翻车的地方。

from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare def generate_launch_description(): urdf_path = LaunchConfiguration( 'urdf_path', default=FindPackageShare('gantry_description') + '/urdf/gantry.xacro' ) robot_description = ParameterValue( Command(['xacro ', urdf_path]), value_type=str ) return LaunchDescription([ DeclareLaunchArgument( 'urdf_path', default_value=FindPackageShare('gantry_description') + '/urdf/gantry.xacro' ), Node( package='moveit_ros_move_group', executable='move_group', name='move_group', output='screen', parameters=[{'robot_description': robot_description}], ), ])

这个 launch 里还没有带 SRDF 和控制器参数，实际工程中 move_group 的 parameters 里需要同时给 robot_description、robot_description_semantic 和 trajectory_execution 三块，否则节点起来后 RViz2 里看不到规划组，或者执行轨迹时找不到控制器。Command 里用 xacro 而不是直接读 urdf，是因为工程里通常用 xacro 做尺寸复用，直接给 urdf 文件也可以，但 xacro 是默认习惯。

看到 move_group 正常起来后，终端会打印它加载的规划组列表。如果 gantry_arm 没出现，先回去检查 SRDF 的 group 名和 URDF 的 joint 名是否完全一致，错一个字符都不会加载。

2.3 三个必调参数：规划器、规划时长与速度缩放

MoveIt2 里 move_group 的规划行为由一组参数控制，龙门机器人自由度少、运动空间规则，参数和六轴机械臂的设置逻辑很不一样。我习惯把下面这三个先调好，再谈避障和轨迹优化。

规划器是在 ompl_planning.yaml 里按规划组指定的，不是随便在代码里改个字符串就生效。RRTConnect 对龙门这种空旷的矩形工作空间很合适；如果你的任务里有密集障碍物，比如要在货架格子里取放，换成 LazyPRM 预计算路标图会更稳。planning_time 设短一点不是限制规划能力，而是让失败反馈来得更及时，行为树可以快速进入重试分支。

max_velocity_scaling_factor 被很多人忽略，但龙门机器人最容易出问题的就是高速下横梁共振。我一般先设 0.3 跑通流程，确认轨迹平滑后再逐步往上提。还有个容易被误解的参数是 eef_step，它属于笛卡尔路径而不是普通规划，默认 0.01 表示末端每走 1 厘米做一次逆解采样。龙门机器人三个平动轴的逆解是解析解，eef_step 甚至可以设到 0.02，计算负担小得多。

如果任务是直线涂胶或 3D 打印这类必须走直线的场景，不要用 setPoseTarget 加 plan()，MoveIt2 默认在关节空间规划，三个轴同时匀速运动的结果在笛卡尔空间是一条斜线而不是直线。要用 computeCartesianPath，这个我在后面避坑章节专门展开。

3. 用 C 语言接 MoveIt2 控制：rclc 与 C++ 桥接的混编工程

3.1 纯 C 写不了 MoveIt2 API，先划清 C 与 C++ 的边界

看到标题里“C 编程语言实现运动规划与控制”，我得先把丑话说在前面：MoveIt2 的规划核心是 C++ 写的，move_group_interface、moveit_core、OMPL 和 FCL 全是 C++ 库，没有官方 C API。ROS2_Humble 的 C 客户端库 rclc 确实存在，能让你用纯 C 写节点、发话题、调服务，但 MoveIt2 的规划能力摸不到。

所以实际工程里“用 C 实现”通常是三种做法。第一种是 C 写业务逻辑层，C++ 写一个桥接节点，两者通过 ROS2 的 topic/service 通信；第二种是把 C++ 的 MoveIt2 封装成 extern "C" 动态库，C 主程序通过这个库调用；第三种是项目本身就是 C/C++ 混编，C 负责底层 IO 和急停，C++ 负责规划和行为树。大多数情况下是第一种加第二种的组合。

这个边界必须在第一天就告诉团队，否则很容易出现有人用 C 硬写运动规划写了两周还在跟链接器搏斗。C 语言在龙门机器人项目里的合理位置是：触摸屏上位机通信、吸盘/夹具 IO 控制、急停逻辑、传感器采集，以及把指令翻译成 MoveIt2 能理解的目标位姿。

顺便提一句开发环境，VSCode 配置 C/C++ 混编工程时，要分别给 .c 文件设 C11、.cpp 文件设 C++17，否则 MoveIt2 的头文件会报一堆语法错误。这一个配置问题能让整个工程在编辑器和编译器的两套思维之间反复横跳，提前把 c_cpp_properties.json 里的 standards 分开设好，能省很多无效排错时间。

3.2 最小 C 节点：rclc 发布目标位姿

先用一个纯 C 节点解决“C 侧怎么把目标坐标交给规划侧”的问题。这里用 rclc 初始化一个节点，发布 geometry_msgs/PoseStamped 到 /goal_pose，C++ 桥接节点订阅这个主题后调用 MoveIt2。这段代码是个骨架，但能编译能跑，常被我用做 C 侧通道的自检程序。

#include <rclc/rclc.h> #include <rclc/executor.h> #include <geometry_msgs/msg/pose_stamped.h> #include <stdio.h> #include <string.h> #include <stdlib.h> int main(int argc, char *argv[]) { rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; if (rclc_support_init(&support, argc, argv, &allocator) != RCL_RET_OK) { fprintf(stderr, "rclc init failed\n"); return -1; } rcl_node_t node = rcl_get_zero_initialized_node(); if (rclc_node_init_default(&node, "gantry_c_cmd", "", &support) != RCL_RET_OK) return -1; rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); if (rclc_publisher_init_default( &pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, PoseStamped), "/goal_pose") != RCL_RET_OK) return -1; geometry_msgs__msg__PoseStamped goal; memset(&goal, 0, sizeof(goal)); // C 结构体的字符串字段需要手动分配内存 goal.header.frame_id.data = malloc(8); strcpy(goal.header.frame_id.data, "/map"); goal.header.frame_id.size = strlen("/map"); goal.header.frame_id.capacity = 8; goal.pose.position.x = 0.25; goal.pose.position.y = 0.10; goal.pose.position.z = -0.10; goal.pose.orientation.w = 1.0; // 其余四元数分量为 0，表示无旋转 rcl_publish(&pub, &goal, NULL); free(goal.header.frame_id.data); rcl_publisher_fini(&pub, &node); rcl_node_fini(&node); rclc_support_fini(&support); return 0; }

这里最值得注意的不是发布函数，而是 C 消息结构体的内存管理。PoseStamped 里的 header.frame_id 是一个带 data、size、capacity 的字符串结构体，不能直接赋值字符串常量，必须自己 malloc 并设置 capacity。很多 C 节点写代码时用 strcpy 进去但忘了给 capacity，桥接节点订阅后解析字符串时直接越界读，这是 C 侧最常见的内存 bug。

CMAKE 里需要 find_package(rclc) 和 find_package(geometry_msgs)，然后 ament_target_dependencies 把两个依赖链进目标。这个节点本身不做规划，它只解决一件事：让 C 程序拥有往 ROS2 世界发指令的能力。规划侧收到这个目标位姿后，经过 TF 变换和路径规划，再发布轨迹。

3.3 把 MoveIt2 调用封装成 extern "C" 接口

如果 C 侧想直接同步拿到规划结果，更高效的做法是给 MoveIt2 包一层 C 接口。这个封装层用 C++ 写，编译成动态库，对外暴露纯 C 函数。注意接口边界上只能传基本类型和 char*，任何 std::string、std::vector 都不能越过 extern "C" 边界返回。

#include "moveit/move_group_interface/move_group_interface.h" extern "C" { typedef struct { moveit::planning_interface::MoveGroupInterface* mgi; } GantryPlanner; GantryPlanner* gantry_planner_new(const char* group_name) { auto* p = new GantryPlanner; p->mgi = new moveit::planning_interface::MoveGroupInterface(group_name); return p; } void gantry_planner_delete(GantryPlanner* p) { delete p->mgi; delete p; } int gantry_planner_set_pose_target(GantryPlanner* p, const char* frame, double x, double y, double z, double qx, double qy, double qz, double qw) { p->mgi->setPoseTarget( moveit::core::RobotState(...), // 实际用 setPoseTarget(pose, frame) 即可 frame); return 0; } int gantry_planner_plan_and_execute(GantryPlanner* p) { moveit::planning_interface::MoveGroupInterface::Plan plan; bool ok = p->mgi->plan(plan); if (!ok) return -1; p->mgi->execute(plan); return 0; } }

这段代码展示的是封装边界，不是完整可编译实现，setPoseTarget 的具体参数在接入真实工程时要按 move_group_interface 的 API 补全。核心原则是：new 出来的指针不跨边界返回，所有 C++ 对象都藏在结构体指针后面；返回值只用 int 表示成功失败；如果要传错误信息，用 int gantry_planner_get_last_error(char* buf, int buf_len) 这种缓冲区拷贝方式带出去。

CMake 侧的链接也和纯 C 不同，要链接 moveit_ros_planning_interface 提供的 cmake target，而不是手动找 .so 文件。这个封装库建议编译成 SHARED，C 主程序在编译期只依赖一个头文件，运行期通过 LD_LIBRARY_PATH 找到动态库。

3.4 C 主程序与行为树的衔接方式

封装好 C 接口之后，C 主程序就可以很干净地做事了：读传感器、判断状态、调用 gantry_planner_set_pose_target 和 gantry_planner_plan_and_execute。但注意这个封装是同步阻塞的，规划加执行可能要好几秒，如果 C 侧需要在这期间响应急停信号，就不能在主线程里干等。

常见做法是把规划执行放到单独线程，主线程只跑急停监控和 IO。行为树进来之后，C 主程序可以退化成“行为树节点的执行后端”，也就是树上的 MoveToXyz 节点内部去调 gantry_planner_plan_and_execute。这样做的好处是任务流逻辑全部上树，C 代码只剩原子动作，后续加分支不用再改 C 代码。

关于同步与异步的边界，我建议封装层只提供同步接口，异步交给调用侧自己做线程包装。C 的 pthread 在行为树里调度起来足够用，强行在封装层做异步反而让状态管理变得复杂。

4. 行为树_Behavior_Trees 接管任务流：节点封装、XML 与 ROS2 驱动

4.1 为什么龙门任务更适合行为树而不是状态机

龙门机器人做上下料或分拣时，任务流看起来是固定的“回原点、去取料、抓取、去放料、释放”，很多人第一反应是写个有限状态机。但状态机有一个让人头疼的问题：异常分支越多，状态转移图越乱。规划失败要重试，吸盘没吸住要重新抓取，急停恢复后要回安全位，这些分支在状态机里会变成爆炸式增长的状态和迁移条件。

行为树的思路完全不同。树的每个节点执行完返回 SUCCESS、FAILURE 或 RUNNING，Sequence 表示顺序执行且任何一个失败就中断，Fallback 表示失败后尝试下一个分支，装饰器可以加重试、超时、反转这类策略。任务流的骨架和异常恢复策略直接在 XML 里表达，不需要写成状态转移表。

行为树在 ROS2_Humble 里不是 MoveIt2 的组成部分，它是个独立库，常见做法是通过 behavior_tree_cpp_v3 这个包引入，与 MoveIt2 只通过我们自己的 C 接口产生联系。这也是为什么标题里的“C 语言”和“行为树”能共存：树本身由 C++ 库驱动，但叶子节点内部的执行动作可以全部走 C 接口。

4.2 把 MoveToXyz 封装成行为树异步动作节点

行为树节点和普通函数最大的区别是它必须响应 tick。规划加执行是一段耗时操作，如果用同步节点，整个树会被卡住几秒钟，其他需要并行监控的节点就失去了响应能力。所以 MoveToXyz 应该继承 ThreadedAction，让行为树执行器在 tick 到这个节点时单独起线程跑，树本身保持可响应。

#include "behaviortree_cpp/action_node.h" #include "behaviortree_cpp/bt_factory.h" class MoveToXyz : public BT::ThreadedAction { public: explicit MoveToXyz(const std::string& name, const BT::NodeConfig& config) : BT::ThreadedAction(name, config) {} static BT::PortsList providedPorts() { return { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("z") }; } BT::NodeStatus tick() override { double x = getInput<double>("x").value(); double y = getInput<double>("y").value(); double z = getInput<double>("z").value(); // gantry_plan_and_execute 来自 extern "C" 封装层 int ret = gantry_plan_and_execute_to_xyz(x, y, z); return ret == 0 ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; } };

InputPort 是行为树黑板的输入端口，数值从 XML 里由父节点传入，也可以在其他节点里通过黑板写入。这里我没写 getInput 失败的处理，实际工程里如果端口没值，getInput 返回 nullopt，直接 return FAILURE 比抛异常安全，因为行为树框架不会替你做空值决策。

ThreadedAction 的语义是：tick() 被放在内部线程执行，返回 SUCCESS 或 FAILURE 之前，节点状态是 RUNNING。这个设计让行为树执行器不会阻塞在单个节点上，但也意味着同一个时间点可能有两个动作节点在并行跑，龙门机器人场景里要小心，别让 MoveToXyz 和回原点同时执行，两个线程同时下发轨迹会给控制层造成竞争。

4.3 用 XML 描述带重试与回退的龙门任务流

有了叶子节点，任务流的上层控制就全部交给 XML。下面这个树描述的是最典型的上下料流程：先回家，再移动到取料点，夹紧，移动到放料点，松开。看起来和顺序代码没什么区别，关键的差异在第二个树里。

<root main_tree_to_execute="MainTree"> <BehaviorTree ID="MainTree"> <Sequence name="pick_and_place"> <MoveHome/> <MoveToXyz x="0.2" y="0.1" z="-0.2"/> <GripperClose/> <MoveToXyz x="0.4" y="-0.1" z="-0.2"/> <GripperOpen/> </Sequence> </BehaviorTree> </root>

Sequence 的语义是“逐个执行，遇到 FAILURE 就终止”，当 MoveToXyz 因为规划超时返回 FAILURE，整个序列在当前位置停下来，这在实际产线上是不可接受的。所以要引入 Fallback 和重试装饰器。第二个树演示了失败恢复：取料失败时先重试三次，三次都失败就回到安全位置并重启任务。

<root main_tree_to_execute="MainTreeWithRecovery"> <BehaviorTree ID="MainTreeWithRecovery"> <Sequence name="pick_place_recovery"> <MoveHome/> <Fallback name="pick_or_retry"> <RetryUntilSuccesful num_attempts="3"> <DoPick/> </RetryUntilSuccesful> <MoveHome/> <DoPick/> </Fallback> <MoveToXyz x="0.4" y="-0.1" z="-0.2"/> <GripperOpen/> </Sequence> </BehaviorTree> </root>

这里 Fallback 的左分支是“重试三次 DoPick”，如果三次都失败，Fallback 会尝试右分支：回原点再抓一次。这个“回退到安全点再重来”的模式，在状态机里要写一组完整的状态迁移，在行为树里只是加两个节点的事。注意 RetryUntilSuccesful 的拼写来自 BehaviorTree.CPP 的内置装饰器节点，不同版本可能略有差异，引入库后先用 factory 打印注册表确认一次名称。

4.4 在 ROS2 节点里周期 tick 行为树

行为树不是独立运行的，需要有人周期性调用 tick。ROS2_Humble 里最省事的方式是用 create_wall_timer 定时驱动整棵树。tick 频率我一般用 50 毫秒，对龙门机器人这种慢速重载设备足够，太高的频率反而会让短时间内多次读取同一状态，徒增 CPU 开销。

#include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" class GantryBtNode : public rclcpp::Node { public: GantryBtNode() : Node("gantry_bt_node") { BT::BehaviorTreeFactory factory; factory.registerNodeType<MoveToXyz>("MoveToXyz"); factory.registerNodeType<MoveHome>("MoveHome"); factory.registerNodeType<DoPick>("DoPick"); factory.registerNodeType<GripperClose>("GripperClose"); factory.registerNodeType<GripperOpen>("GripperOpen"); tree_ = std::make_shared<BT::Tree>( factory.createTreeFromFile("gantry_bt.xml")); tree_timer_ = this->create_wall_timer( std::chrono::milliseconds(50), [this]() { tree_->tickOnce(); }); } private: std::shared_ptr<BT::Tree> tree_; rclcpp::TimerBase::SharedPtr tree_timer_; };

tickOnce 会从根节点开始执行整棵树，内部通过线程池调度 ThreadedAction。这里的坑是 tree_ 必须用 shared_ptr 持有，如果用栈对象然后被 lambda 捕获地址，节点还没执行完树就析构了，会让行为树框架随机崩溃。这个崩溃很玄学，因为和时序相关，我只能建议一步到位用 shared_ptr。

createTreeFromFile 读的是 XML 文件路径，这个文件建议放到安装目录的 share 下，不要用相对路径依赖当前工作目录，否则用 ros2 launch 启动时工作目录一变，树就加载不到了。

注意：行为树节点内部不要直接调用 rclcpp 的 spin，所有 ROS2 通信都走外部节点上下文，树只做决策。把 IO 和决策分开，后面排查问题会轻松很多。

5. 集成避坑：龙门轴同步、直线路径与 C/C++ 桥接的四个易翻车点

5.1 上电不回原点直接规划，轨迹起点全乱

现象：设备重启后直接下发目标位姿，机械结构先是像抽搐一样快速冲向行程极限，然后才慢悠悠往目标走，有时直接触发硬限位报警。

原因：龙门机器人大多用增量编码器，断电后零点丢失。MoveIt2 规划的起点来自当前关节位置反馈，如果机器人实际在机械原点附近，而某根轴的编码器计数值还是断电前的位置，move_group 以为起始点在行程中间，规划出的轨迹自然是从错误起点出发的。行为树逻辑里没有把“回原点”放在最高优先级，是另一个间接原因。

解决：行为树的第一个节点必须是回原点，并且这个节点要确认限位开关信号真的触发过，而不能只把关节角设置为 0。我习惯在回原点节点内部先低速把各轴往负方向运动，直到读到三个限位开关的闭合信号，然后置位零点。这一步做扎实，MoveIt2 的规划起点才有意义。

5.2 双驱龙门 X 轴不同步，MoveIt2 不报错但机械卡顿

现象：plan 和 execute 都返回成功，但横梁在运动过程中发出吱嘎声，偶尔伴随过流报警，严重时龙门会卡死在一个位置。

原因：大跨距龙门 X 轴通常由左右两个电机同步驱动，MoveIt2 规划的 joint_x 是一个虚拟轴，它只代表横梁的平移，不知道左右电机实际位置。左右电机的位置环跟踪如果存在恒定偏差，横梁就被拧着，机械应力全施加在导轨和连轴器上。

解决：控制器的同步位置误差要单独保护，左右编码器位置差超过阈值（我一般设 0.5 到 1 毫米）立即急停。行为树里加一个 SyncCheck 节点，在回原点和每个动作之间检查同步误差。这个节点不需要 MoveIt2，直接读两个电机的反馈即可。MoveIt2 侧不用改，因为它是规划层，不该管电机级同步。

5.3 调用 plan() 以为走直线，实际拉出一条弧线

现象：从 A 点移动到 B 点，RViz2 里看轨迹不是直线，而是一条平滑曲线，对涂胶和打印类任务这完全不可接受。

原因：MoveIt2 默认在关节空间规划，龙门三个平动轴的关节空间和笛卡尔空间虽然映射简单，但 RRTConnect 不会保证末端走直线。开发者用了 setPoseTarget 加 plan()，心里以为目标是“直线走到那”，实际规划器在关节空间做插值，路径是三个轴速度曲线的合成结果。

解决：强制走直线要用 computeCartesianPath，它按末端步长采样并逐点做逆解，返回的 fraction 表示路径覆盖率。下面这段代码里 eef_step 是末端采样步长，jump_threshold 设 0 表示不检查关节跳跃限制，适合龙门这类无奇异姿态的结构。

std::vector<geometry_msgs::msg::Pose> waypoints; waypoints.push_back(start_pose); waypoints.push_back(end_pose); double fraction = mgi->computeCartesianPath( waypoints, // 路径点列表 0.01, // eef_step：末端每 1 厘米采样一次 0.0 // jump_threshold：不检查关节跳跃 ); if (fraction < 0.9) { // 路径没有完全解出，fraction 是完成比例 }

fraction 低于 0.9 时不要执行轨迹，先检查 waypoints 是否在运动空间内，或者中间有没有碰撞物体。龙门机器人的工作空间是规则长方体，这个检查可以用机械臂场景里常用的 isPathValid 接口，但更快的办法是直接用 URDF 的关节 limit 手动过滤一遍目标点。

5.4 extern "C" 接口返回 std::string，C 侧内存崩溃

现象：C 主程序调用封装库拿到错误信息，第一次打印正常，第二次调用就段错误，或者字符串变成乱码。跟踪时发现崩溃点不在规划逻辑里，而在打印字符串时。

原因：extern "C" 边界上返回了 std::string，C 侧按 char* 接收，内存所有权跨了 C++ 和 C 两个运行时边界。std::string 的内部缓冲区由 C++ 分配器管理，C 侧没有对应的析构逻辑，一旦 C++ 侧先释放，C 侧指针就悬空了。这是混编里最典型的血泪经验。

解决：C 接口只返回 int 或 void，错误信息通过调用方提供的缓冲区带出去。封装层内部把 std::string 拷贝到 char 数组里，缓冲区由 C 侧分配和释放。这样内存生命周期清晰，谁分配谁释放，不跨边界。

extern "C" int gantry_planner_get_last_error(char* buf, int buf_len) { strncpy(buf, g_last_error.c_str(), buf_len - 1); buf[buf_len - 1] = '\0'; return 0; }

5.5 rclc 发的 PoseStamped 没有时间戳，move_group 说 Frame not found

现象：C 节点发的目标位姿被桥接节点收到，但调用 MoveIt2 时报 TF 查询失败，提示目标坐标系找不到。

原因：C 语言里初始化消息容易漏字段，header.frame_id 的字符串内存没分配，或者 header.stamp 还是全 0。MoveIt2 内部要先从目标坐标做 TF 变换到规划坐标系，时间戳为 0 的消息在 TF 缓存里查不到对应时刻的变换，于是报未找到坐标系错误。

解决：按 3.2 节的代码把 frame_id 的字符串内存分配好，并用 ROS2 时钟填写真实时间戳，不要从 C 库函数拿系统时间。在 rclc 里可用 rcl_clock 接口获取当前时间填充 stamp，仿真环境下要确认时钟是 /use_sim_time 管理的仿真时间，否则和 Gazebo 的时钟不同步，同样查不到 TF。

6. 验证方法：RViz2 与 Gazebo 同步对比，把行为树一次跑透

验证流程我习惯分三步走，顺序错了会多花好几倍时间。第一步先在 RViz2 里手动用 MoveIt2 插件拖拽末端目标，看规划轨迹是否合理、是否穿过工件；这一步只验证纯规划，不碰控制器。第二步启动 Gazebo 和 ros2_control 的硬件接口，加载真实控制器，但先不执行动作，只对比 RViz2 里的机器人模型和 Gazebo 里的模型初始姿态是否一致。第三步才让行为树完整跑一遍任务流。

同步验证的关键是数据对比，光看两边动没动不够。我习惯把一次完整运行录成 ros2 bag，同时记录规划层发布的可视化轨迹和控制器反馈的实际位置。

ros2 bag record -o gantry_verify \ /joint_states \ /display_planned_path \ /goal_pose

回放 bag 后在 RViz2 里把 /display_planned_path 和 /joint_states 画在同一坐标系下，前者是 MoveIt2 生成的理论轨迹，后者是实际执行轨迹。两条曲线在直线段如果偏差超过几个毫米，优先怀疑控制器速度环和加速度环没跟上规划的时间最优曲线，我一般先调低 max_velocity_scaling_factor 再重新录一次，而不是调整控制器 PID。

一个我坚持了很久的习惯是：每修改一个行为树节点，先把它单独拿出来 tick 几十次确认稳定，确认单独执行没问题，才放回整棵树里跑。MoveToXyz 和 GripperClose 两个节点单独跑都正常，不代表它们在 Sequence 里也正常，因为树会让动作并发。第一次整树运行我永远用半速跑，手放在急停旁边，这个习惯帮我少翻了不知道多少次车。希望帮到你。

本文还有配套的精品资源，点击获取