机器人 / 系统课时
用 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 |
删除世界碰撞对象 |

这里使用 panda_link0 作为坐标参考帧,长度单位为米;运行其他机器人时应改为其规划场景能识别的帧。注意对象整体位姿 pose 与形状局部位姿 primitive_poses 是两级变换,不要把同一平移同时填入两处。
准备 Panda 演示与功能包
先在一个终端启动已安装的 Panda MoveIt 演示;它需提供 move_group、规划场景服务与 RViz。如果使用自己生成的配置包,请换成对应的 demo.launch.py。
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py预期结果:RViz 显示 Panda 机械臂,move_group 正常启动。终端启动日志和时间因环境不同而不同;没有安装该演示包时,先安装相应的 MoveIt Panda 资源或使用已有机器人配置。
另开终端,在示例工作空间创建 ament_cmake 包;已有同名包时跳过创建。
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。
#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_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,再在第二个终端执行:
cd ~/ros2_ws
colcon build --packages-select scene_update_demo
source install/setup.bash
ros2 run scene_update_demo scene_update预期结果(仅摘录本例程序标准输出;按提示各按一次 Enter,编译耗时与 ROS 日志不固定):
ADD confirmed: x=0.5
Press Enter to MOVE
MOVE confirmed: y=0.3
Press Enter to REMOVE
REMOVE confirmed: absent怎样确认新规划使用最新障碍物
- 程序打印
Press Enter to MOVE时,先在 RViz 的 MotionPlanning 面板保持目标不变,点击Plan,观察新增长方体和预览轨迹。 - 按第一次 Enter 后,确认箱体沿 y 轴移动了
0.30 m,再点击Plan发起新的规划请求。应检查新轨迹不穿过新位置的碰撞体;规划可能成功、绕行,也可能因目标不可达而失败。 - 按第二次 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 并不会自动停止或重验已下发轨迹。