在本教程中，我们将研究使用计划场景差异来执行两个操作：
将对象添加和移除到世界中
将物体附加到机器人和从机器人分离
入门
如果您还没有这样做，请确保您已完成入门中的步骤。
运行代码
打开两个 shell。在第一个 shell 中启动 RViz 并等待所有内容加载完成：
ros2 launch moveit2_tutorials move_group.launch.py在第二个 shell 中，运行此演示的启动文件：
ros2 launch moveit2_tutorials planning_scene_ros_api_tutorial.launch.py片刻之后，RViz 窗口应该会出现，并且看起来与可视化教程中的此步骤类似。要通过每个演示步骤，请按屏幕底部 RvizVisualToolsGui 面板中的 Next 按钮，或在屏幕顶部的 Tools 面板中选择 Key Tool，然后在 RViz 聚焦时按键盘上的 0。
预期输出
在 RViz 中，您应该能够看到以下内容：
对象出现在规划场景中。
对象附加到机器人。
物体从机器人上脱离。
对象已从规划场景中移除。
整个代码
整个代码可以在 MoveIt GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/tree/main/doc/examples/planning_scene_ros_api/src
#include <rclcpp/rclcpp.hpp> // 包含ROS 2的C++客户端库
#include <geometry_msgs/msg/pose.hpp> // 包含几何消息中的位姿消息
// MoveIt
#include <moveit_msgs/msg/planning_scene.hpp> // 包含MoveIt的规划场景消息
#include <moveit_msgs/msg/attached_collision_object.hpp> // 包含MoveIt的附加碰撞对象消息
#include <moveit_msgs/srv/get_state_validity.hpp> // 包含MoveIt的获取状态有效性服务
#include <moveit_msgs/msg/display_robot_state.hpp> // 包含MoveIt的显示机器人状态消息
#include <moveit_msgs/srv/apply_planning_scene.hpp> // 包含MoveIt的应用规划场景服务
#include <moveit/robot_model_loader/robot_model_loader.h> // 包含MoveIt的机器人模型加载器
#include <moveit/robot_state/robot_state.h> // 包含MoveIt的机器人状态
#include <moveit/robot_state/conversions.h> // 包含MoveIt的机器人状态转换
#include <rviz_visual_tools/rviz_visual_tools.hpp> // 包含Rviz可视化工具
static const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_ros_api_tutorial"); // 定义日志记录器
int main(int argc, char** argv)
{
rclcpp::init(argc, argv); // 初始化ROS 2
rclcpp::NodeOptions node_options; // 创建节点选项
node_options.automatically_declare_parameters_from_overrides(true); // 自动声明参数
auto node = rclcpp::Node::make_shared("planning_scene_ros_api_tutorial", node_options); // 创建节点
rclcpp::executors::SingleThreadedExecutor executor; // 创建单线程执行器
executor.add_node(node); // 将节点添加到执行器
std::thread(&executor { executor.spin(); }).detach(); // 启动一个线程来运行执行器
// BEGIN_TUTORIAL
//
// Visualization
// ^^^^^^^^^^^^^
// The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
// and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "planning_scene_ros_api_tutorial", node); // 创建Rviz可视化工具
visual_tools.loadRemoteControl(); // 加载远程控制
visual_tools.deleteAllMarkers(); // 删除所有标记
// ROS API
// ^^^^^^^
// The ROS API to the planning scene publisher is through a topic interface
// using "diffs". A planning scene diff is the difference between the current
// planning scene (maintained by the move_group node) and the new planning
// scene desired by the user.
//
// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// We create a publisher and wait for subscribers.
// Note that this topic may need to be remapped in the launch file.
rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher =
node->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 1); // 创建规划场景发布者
while (planning_scene_diff_publisher->get_subscription_count() < 1) // 等待订阅者
{
rclcpp::sleep_for(std::chrono::milliseconds(500)); // 每500毫秒检查一次
}
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户继续演示
// Define the attached object message
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// We will use this message to add or
// subtract the object from the world
// and to attach the object to the robot.
moveit_msgs::msg::AttachedCollisionObject attached_object; // 定义附加碰撞对象消息
attached_object.link_name = "panda_hand"; // 设置链接名称
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "panda_hand"; // 设置帧ID
/* The id of the object */
attached_object.object.id = "box"; // 设置对象ID
/* A default pose */
geometry_msgs::msg::Pose pose; // 定义默认姿态
pose.position.z = 0.11; // 设置位置
pose.orientation.w = 1.0; // 设置方向
/* Define a box to be attached */
shape_msgs::msg::SolidPrimitive primitive; // 定义一个要附加的盒子
primitive.type = primitive.BOX; // 设置类型为盒子
primitive.dimensions.resize(3); // 设置尺寸
primitive.dimensions[0] = 0.075;
primitive.dimensions[1] = 0.075;
primitive.dimensions[2] = 0.075;
attached_object.object.primitives.push_back(primitive); // 将盒子添加到对象中
attached_object.object.primitive_poses.push_back(pose); // 将姿态添加到对象中
// Note that attaching an object to the robot requires
// the corresponding operation to be specified as an ADD operation.
attached_object.object.operation = attached_object.object.ADD; // 设置操作为添加
// Since we are attaching the object to the robot hand to simulate picking up the object,
// we want the collision checker to ignore collisions between the object and the robot hand.
attached_object.touch_links = std::vector<std::string>{ "panda_hand", "panda_leftfinger", "panda_rightfinger" }; // 设置触碰链接
// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to
// the set of collision objects in the "world" part of the
// planning scene. Note that we are using only the "object"
// field of the attached_object message here.
RCLCPP_INFO(LOGGER, "Adding the object into the world at the location of the hand."); // 打印信息
moveit_msgs::msg::PlanningScene planning_scene; // 创建规划场景消息
planning_scene.world.collision_objects.push_back(attached_object.object); // 将对象添加到世界中
planning_scene.is_diff = true; // 设置为差异
planning_scene_diff_publisher->publish(planning_scene); // 发布规划场景
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户继续演示
// Interlude: Synchronous vs Asynchronous updates
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// There are two separate mechanisms available to interact
// with the move_group node using diffs:
//
// * Send a diff via a rosservice call and block until
// the diff is applied (synchronous update)
// * Send a diff via a topic, continue even though the diff
// might not be applied yet (asynchronous update)
//
// While most of this tutorial uses the latter mechanism (given the long sleeps
// inserted for visualization purposes asynchronous updates do not pose a problem),
// it would be perfectly justified to replace the planning_scene_diff_publisher
// by the following service client:
rclcpp::Client<moveit_msgs::srv::ApplyPlanningScene>::SharedPtr planning_scene_diff_client =
node->create_client<moveit_msgs::srv::ApplyPlanningScene>("apply_planning_scene"); // 创建服务客户端
planning_scene_diff_client->wait_for_service(); // 等待服务
// and send the diffs to the planning scene via a service call:
auto request = std::make_shared<moveit_msgs::srv::ApplyPlanningScene::Request>(); // 创建请求
request->scene = planning_scene; // 设置请求场景
std::shared_future<std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response>> response_future;
response_future = planning_scene_diff_client->async_send_request(request).future.share(); // 异步发送请求
// wait for the service to respond
std::chrono::seconds wait_time(1); // 设置等待时间
std::future_status fs = response_future.wait_for(wait_time); // 等待响应
if (fs == std::future_status::timeout) // 如果超时
{
RCLCPP_ERROR(LOGGER, "Service timed out."); // 打印错误信息
}
else
{
std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response> planning_response;
planning_response = response_future.get(); // 获取响应
if (planning_response->success) // 如果成功
{
RCLCPP_INFO(LOGGER, "Service successfully added object."); // 打印成功信息
}
else
{
RCLCPP_ERROR(LOGGER, "Service failed to add object."); // 打印失败信息
}
}
// Note that this does not continue until we are sure the diff has been applied.
//
// Attach an object to the robot
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// When the robot picks up an object from the environment, we need to
// "attach" the object to the robot so that any component dealing with
// the robot model knows to account for the attached object, e.g. for
// collision checking.
//
// Attaching an object requires two operations
// * Removing the original object from the environment
// * Attaching the object to the robot
/* First, define the REMOVE object message*/
moveit_msgs::msg::CollisionObject remove_object; // 首先，定义移除对象消息
remove_object.id = "box"; // 设置对象ID
remove_object.header.frame_id = "panda_hand"; // 设置帧ID
remove_object.operation = remove_object.REMOVE; // 设置操作为移除
// Note how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
/* Carry out the REMOVE + ATTACH operation */
RCLCPP_INFO(LOGGER, "Attaching the object to the hand and removing it from the world."); // 打印信息
planning_scene.world.collision_objects.clear(); // 清除世界中的碰撞对象
planning_scene.world.collision_objects.push_back(remove_object); // 将移除对象添加到世界中
planning_scene.robot_state.attached_collision_objects.push_back(attached_object); // 将附加对象添加到机器人状态中
planning_scene.robot_state.is_diff = true; // 设置机器人状态为差异
planning_scene_diff_publisher->publish(planning_scene); // 发布规划场景
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户继续演示
// Detach an object from the robot
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Detaching an object from the robot requires two operations
// * Detaching the object from the robot
// * Re-introducing the object into the environment
/* First, define the DETACH object message*/
moveit_msgs::msg::AttachedCollisionObject detach_object; // 首先，定义分离对象消息
detach_object.object.id = "box"; // 设置对象ID
detach_object.link_name = "panda_hand"; // 设置链接名称
detach_object.object.operation = attached_object.object.REMOVE; // 设置操作为移除
// Note how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
/* Carry out the DETACH + ADD operation */
RCLCPP_INFO(LOGGER, "Detaching the object from the robot and returning it to the world."); // 打印信息
planning_scene.robot_state.attached_collision_objects.clear(); // 清除机器人状态中的附加碰撞对象
planning_scene.robot_state.attached_collision_objects.push_back(detach_object); // 将分离对象添加到机器人状态中
planning_scene.robot_state.is_diff = true; // 设置机器人状态为差异
planning_scene.world.collision_objects.clear(); // 清除世界中的碰撞对象
planning_scene.world.collision_objects.push_back(attached_object.object); // 将附加对象添加到世界中
planning_scene.is_diff = true; // 设置规划场景为差异
planning_scene_diff_publisher->publish(planning_scene); // 发布规划场景
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户继续演示
// Remove the object from the collision world
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Removing the object from the collision world just requires
// using the remove object message defined earlier.
// Note, also how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
RCLCPP_INFO(LOGGER, "Removing the object from the world."); // 打印信息
planning_scene.robot_state.attached_collision_objects.clear(); // 清除机器人状态中的附加碰撞对象
planning_scene.world.collision_objects.clear(); // 清除世界中的碰撞对象
planning_scene.world.collision_objects.push_back(remove_object); // 将移除对象添加到世界中
planning_scene_diff_publisher->publish(planning_scene); // 发布规划场景
// END_TUTORIAL
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to end the demo"); // 提示用户结束演示
rclcpp::shutdown(); // 关闭ROS 2
return 0; // 返回0
}可视化
该软件包 MoveItVisualTools 提供了许多在 RViz 中可视化对象、机器人和轨迹的功能，以及诸如脚本逐步内省等调试工具。
rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "planning_scene_ros_api_tutorial", node);
// RViz 可视化工具初始化
visual_tools.loadRemoteControl(); // 加载远程控制
visual_tools.deleteAllMarkers(); // 删除所有标记ROS API
ROS API 到规划场景发布器是通过使用“diffs”的主题接口。规划场景 diff 是当前规划场景（由 move_group 节点维护）和用户期望的新规划场景之间的差异。
广播所需的主题
我们创建一个发布者并等待订阅者。请注意，可能需要在启动文件中重新映射此主题。
rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher =
node->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 1);
// 创建规划场景发布者
while (planning_scene_diff_publisher->get_subscription_count() < 1)
{
rclcpp::sleep_for(std::chrono::milliseconds(500));
// 等待订阅者连接
}
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// 提示用户继续定义附加对象消息
我们将使用此消息从世界中添加或减去对象，并将对象附加到机器人上。
moveit_msgs::msg::AttachedCollisionObject attached_object;
// 定义附加的碰撞对象消息
attached_object.link_name = "panda_hand";
attached_object.object.header.frame_id = "panda_hand";
attached_object.object.id = "box";
// 设置附加对象的相关信息
geometry_msgs::msg::Pose pose; // 定义位姿
pose.position.z = 0.11; // 设置位姿的位置
pose.orientation.w = 1.0; // 设置位姿的方向
shape_msgs::msg::SolidPrimitive primitive;
// 定义一个立方体作为附加对象
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.075;
primitive.dimensions[1] = 0.075;
primitive.dimensions[2] = 0.075;
// 设置立方体的尺寸
attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);
// 将立方体添加到附加对象中请注意，将对象附加到机器人需要将相应的操作指定为 ADD 操作。
attached_object.object.operation = attached_object.object.ADD;
// 设置对象的操作为添加由于我们将物体附加到机器人手上以模拟拾取物体，我们希望碰撞检查器忽略物体与机器人手之间的碰撞。
attached_object.touch_links = std::vector<std::string>{ "panda_hand", "panda_leftfinger", "panda_rightfinger" };
// 设置触碰连杆，以避免碰撞检测将对象添加到环境中
将对象添加到环境中，通过将其添加到规划场景的“世界”部分的碰撞对象集合中。请注意，这里我们仅使用 attached_object 消息的“object”字段。
RCLCPP_INFO(LOGGER, "Adding the object into the world at the location of the hand.");
// 日志输出：将对象添加到世界中
moveit_msgs::msg::PlanningScene planning_scene;
// 定义规划场景消息
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher->publish(planning_scene);
// 发布规划场景消息
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// 提示用户继续
[planning_scene_ros_api_tutorial-1] [INFO] [1722603572.974460813] [planning_scene_ros_api_tutorial.remote_control]: ... continuing
[planning_scene_ros_api_tutorial-1] [INFO] [1722603572.974618773] [planning_scene_ros_api_tutorial]: Adding the object into the world at the location of the hand.
[planning_scene_ros_api_tutorial-1] [INFO] [1722603572.975347268] [planning_scene_ros_api_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo插曲：同步与异步更新
有两种独立的机制可以使用差异与 move_group 节点进行交互：
通过 rosservice 调用发送差异，并阻塞直到差异应用（同步更新）
通过主题发送差异，即使差异可能尚未应用（异步更新）
虽然本教程的大部分内容使用后者机制（考虑到为可视化目的插入的长时间休眠，异步更新不会构成问题），但完全可以用以下服务客户端替换 planning_scene_diff_publisher：
rclcpp::Client<moveit_msgs::srv::ApplyPlanningScene>::SharedPtr planning_scene_diff_client =
node->create_client<moveit_msgs::srv::ApplyPlanningScene>("apply_planning_scene");
// 创建应用规划场景的服务客户端
planning_scene_diff_client->wait_for_service();
// 等待服务启动并通过服务调用将差异发送到规划场景：
// 并通过服务调用将差异发送到规划场景：
auto request = std::make_shared<moveit_msgs::srv::ApplyPlanningScene::Request>(); // 创建请求
request->scene = planning_scene; // 设置场景
std::shared_future<std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response>> response_future;
response_future = planning_scene_diff_client->async_send_request(request).future.share(); // 发送请求等待服务响应
// 等待服务响应
std::chrono::seconds wait_time(1); // 设置等待时间
std::future_status fs = response_future.wait_for(wait_time); // 等待响应
if (fs == std::future_status::timeout) // 如果超时
{
RCLCPP_ERROR(LOGGER, "Service timed out."); // 打印错误信息
}
else
{
std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response> planning_response;
planning_response = response_future.get(); // 获取响应
if (planning_response->success) // 如果成功
{
RCLCPP_INFO(LOGGER, "Service successfully added object."); // 打印信息
}
else
{
RCLCPP_ERROR(LOGGER, "Service failed to add object."); // 打印错误信息
}
}请注意，这不会继续，直到我们确定差异已被应用。
将物体附加到机器人
当机器人从环境中拾取物体时，我们需要将物体“附加”到机器人上，以便处理机器人模型的任何组件都知道要考虑附加的物体，例如进行碰撞检查。
附加一个对象需要两个操作
从环境中移除原始对象
将物体附加到机器人上
/* 首先，定义REMOVE对象消息 */
moveit_msgs::msg::CollisionObject remove_object; // 定义移除碰撞对象
remove_object.id = "box"; // 设置对象ID
remove_object.header.frame_id = "panda_hand"; // 设置帧ID
remove_object.operation = remove_object.REMOVE; // 设置操作为移除请注意，我们首先清除这些字段，以确保差异消息中不包含其他附加对象或碰撞对象。
/* 执行REMOVE + ATTACH操作 */
RCLCPP_INFO(LOGGER, "Attaching the object to the hand and removing it from the world."); // 打印信息
planning_scene.world.collision_objects.clear(); // 清除碰撞对象
planning_scene.world.collision_objects.push_back(remove_object); // 添加移除对象
planning_scene.robot_state.attached_collision_objects.push_back(attached_object); // 添加附加对象
planning_scene.robot_state.is_diff = true; // 设置差异
planning_scene_diff_publisher->publish(planning_scene); // 发布规划场景
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户继续演示
[planning_scene_ros_api_tutorial-1] [INFO] [1722603661.313782382] [planning_scene_ros_api_tutorial.remote_control]: ... continuing
[planning_scene_ros_api_tutorial-1] [INFO] [1722603661.317622591] [planning_scene_ros_api_tutorial]: Service successfully added object.
[planning_scene_ros_api_tutorial-1] [INFO] [1722603661.317698201] [planning_scene_ros_api_tutorial]: Attaching the object to the hand and removing it from the world.
[planning_scene_ros_api_tutorial-1] [INFO] [1722603661.317923733] [planning_scene_ros_api_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo从机器人上分离一个物体
从机器人上分离物体需要两个操作
-
从机器人上分离物体
将对象重新引入环境中
/* 首先，定义DETACH对象消息 */
moveit_msgs::msg::AttachedCollisionObject detach_object; // 定义分离碰撞对象
detach_object.object.id = "box"; // 设置对象ID
detach_object.link_name = "panda_hand"; // 设置链接名称
detach_object.object.operation = attached_object.object.REMOVE; // 设置操作为移除请注意，我们首先清除这些字段，以确保差异消息中不包含其他附加对象或碰撞对象。
/* 执行DETACH + ADD操作 */
RCLCPP_INFO(LOGGER, "Detaching the object from the robot and returning it to the world."); // 打印信息
planning_scene.robot_state.attached_collision_objects.clear(); // 清除附加碰撞对象
planning_scene.robot_state.attached_collision_objects.push_back(detach_object); // 添加分离对象
planning_scene.robot_state.is_diff = true; // 设置差异
planning_scene.world.collision_objects.clear(); // 清除碰撞对象
planning_scene.world.collision_objects.push_back(attached_object.object); // 添加附加对象
planning_scene.is_diff = true; // 设置差异
planning_scene_diff_publisher->publish(planning_scene); // 发布规划场景
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户继续演示
[planning_scene_ros_api_tutorial-1] [INFO] [1722603761.997986474] [planning_scene_ros_api_tutorial.remote_control]: ... continuing
[planning_scene_ros_api_tutorial-1] [INFO] [1722603761.998033794] [planning_scene_ros_api_tutorial]: Detaching the object from the robot and returning it to the world.
[planning_scene_ros_api_tutorial-1] [INFO] [1722603761.998106080] [planning_scene_ros_api_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo从碰撞世界中移除对象
从碰撞世界中移除对象只需要使用前面定义的移除对象消息。请注意，我们还通过首先清除这些字段来确保差异消息不包含其他附加对象或碰撞对象。
RCLCPP_INFO(LOGGER, "Removing the object from the world."); // 打印信息
planning_scene.robot_state.attached_collision_objects.clear(); // 清除附加碰撞对象
planning_scene.world.collision_objects.clear(); // 清除碰撞对象
planning_scene.world.collision_objects.push_back(remove_object); // 添加移除对象
planning_scene_diff_publisher->publish(planning_scene); // 发布规划场景
[planning_scene_ros_api_tutorial-1] [INFO] [1722603798.208661506] [planning_scene_ros_api_tutorial.remote_control]: ... continuing
[planning_scene_ros_api_tutorial-1] [INFO] [1722603798.208724469] [planning_scene_ros_api_tutorial]: Removing the object from the world.
[planning_scene_ros_api_tutorial-1] [INFO] [1722603798.208804998] [planning_scene_ros_api_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to end the demo
151
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
