Movelt2 规划场景 ROS API

文章目录前言一、使用rviz可视化二、ROS API1.发布所需话题2.定义附着碰撞物体的消息3.将一个对象添加到环境中4.同步更新和异步更新的区别5.将一个物体连接到机器人上6.从机器人上分离一个物体7.将物体从碰撞世界移除总结前言本文整理的是 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 API1.发布所需话题rclcpp::Publishermoveit_msgs::msg::PlanningScene::SharedPtr planning_scene_diff_publishernode-create_publishermoveit_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::Publishermoveit_msgs::msg::PlanningScene::SharedPtr planning_scene_diff_publishernode-create_publishermoveit_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 中点击 Next2.定义附着碰撞物体的消息以下代码是在世界中添加将物体连接到机器人上然后也可去掉物体。moveit_msgs::msg::AttachedCollisionObject attached_object;attached_object.link_namepanda_hand;attached_object.object.header.frame_idpanda_hand;attached_object.object.idbox;geometry_msgs::msg::Pose pose;pose.position.z0.11;pose.orientation.w1.0;shape_msgs::msg::SolidPrimitive primitive;primitive.typeprimitive.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_namepanda_hand;attached_object.object.header.frame_idpanda_hand;attached_object.object.idbox;这里是创建一个可以连接到机器人的碰撞物体附着在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::vectorshape_msgs::msg::SolidPrimitive primitives;std::vectorgeometry_msgs::msg::Pose primitive_poses;std::vectorshape_msgs::msg::Mesh meshes;std::vectorgeometry_msgs::msg::Pose mesh_poses;std::vectorshape_msgs::msg::Plane planes;std::vectorgeometry_msgs::msg::Pose plane_poses;std::vectorstd::string subframe_names;std::vectorgeometry_msgs::msg::Pose subframe_poses;uint8_t operation;geometry_msgs::msg::Pose pose;pose.position.z0.11;pose.orientation.w1.0;定义碰撞物体的位姿这个位姿是相对于panda_hand来说。shape_msgs::msg::SolidPrimitive primitive;primitive.typeprimitive.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.operationattached_object.object.ADD;这里使用的“operation”是表示要对这个box进行添加操作。常见的操作有ADD // 添加或更新物体REMOVE // 删除物体APPEND // 追加几何体MOVE // 移动物体在把碰撞物体附加在机器人手上后需要碰撞检测器忽略物体与机器人之间的碰撞attached_object.touch_linksstd::vectorstd::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_difftrue;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_difftrue;这里是说在原来的PlanningScene基础上加入这个box为的是不把原来的场景覆盖掉。planning_scene_diff_publisher-publish(planning_scene);这里是把当前这个PlanningScene的更新发布让moveit更新场景发布到/planning_scene下面是全面创建的发布者。node-create_publishermoveit_msgs::msg::PlanningScene(“planning_scene”, 1);visual_tools.prompt(Press next in the RvizVisualToolsGui window to continue the demo);暂停等 RViz 点击 Next4.同步更新和异步更新的区别有两种独立的机制可以通过差异更新diffs与 move_group 节点进行交互。1.同步更新通过 ROS 服务调用发送一个差异更新并阻塞等待直到这个差异更新被应用完成。2.异步更新通过话题发送一个差异更新即使这个差异更新可能还没有被应用也继续往下执行。全面的教程是使用异步更新接下来介绍同步更新更稳定rclcpp::Clientmoveit_msgs::srv::ApplyPlanningScene::SharedPtr planning_scene_diff_clientnode-create_clientmoveit_msgs::srv::ApplyPlanningScene(apply_planning_scene);planning_scene_diff_client-wait_for_service();创建客户端话题名为/apply_planning_scene后续可以用他发生请求。moveit_msgs::srv::ApplyPlanningScene 是专门用于先movelt申请更新PlanningSceneplanning_scene_diff_client-wait_for_service();等待服务启动这个服务通常由 MoveIt 的 move_group 节点提供。前面是使用topic发布/planning_scene属于异步更新发出去就继续不保证 MoveIt 已经处理完。这里是使用service属于同步更新会等待 MoveIt 返回结果确认场景是否应用成功。autorequeststd::make_sharedmoveit_msgs::srv::ApplyPlanningScene::Request();request-sceneplanning_scene;std::shared_futurestd::shared_ptrmoveit_msgs::srv::ApplyPlanningScene_Responseresponse_future;response_futureplanning_scene_diff_client-async_send_request(request).future.share();代码解释autorequeststd::make_sharedmoveit_msgs::srv::ApplyPlanningScene::Request();request-sceneplanning_scene;这里是创建请求因为客户端需要向服务端发送请求服务端才会进行处理把planning_scene填入请求中。std::shared_futurestd::shared_ptrmoveit_msgs::srv::ApplyPlanningScene_Responseresponse_future;response_futureplanning_scene_diff_client-async_send_request(request).future.share();定义一个未来返回结果的对象用于保存未来返回的结果然后异步发送请求(程序不会在这一行卡住等结果)也就是向向 /apply_planning_scene 服务发送请求然后把结果保存到response_futurestd::chrono::secondswait_time(1);std::future_status fsresponse_future.wait_for(wait_time);if(fsstd::future_status::timeout){RCLCPP_ERROR(LOGGER,Service timed out.);}else{std::shared_ptrmoveit_msgs::srv::ApplyPlanningScene_Responseplanning_response;planning_responseresponse_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 fsresponse_future.wait_for(wait_time);设置等待时间fs是返回的结果类型是std::future_statusstd::future_status::readystd::future_status::timeoutstd::future_status::deferredstd::shared_ptrmoveit_msgs::srv::ApplyPlanningScene_Responseplanning_response;planning_responseresponse_future.get();这里创建一个planning_response来获取response_future中服务端的响应。5.将一个物体连接到机器人上/* First, define the REMOVE object message*/moveit_msgs::msg::CollisionObject remove_object;remove_object.idbox;remove_object.header.frame_idpanda_hand;remove_object.operationremove_object.REMOVE;定义一个删除碰撞物体的消息从规划场景中删除“box”这个box和前文创建的是同一个boxmovelt主要是根据 id 找对应的 collision object。设置参考坐标系类型是REMOVEattached_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_difftrue;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_difftrue;planning_scene_diff_publisher-publish(planning_scene);robot_state 这部分是增量更新不是替换机器人的状态然后发布规划场景更新。6.从机器人上分离一个物体moveit_msgs::msg::AttachedCollisionObject detach_object;detach_object.object.idbox;detach_object.link_namepanda_hand;detach_object.object.operationattached_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_difftrue;planning_scene.world.collision_objects.clear();planning_scene.world.collision_objects.push_back(attached_object.object);planning_scene.is_difftrue;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::Publishermoveit_msgs::msg::PlanningScene::SharedPtr node-create_publishermoveit_msgs::msg::PlanningScene()planning_scene_diff_publisher-get_subscription_count()planning_scene_diff_publisher-publish()rclcpp::Clientmoveit_msgs::srv::ApplyPlanningScene::SharedPtr node-create_clientmoveit_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_Responsestd::make_sharedmoveit_msgs::srv::ApplyPlanningScene::Request()request-scenestd::shared_futurestd::shared_ptrmoveit_msgs::srv::ApplyPlanningScene_Responseresponse_future.wait_for()response_future.get()response_futureplanning_scene_diff_client-async_send_request(request).future.share()std::chrono::seconds std::chrono::milliseconds rclcpp::sleep_for()std::future_status std::future_status::timeoutmoveit_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_diffmoveit_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_linksshape_msgs::msg::SolidPrimitive primitive.type primitive.BOX primitive.dimensions primitive.dimensions.resize()geometry_msgs::msg::Pose pose.position.z pose.orientation.wstd::vectorstd::stringstd::vectorstd::string{}RCLCPP_INFO()RCLCPP_ERROR()