Movelt2 规划场景 ROS API

前言

本文整理的是 MoveIt2 官方示例
主要学习

添加和移除世界中的对象
将物体安装和拆卸到机器人上
https://github.com/ros-planning/moveit2_tutorials

一、使用rviz可视化

rviz_visual_tools::RvizVisualToolsvisual_tools("panda_link0","planning_scene_ros_api_tutorial",node);visual_tools.loadRemoteControl();visual_tools.deleteAllMarkers();

这三行代码主要是用 RVizVisualTools 在 RViz 中显示可视化标记，并启用交互控制。

代码解释

rviz_visual_tools::RvizVisualToolsvisual_tools("panda_link0","planning_scene_ros_api_tutorial",node);

这里是创建RvizVisualTools对象，往rviz里面发布可视化内容，可视化内容以"panda_link0"为参考坐标系，"planning_scene_ros_api_tutorial"是话题名(可以自己取)，node是当前的ROS2节点。

visual_tools.loadRemoteControl();

这里是加载rviz的远程控制功能

visual_tools.deleteAllMarkers();

删除 RViz 中之前发布过的所有 Marker。

二、ROS API

1.发布所需话题

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");

代码解释

rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_diff_publisher=node->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene",1);

这里是创建规划场景的发布者，moveit_msgs::msg::PlanningScene是发布消息的类型。
话题名称是"planning_scene"，队列大小是1（队列的意思是只缓存最新的一条消息）

一般是MoveIt 里的PlanningSceneMonitor来对这个发布消息进行订阅。

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");

这里是等待相关节点开始订阅消息。

visual_tools.prompt(“Press ‘next’ in the RvizVisualToolsGui window to continue the demo”)：暂停程序，等用户在 RVizVisualToolsGui 中点击 Next

2.定义附着碰撞物体的消息

以下代码是在世界中添加，将物体连接到机器人上，然后也可去掉物体。

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);

代码解释

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";

这里是创建一个可以连接到机器人的碰撞物体，附着在"panda_hand"，以"panda_hand"为参考位姿，碰撞物体id为box。其中，frame_id必须是TF树中存在的坐标系。

AttachedCollisionObject有以下内容：
std::string link_name;
moveit_msgs::msg::CollisionObject object;
std::vectorstd::string touch_links;
trajectory_msgs::msg::JointTrajectory detach_posture;
double weight;

补一个 “moveit_msgs::msg::CollisionObject object” 的内容

std_msgs::msg::Header header;
geometry_msgs::msg::Pose pose;
std::string id;
object_recognition_msgs::msg::ObjectType type;
std::vector<shape_msgs::msg::SolidPrimitive> primitives;
std::vector<geometry_msgs::msg::Pose> primitive_poses;
std::vector<shape_msgs::msg::Mesh> meshes;
std::vector<geometry_msgs::msg::Pose> mesh_poses;
std::vector<shape_msgs::msg::Plane> planes;
std::vector<geometry_msgs::msg::Pose> plane_poses;
std::vectorstd::string subframe_names;
std::vector<geometry_msgs::msg::Pose> subframe_poses;
uint8_t operation;

geometry_msgs::msg::Pose pose;pose.position.z=0.11;pose.orientation.w=1.0;

定义碰撞物体的位姿，这个位姿是相对于"panda_hand"来说。

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;

创建一个简单的盒子box。

attached_object.object.primitives.push_back(primitive);attached_object.object.primitive_poses.push_back(pose);

这里是把刚刚创建的box加入到object，然后把box的位姿也加进object。

刚刚上面是定义了碰撞物体，需要使用ADD将碰撞物体附加到机器人上，如下：

attached_object.object.operation=attached_object.object.ADD;

这里使用的“operation”是表示要对这个box进行添加操作。

常见的操作有：
ADD // 添加或更新物体
REMOVE // 删除物体
APPEND // 追加几何体
MOVE // 移动物体

在把碰撞物体附加在机器人手上后，需要碰撞检测器忽略物体与机器人之间的碰撞

attached_object.touch_links=std::vector<std::string>{"panda_hand","panda_leftfinger","panda_rightfinger"};

tocuh_links是指定运行碰撞的关节。

3.将一个对象添加到环境中

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");

moveit_msgs::msg::PlanningScene planning_scene;

创建规划环境消息。

planning_scene.world.collision_objects.push_back(attached_object.object);

把刚刚创建的box加入到PlanningScene的世界环境中。

planning_scene.is_diff=true;

这里是说在原来的PlanningScene基础上加入这个box，为的是不把原来的场景覆盖掉。

planning_scene_diff_publisher->publish(planning_scene);

这里是把当前这个PlanningScene的更新发布，让moveit更新场景，发布到/planning_scene，下面是全面创建的发布者。

node->create_publisher<moveit_msgs::msg::PlanningScene>(“planning_scene”, 1);

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

暂停，等 RViz 点击 Next

4.同步更新和异步更新的区别

有两种独立的机制，可以通过差异更新（diffs）与 move_group 节点进行交互。

1.同步更新：通过 ROS 服务调用发送一个差异更新，并阻塞等待，直到这个差异更新被应用完成。

2.异步更新：通过话题发送一个差异更新，即使这个差异更新可能还没有被应用，也继续往下执行。

全面的教程是使用异步更新，接下来介绍同步更新（更稳定）

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();

创建客户端，话题名为：/apply_planning_scene，后续可以用他发生请求。

