机器人 / 系统课时

用 PlanningSceneInterface 更新障碍物

用 PlanningSceneInterface 更新障碍物

本篇用 Panda 场景中的一个长方体演示 C++ 添加、移动与删除障碍物,并通过服务回读对象位姿。完成后,你将能区分“发出更新请求”“规划场景确认更新”和“新规划使用该环境”,避免障碍物已移动、轨迹却仍按旧场景执行。

学习目标

  • 构造包含坐标系、几何尺寸、位姿和操作类型的 CollisionObject。
  • 使用同步接口执行 ADD、MOVE、REMOVE 并回读结果。
  • 在 RViz 中重新规划,验证路径针对更新后的障碍物生成。

场景对象怎样进入规划器

适用环境为 ROS 2 Jazzy、MoveIt 2 和 Panda 演示配置。PlanningSceneInterface 向 move_group 提交规划场景差量;其 applyCollisionObject() 经 /apply_planning_scene 服务同步提交,返回值表示服务是否接受并应用差量。getObjectPoses()、getObjects() 从规划场景查询实际状态,不等同于检查客户端变量。

操作 CollisionObject 关键内容 场景结果
ADD id、header.frame_id、pose、几何体及局部位姿 新增对象;同 ID 会替换旧对象
MOVE 已存在的 id、参考帧和新 pose 保留几何,仅更改位姿
REMOVE 待删除的 id 删除世界碰撞对象

C++ 场景变更经 MoveIt 服务进入规划场景并影响后续规划的数据流

这里使用 panda_link0 作为坐标参考帧,长度单位为米;运行其他机器人时应改为其规划场景能识别的帧。注意对象整体位姿 pose 与形状局部位姿 primitive_poses 是两级变换,不要把同一平移同时填入两处。

准备 Panda 演示与功能包

先在一个终端启动已安装的 Panda MoveIt 演示;它需提供 move_group、规划场景服务与 RViz。如果使用自己生成的配置包,请换成对应的 demo.launch.py。

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

预期结果:RViz 显示 Panda 机械臂,move_group 正常启动。终端启动日志和时间因环境不同而不同;没有安装该演示包时,先安装相应的 MoveIt Panda 资源或使用已有机器人配置。

另开终端,在示例工作空间创建 ament_cmake 包;已有同名包时跳过创建。

bash
source /opt/ros/jazzy/setup.bash
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create scene_update_demo --build-type ament_cmake --license Apache-2.0 --dependencies rclcpp moveit_ros_planning_interface moveit_msgs shape_msgs

预期结果:生成 scene_update_demo/package.xml 和 scene_update_demo/CMakeLists.txt,其依赖已写入包描述。工作空间不在 ~/ros2_ws 时需相应替换路径。

用 C++ 增加、移动和删除箱体

在 scene_update_demo/src/scene_update.cpp 写入以下完整程序。长方体尺寸为 0.16 × 0.20 × 0.30 m;对象原点起初在 (0.50, 0.00, 0.25),随后沿 y 轴移动到 0.30 m。每次提交后都回读,程序在两次按 Enter 时暂停,方便观察 RViz。

