机器人 / 系统课时
向 Planning Scene 加入工作台障碍物
向 Planning Scene 加入工作台障碍物
本篇把工作台建成盒形碰撞对象,写入 Panda 机械臂的 Planning Scene,再移动和移除它。你将能在 RViz 中看见障碍物的变化,并用相同起点、相同目标比较加入障碍前后的规划结果。
学习目标
- 说明世界碰撞对象的 ID、几何、坐标系及位姿各自的作用。
- 用 MoveIt 2 C++ 接口添加、移动和删除工作台。
- 在 RViz 与场景查询结果中验证更新,并判断障碍为何影响或未影响路径。
工作台的碰撞几何
Planning Scene 同时维护机器人状态和世界中的障碍物。工作台属于世界碰撞对象,不是随夹爪运动的 AttachedCollisionObject。这里只用一个长方体近似桌面,暂不建桌腿,所有长度单位为米。
| 属性 | 教学取值 | 含义 |
|---|---|---|
id |
worktable |
后续移动、删除都使用同一 ID |
header.frame_id |
panda_link0 |
Panda 固定基座演示中的参考坐标系 |
| 盒体尺寸 X/Y/Z | 0.70 / 0.60 / 0.10 | 长、宽、厚 |
| 初始中心 X/Y/Z | 0.55 / 0 / 0.20 | 桌面上缘高度为 0.25 |
| 移动后中心 X/Y/Z | 0.70 / 0.18 / 0.20 | 几何形状不变,仅改变位姿 |
CollisionObject.pose 表示对象坐标系相对于 header.frame_id 的位姿;primitive_poses 表示盒体相对于对象坐标系的位姿,本例设为单位位姿。四元数 w=1 表示不旋转。ADD 新建对象;对同一 ID 再执行 ADD 会替换原有几何;MOVE 只更新已有对象的位置和姿态;REMOVE 按 ID 删除。