moveit_msgs::srv::ApplyPlanningScene ：是专门用于先movelt申请更新PlanningScene

planning_scene_diff_client->wait_for_service();

等待服务启动，这个服务通常由 MoveIt 的 move_group 节点提供。

前面是使用topic发布/planning_scene，属于异步更新，发出去就继续，不保证 MoveIt 已经处理完。
这里是使用service，属于同步更新，会等待 MoveIt 返回结果，确认场景是否应用成功。

autorequest=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();

代码解释

autorequest=std::make_shared<moveit_msgs::srv::ApplyPlanningScene::Request>();request->scene=planning_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();

定义一个未来返回结果的对象，用于保存未来返回的结果；然后异步发送请求(程序不会在这一行卡住等结果)，也就是向向 /apply_planning_scene 服务发送请求，然后把结果保存到response_future

std::chrono::secondswait_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.");}}

代码解释

std::chrono::secondswait_time(1);std::future_status fs=response_future.wait_for(wait_time);

设置等待时间；fs是返回的结果，类型是std::future_status

std::future_status::ready
std::future_status::timeout
std::future_status::deferred

std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response>planning_response;planning_response=response_future.get();

这里创建一个planning_response来获取response_future中服务端的响应。

5.将一个物体连接到机器人上

/* First, define the REMOVE object message*/moveit_msgs::msg::CollisionObject remove_object;remove_object.id="box";remove_object.header.frame_id="panda_hand";remove_object.operation=remove_object.REMOVE;

定义一个删除碰撞物体的消息，从规划场景中删除“box”，这个box和前文创建的是同一个box，movelt主要是根据 id 找对应的 collision object。设置参考坐标系，类型是REMOVE

attached_object.object.id = “box”;

/* 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");

代码解释

planning_scene.world.collision_objects.clear();planning_scene.world.collision_objects.push_back(remove_object);

这里是把当前这个规划环境世界的碰撞物体清空，然后再把box移除。

planning_scene.robot_state.attached_collision_objects.push_back(attached_object);

这里是把“box”连接到机器人的手上。

planning_scene.robot_state.is_diff=true;planning_scene_diff_publisher->publish(planning_scene);

robot_state 这部分是增量更新，不是替换机器人的状态；然后发布规划场景更新。

6.从机器人上分离一个物体

moveit_msgs::msg::AttachedCollisionObject detach_object;detach_object.object.id="box";detach_object.link_name="panda_hand";detach_object.object.operation=attached_object.object.REMOVE;

这里是设置一个把附着在机器人手上的box移除掉的对象，后续调用。

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.robot_state.attached_collision_objects.clear();planning_scene.robot_state.attached_collision_objects.push_back(detach_object);

这里是先清空当前 planning_scene 消息对象里的数组；添加前面的分离box对象，意思就是把“box”从机器人手上分离。

planning_scene.world.collision_objects.push_back(attached_object.object);

把box重新加回到世界中。

planning_scene_diff_publisher->publish(planning_scene);

发布消息，这里才是真正的对规划环境的变化，执行分离操作。

7.将物体从碰撞世界移除

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);

把box从环境中彻底。

总结

用到的接口如下：

rviz_visual_tools::RvizVisualTools visual_tools.loadRemoteControl()visual_tools.deleteAllMarkers()visual_tools.prompt()

rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr node->create_publisher<moveit_msgs::msg::PlanningScene>()planning_scene_diff_publisher->get_subscription_count()planning_scene_diff_publisher->publish()

rclcpp::Client<moveit_msgs::srv::ApplyPlanningScene>::SharedPtr node->create_client<moveit_msgs::srv::ApplyPlanningScene>()planning_scene_diff_client->wait_for_service()planning_scene_diff_client->async_send_request()

moveit_msgs::srv::ApplyPlanningScene moveit_msgs::srv::ApplyPlanningScene::Request moveit_msgs::srv::ApplyPlanningScene_Response

std::make_shared<moveit_msgs::srv::ApplyPlanningScene::Request>()request->scene

std::shared_future<std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response>>response_future.wait_for()response_future.get()response_future=planning_scene_diff_client->async_send_request(request).future.share()

std::chrono::seconds std::chrono::milliseconds rclcpp::sleep_for()std::future_status std::future_status::timeout

moveit_msgs::msg::PlanningScene planning_scene.world planning_scene.world.collision_objects planning_scene.world.collision_objects.clear()planning_scene.world.collision_objects.push_back()planning_scene.robot_state planning_scene.robot_state.attached_collision_objects planning_scene.robot_state.attached_collision_objects.clear()planning_scene.robot_state.attached_collision_objects.push_back()planning_scene.robot_state.is_diff planning_scene.is_diff

moveit_msgs::msg::CollisionObject collision_object.id collision_object.header.frame_id collision_object.operation collision_object.ADD collision_object.REMOVE collision_object.primitives collision_object.primitives.push_back()collision_object.primitive_poses collision_object.primitive_poses.push_back()

moveit_msgs::msg::AttachedCollisionObject attached_object.link_name attached_object.object attached_object.object.header.frame_id attached_object.object.id attached_object.object.operation attached_object.touch_links

shape_msgs::msg::SolidPrimitive primitive.type primitive.BOX primitive.dimensions primitive.dimensions.resize()

geometry_msgs::msg::Pose pose.position.z pose.orientation.w

std::vector<std::string>std::vector<std::string>{}

RCLCPP_INFO()RCLCPP_ERROR()