机器人 / 系统课时

向 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 删除。

Planning Scene 中工作台碰撞体、坐标系与规划路径的关系

启动 Panda 并创建工程

适用环境为 Ubuntu 24.04、ROS 2 Jazzy 和已安装的 MoveIt 2 Panda 演示配置。在第一个终端启动 move_group、机器人模型与 RViz:

bash
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py

预期结果:RViz 显示 Panda 机械臂和 MotionPlanning 相关面板。该终端应保持运行;如果提示找不到包,需要先安装或构建 Panda 演示资源。

在第二个终端检查场景读写服务:

bash
source /opt/ros/jazzy/setup.bash
ros2 service list | grep -E '/(apply_planning_scene|get_planning_scene)$'

默认无命名空间的演示中预期输出:

text
/apply_planning_scene
/get_planning_scene

如果使用了自定义命名空间,服务名称可能带有前缀。随后建立独立的 C++ 演示包:

bash
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 是否刷新来判断是否写入成功。

cpp
#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() 保留不动。

cmake
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 演示运行,在第二个终端构建并加入工作台:

bash
cd ~/moveit_ws
colcon build --packages-select scene_table_demo
source install/setup.bash
ros2 run scene_table_demo table_scene add

预期输出的程序结果行(可能另有 ROS 日志):

text
worktable: 0.55, 0, 0.2

这说明 move_group 的世界对象中已有 worktable。先在 RViz 观察桌面盒体,再单独运行移动命令:

bash
ros2 run scene_table_demo table_scene move

预期输出:

text
worktable: 0.7, 0.18, 0.2

这次仍是同一个对象 ID,盒体中心发生了位移。观察完成后移除:

bash
ros2 run scene_table_demo table_scene remove

预期输出:

text
worktable: absent

MOVE 必须发生在 ADD 之后;每条命令都是新进程,但场景保存在仍在运行的 move_group 中。重新启动演示会重建场景,不应假设这些临时对象已持久保存。

对照规划结果与定位异常

在 RViz 的 MotionPlanning 面板选择 Panda 机械臂规划组,使用交互标记设定一个无障碍时能成功规划、但可能穿过桌面占据空间的目标。保持同一起始关节状态和目标姿态,按以下顺序逐次点击 Plan,不要点击 Execute:

  1. 删除工作台后规划一次,记录规划成功与否及预览轨迹。
  2. 重新运行 add,等待盒体出现在场景中,再用原目标点击 Plan。
  3. 运行 move,再次规划;对比路径绕行、失败或保持不变的情况。

如果目标本身位于盒体内,加入工作台后通常无法得到无碰撞方案;如果原始轨迹没有经过障碍区域,规划不变也是合理结果。随机采样式规划器每次轨迹可能不同,判断时优先检查是否碰撞、是否可达,而不要求路径逐点一致。

看不到盒体时,先在 RViz 左侧 Displays 中展开 PlanningScene → Scene Geometry 并检查 Show Scene Geometry,再确认场景订阅话题与 move_group 的 monitored planning scene 一致。若命令一直等待,检查 /apply_planning_scene、/get_planning_scene 是否存在;若服务正常但坐标不对,核对 panda_link0 是否为当前机器人有效的参考帧。getObjectPoses 能读到对象而轨迹仍穿过桌面时,确认规划使用的是更新后的场景、正确的规划组和同一个碰撞对象 ID。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

向 Planning Scene 加入工作台障碍物

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通