c++saverosmoveit

Save MoveIt! Octomap


I am running Ubuntu 14.04 and ROS Indigo. I want to access the OctoMap generated via MoveIt! and convert it in standard OctoMap format and save/process it. The following piece of code should do it:

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::AbstractOcTree* my_map = octomap_msgs::binaryMsgToMap(octomap_content);
}

Please forgive the mess and the lack of imagination for the names. It works, until the conversion to AbstractOcTree. When I catkin_make, I have the error that many, many references are undefined, as for example the reference to octomath::Quaternion::Quaternion(octomath::Quaternion const&). Even if I include all the headers available for octomap and octomath, the error stays the same. How can I solve this?


Solution

  • 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.