Saving a map from MoveIt!

[Enrico Liscio]

Hey! 

I am quite new to OctoMaps, so this question may seem silly, but an answer would be really useful to me. I am using a robot with MoveIt! and hence it has perception of the surrounding environment as an OctoMap. I would like to save or to access that OctoMap, in order to collect occupancy information from specific points in a specific area of the map. How can I save it from MoveIt!? I could only find the topic in which it is published, but is there a way of saving it in a .bt file, or to access it? I don't need it to be updated, a frame taken in a specific moment would be more than enough.

Thank you.

[Enrico Liscio]

Update: I implemented the following piece of code (I hope you will forgive the bad organisation and the lack of imagination for the names):

void cloud_cb(const moveit_msgs::PlanningScenePtr& input)

  {

    moveit_msgs::PlanningScene::Ptr my_planning_scene(new moveit_msgs::PlanningScene);

    *my_planning_scene = *input;

    moveit_msgs::PlanningSceneWorld my_world = (*my_planning_scene).world;

    moveit_msgs::PlanningSceneWorld::Ptr real_map(&my_world);

    octomap_msgs::OctomapWithPose octomap_pose_content = (*real_map).octomap;

    octomap_msgs::OctomapWithPose::Ptr octomap_pose(&octomap_pose_content);

    octomap_msgs::Octomap octomap_content = (*octomap_pose).octomap;

    octomap_msgs::Octomap::Ptr octomap(&octomap_content);

    octomap::AbstractOcTree* my_map = octomap_msgs::binaryMsgToMap(octomap_content);

  }

It works, or at least until before the conversion to AbstractOcTree, because when I build it, I need to include the header <octomap/AbstractOcTree.h>, and along with that all the other headers in the same folder for letting it work. Nevertheless, when I do so, I have the problem that "OcTreeBaseSE.hxx:83:31: error: ‘octomap::point3d’ has no member named ‘norm2’". And indeed, for a point3d type only .norm is defined, I couldn't find any .norm2. So I guess there is a bug in what I downloaded. I didn't directly install it, since it came along with the robot, but in any case if I run 'sudo apt-get install ros-indigo-octomap', I have the latest version. How do I solve this?

[Enrico Liscio]

Final update: I solved it. The following code works, given the inclusion of all the correct headers:

using namespace octomap;

void cloud_cb(const moveit_msgs::PlanningScenePtr& input)
{

// Extracting the MoveIt! planning scene world published by /move_group

moveit_msgs::PlanningScene::Ptr my_planning_scene(new moveit_msgs::PlanningScene);
*my_planning_scene = *input;
moveit_msgs::PlanningSceneWorld my_world = (*my_planning_scene).world;

// Extracting only the OctoMap
octomap_msgs::OctomapWithPose octomap_pose = my_world.octomap;
octomap_msgs::Octomap octomap = octomap_pose.octomap;

// Conversion from octomap_msgs to octomap
AbstractOcTree* my_abstract_map = octomap_msgs::msgToMap(octomap);

// Obtaining the actual OctoMap tree
OcTree* my_map = (OcTree*)my_abstract_map;
OcTree tree = *my_map;

tree.writeBinary("my_tree.bt"); //if you want to save the OcTree in a file
}

[Enrico Liscio]

[Armin Hornung]

Yes, that code looks like it's doing the right thing! By the way, there's also an `octomap_saver` node in the octomap_server ROS package. It directly allows you to save an octree via service call, although not via topic subscription.

Best,
Armin