启动 Panda 并创建工程
适用环境为 Ubuntu 24.04、ROS 2 Jazzy 和已安装的 MoveIt 2 Panda 演示配置。在第一个终端启动 move_group、机器人模型与 RViz:
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py预期结果:RViz 显示 Panda 机械臂和 MotionPlanning 相关面板。该终端应保持运行;如果提示找不到包,需要先安装或构建 Panda 演示资源。
在第二个终端检查场景读写服务:
source /opt/ros/jazzy/setup.bash
ros2 service list | grep -E '/(apply_planning_scene|get_planning_scene)$'默认无命名空间的演示中预期输出:
/apply_planning_scene
/get_planning_scene如果使用了自定义命名空间,服务名称可能带有前缀。随后建立独立的 C++ 演示包:
mkdir -p ~/moveit_ws/src
cd ~/moveit_ws/src
ros2 pkg create scene_table_demo --build-type ament_cmake --dependencies rclcpp moveit_ros_planning_interface moveit_msgs shape_msgs geometry_msgs
mkdir -p scene_table_demo/src预期结果:生成 scene_table_demo/package.xml 和 scene_table_demo/CMakeLists.txt,并具有可写入 C++ 源码的 src 目录。若工程已存在,不要重复执行创建命令。
编写场景增删改程序
将以下完整代码保存为 ~/moveit_ws/src/scene_table_demo/src/table_scene.cpp。applyCollisionObject 同步提交场景差异到 move_group;提交后再用 getObjectPoses 读取对象,避免只根据 RViz 是否刷新来判断是否写入成功。
#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <iostream>
#include <string>
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
if (argc != 2) {
std::cerr << "Usage: table_scene add|move|remove\n";
rclcpp::shutdown();
return 1;
}
moveit::planning_interface::PlanningSceneInterface scene;
moveit_msgs::msg::CollisionObject table;
table.id = "worktable";
table.header.frame_id = "panda_link0";
table.pose.orientation.w = 1.0;
const std::string action = argv[1];
bool ok = false;
if (action == "add") {
// 盒体局部位姿保持单位变换,位置交给对象 pose 管理。
shape_msgs::msg::SolidPrimitive box;
box.type = shape_msgs::msg::SolidPrimitive::BOX;
box.dimensions = {0.70, 0.60, 0.10};
geometry_msgs::msg::Pose local;
local.orientation.w = 1.0;
table.primitives.push_back(box);
table.primitive_poses.push_back(local);
table.pose.position.x = 0.55;
table.pose.position.z = 0.20;
table.operation = moveit_msgs::msg::CollisionObject::ADD;
ok = scene.applyCollisionObject(table);
} else if (action == "move") {
// MOVE 只发送新的对象位姿,不重复发送几何体。
table.pose.position.x = 0.70;
table.pose.position.y = 0.18;
table.pose.position.z = 0.20;
table.operation = moveit_msgs::msg::CollisionObject::MOVE;
ok = scene.applyCollisionObject(table);
} else if (action == "remove") {
table.operation = moveit_msgs::msg::CollisionObject::REMOVE;
ok = scene.applyCollisionObject(table);
} else {
std::cerr << "Unknown action\n";
rclcpp::shutdown();
return 1;
}
if (!ok) {
std::cerr << "Scene update failed\n";
rclcpp::shutdown();
return 2;
}
const auto poses = scene.getObjectPoses({"worktable"});
if (poses.count("worktable")) {
const auto& p = poses.at("worktable").position;
std::cout << "worktable: " << p.x << ", " << p.y
<< ", " << p.z << '\n';
} else {
std::cout << "worktable: absent\n";
}
rclcpp::shutdown();
return 0;
}预期结果:按 add、move、remove 参数运行时,程序分别打印已保存对象的中心坐标或 absent;下面给出逐次运行结果。
片段示例:在生成的 CMakeLists.txt 中,将下列内容添加到末尾 ament_package() 之前;原有 find_package(...) 和 ament_package() 保留不动。
add_executable(table_scene src/table_scene.cpp)
ament_target_dependencies(table_scene
rclcpp moveit_ros_planning_interface moveit_msgs shape_msgs geometry_msgs
)
install(TARGETS table_scene DESTINATION lib/${PROJECT_NAME})这段 CMake 配置会构建 table_scene 可执行文件并使 ros2 run 能找到它;是否生效以接下来的编译与运行结果为准。
编译并检查每次更新
保持 Panda 演示运行,在第二个终端构建并加入工作台:
cd ~/moveit_ws
colcon build --packages-select scene_table_demo
source install/setup.bash
ros2 run scene_table_demo table_scene add预期输出的程序结果行(可能另有 ROS 日志):
worktable: 0.55, 0, 0.2这说明 move_group 的世界对象中已有 worktable。先在 RViz 观察桌面盒体,再单独运行移动命令:
ros2 run scene_table_demo table_scene move预期输出:
worktable: 0.7, 0.18, 0.2这次仍是同一个对象 ID,盒体中心发生了位移。观察完成后移除:
ros2 run scene_table_demo table_scene remove预期输出:
worktable: absentMOVE 必须发生在 ADD 之后;每条命令都是新进程,但场景保存在仍在运行的 move_group 中。重新启动演示会重建场景,不应假设这些临时对象已持久保存。
对照规划结果与定位异常
在 RViz 的 MotionPlanning 面板选择 Panda 机械臂规划组,使用交互标记设定一个无障碍时能成功规划、但可能穿过桌面占据空间的目标。保持同一起始关节状态和目标姿态,按以下顺序逐次点击 Plan,不要点击 Execute:
- 删除工作台后规划一次,记录规划成功与否及预览轨迹。
- 重新运行
add,等待盒体出现在场景中,再用原目标点击Plan。 - 运行
move,再次规划;对比路径绕行、失败或保持不变的情况。
如果目标本身位于盒体内,加入工作台后通常无法得到无碰撞方案;如果原始轨迹没有经过障碍区域,规划不变也是合理结果。随机采样式规划器每次轨迹可能不同,判断时优先检查是否碰撞、是否可达,而不要求路径逐点一致。
看不到盒体时,先在 RViz 左侧 Displays 中展开 PlanningScene → Scene Geometry 并检查 Show Scene Geometry,再确认场景订阅话题与 move_group 的 monitored planning scene 一致。若命令一直等待,检查 /apply_planning_scene、/get_planning_scene 是否存在;若服务正常但坐标不对,核对 panda_link0 是否为当前机器人有效的参考帧。getObjectPoses 能读到对象而轨迹仍穿过桌面时,确认规划使用的是更新后的场景、正确的规划组和同一个碰撞对象 ID。