cpp
#include <cmath>
#include <iostream>
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <rclcpp/rclcpp.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  moveit::planning_interface::PlanningSceneInterface scene;

  moveit_msgs::msg::CollisionObject box;
  box.header.frame_id = "panda_link0";
  box.id = "lesson_box";
  box.pose.orientation.w = 1.0;
  box.pose.position.x = 0.50;
  box.pose.position.y = 0.00;
  box.pose.position.z = 0.25;

  shape_msgs::msg::SolidPrimitive shape;
  shape.type = shape.BOX;
  shape.dimensions = {0.16, 0.20, 0.30};
  box.primitives.push_back(shape);

  // 形状相对对象原点保持单位位姿,防止坐标平移叠加。
  geometry_msgs::msg::Pose local_pose;
  local_pose.orientation.w = 1.0;
  box.primitive_poses.push_back(local_pose);
  box.operation = box.ADD;
  if (!scene.applyCollisionObject(box)) {
    std::cerr << "ADD failed\n";
    rclcpp::shutdown();
    return 1;
  }
  const auto first = scene.getObjectPoses({box.id});
  if (first.count(box.id) != 1) {
    std::cerr << "ADD read-back failed\n";
    rclcpp::shutdown();
    return 1;
  }
  std::cout << "ADD confirmed: x=" << first.at(box.id).position.x << "\n";
  std::cout << "Press Enter to MOVE\n";
  std::cin.get();

  // MOVE 沿用现有几何体,消息中的形状数组必须留空。
  moveit_msgs::msg::CollisionObject moved;
  moved.header.frame_id = box.header.frame_id;
  moved.id = box.id;
  moved.pose = box.pose;
  moved.pose.position.y = 0.30;
  moved.operation = moved.MOVE;
  if (!scene.applyCollisionObject(moved)) {
    std::cerr << "MOVE failed\n";
    rclcpp::shutdown();
    return 1;
  }
  const auto second = scene.getObjectPoses({box.id});
  if (second.count(box.id) != 1 ||
      std::abs(second.at(box.id).position.y - 0.30) > 1e-5) {
    std::cerr << "MOVE read-back failed\n";
    rclcpp::shutdown();
    return 1;
  }
  std::cout << "MOVE confirmed: y=" << second.at(box.id).position.y << "\n";
  std::cout << "Press Enter to REMOVE\n";
  std::cin.get();

  // 用同步差量删除,避免发出异步删除后立即开始下一次规划。
  moveit_msgs::msg::CollisionObject removed;
  removed.id = box.id;
  removed.operation = removed.REMOVE;
  if (!scene.applyCollisionObject(removed) ||
      !scene.getObjects({removed.id}).empty()) {
    std::cerr << "REMOVE verification failed\n";
    rclcpp::shutdown();
    return 1;
  }
  std::cout << "REMOVE confirmed: absent\n";
  rclcpp::shutdown();
  return 0;
}

预期结果:编译运行后应依次读回 x=0.5、y=0.3,最后查询不到该 ID;打印内容见下一节。若任一 applyCollisionObject() 失败,程序返回非零值,禁止把它当成已完成的场景更新。

编译、运行与结果核对

将包内 CMakeLists.txt 替换成下方完整配置,使可执行程序链接 MoveIt 的规划接口。package.xml 已由创建命令加入依赖;不要误把这个节点当作独立规划器。

cmake
cmake_minimum_required(VERSION 3.8)
project(scene_update_demo)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(shape_msgs REQUIRED)

add_executable(scene_update src/scene_update.cpp)
ament_target_dependencies(scene_update
  rclcpp moveit_ros_planning_interface moveit_msgs shape_msgs)
install(TARGETS scene_update DESTINATION lib/${PROJECT_NAME})
ament_package()

生效行为:构建后通过 ros2 run scene_update_demo scene_update 启动。确保先启动 Panda 的 move_group,再在第二个终端执行:

bash
cd ~/ros2_ws
colcon build --packages-select scene_update_demo
source install/setup.bash
ros2 run scene_update_demo scene_update

预期结果(仅摘录本例程序标准输出;按提示各按一次 Enter,编译耗时与 ROS 日志不固定):

text
ADD confirmed: x=0.5
Press Enter to MOVE
MOVE confirmed: y=0.3
Press Enter to REMOVE
REMOVE confirmed: absent

怎样确认新规划使用最新障碍物

  1. 程序打印 Press Enter to MOVE 时,先在 RViz 的 MotionPlanning 面板保持目标不变,点击 Plan,观察新增长方体和预览轨迹。
  2. 按第一次 Enter 后,确认箱体沿 y 轴移动了 0.30 m,再点击 Plan 发起新的规划请求。应检查新轨迹不穿过新位置的碰撞体;规划可能成功、绕行,也可能因目标不可达而失败。
  3. 按第二次 Enter 移除对象,确认 RViz 中该对象消失,然后重新 Plan;旧轨迹不会因为场景更新自动重算。

如果对象未出现,先检查 header.frame_id 是否为当前机器人已知坐标帧、pose.orientation.w 是否设置为 1.0,再确认服务 /apply_planning_scene 和 RViz 监听的是同一套 move_group 场景。如果回读位姿与预期不符,检查是否把平移重复写到 pose 与 primitive_poses,以及是否有其他节点使用相同 ID 覆盖对象。需要立刻基于新环境规划时,优先使用同步 applyCollisionObject();addCollisionObjects() 与 removeCollisionObjects() 通过话题异步发布,不能把“调用已返回”当成“场景已更新”。

实际项目应遵循更新、回读、重新规划、检查碰撞结果再执行的顺序;对正在执行中的机械臂,单独更新 Planning Scene 并不会自动停止或重验已下发轨迹。

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

目录与阅读设置

阅读设置

17

记录一个想法

用 PlanningSceneInterface 更新障碍物

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

登录哪都通