Revision history [back]

Ok, my problem was actually pretty stupid: I had not included the OctoMap directories and did not link the libraries in the CMakeList, as stated in http://wiki.ros.org/octomap. Stupid me. In any case I am posting here my solution. I don't know if there is a smarter/faster way of doing it, but this one works:

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
}

And remember to include all the needed headers, as octomap_msgs/conversions.h and so on.