Robotics StackExchange | Archived questions

How to construct a PlanningSceneWorld message in Moveit2

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

Asked by khairulm on 2022-09-05 21:04:42 UTC

Comments

Answers

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

Asked by khairulm on 2022-09-08 22:07:15 UTC

Comments