ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

How to construct a PlanningSceneWorld message in Moveit2

asked 2022-09-05 21:04:42 -0500

khairulm gravatar image

Hi, i'm currently building an autonomous drone that can do indoor navigation, mapping, and localization with ROS 2 Foxy. For the navigation stack, i chose to use Moveit2 since it has 3d planning and navigation. But since i was already using RTAB-Map for SLAM, i wanted to utilize the octomap published by it to update the planning scene in Moveit2. I saw in the documentation that i can do this by publishing octomap updates using a PlanningSceneWorld message to the /planning_scene_world topic (link). In the message definition i notice that it was using octomap_msgs/OctomapWithPose to feed outside octomap update, and i can't seem to find a way on how to construct this message using the provided octomap from RTAB-Map. So my question is:

  1. what should be the frame in the header of octomap_msgs/OctomapWithPose if i already have a map -> odom -> base_link transformation
  2. how to get the pose information of the octomap generated by RTAB-Map for the pose field in octomap_msgs/OctomapWithPose
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2022-09-08 22:07:15 -0500

khairulm gravatar image

I think i manage to solve this myself,

  1. use "map" for the header frame of octomap
  2. i just use a zero vector pose (i.e. msg.origin = geometry_msgs::msg::Pose())

i've visualize the planning scene using moveit visualization package in rviz and it seems correct, at least to my eyes (the scale and the pose of the octomap)

other thing to note is that the octomap generated by RTAB-Map uses ColorOcTree, whereas the planning scene monitor only accepts OcTree, you need to deserialize the ColorOcTree from RTAB-Map first and convert it to an OcTree before publishing to the planning scene monitor

// read and convert the coloroctree by rtabmap to regular octree and create the message for it
octomap::OcTree *octree = static_cast<octomap::OcTree *>(octomap_msgs::binaryMsgToMap(*msg));
octomap_msgs::binaryMapToMsg<octomap::OcTree>(*octree, *msg);
msg->id = "OcTree";
edit flag offensive delete link more

Question Tools


Asked: 2022-09-05 21:04:42 -0500

Seen: 228 times

Last updated: Sep 08 '22