Ask Your Question
0

How to save OctoMap from MoveIt!

asked 2017-03-06 03:45:33 -0600

HenrySleek gravatar image

updated 2017-03-08 02:41:02 -0600

I am running Ubuntu 14.04 and ROS Indigo. I am currently using MoveIt! for generating an OctoMap from the robot sensors. How can I save that OctoMap? Or, alternatively, how can I access it in order to obtain occupancy information from specific points? I just know the type of the topic (moveit_msgs/PlanningScene), but I don't know how to work on the map or access it.

I know that there is a similar question here in ROSanswers, I checked it, but that does not answer my question since the map is not generated via MoveIt!. So if you know the answer, I would extremely appreciate.

edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
1

answered 2017-03-08 03:25:12 -0600

v4hn gravatar image

There are multiple ways to do that and it depends on your setup. I assume for you it's probably most easy to call the get_planning_scene ROS service offered by move_group (set the OCTOMAP bit in the components bit field to ask for the octomap).

Then, if you want to convert the octomap message to an object of liboctomap, have a look at the conversions.h header in the octomap_msgs package.

edit flag offensive delete link more

Comments

Thank you, I already found a (more complicated) way for getting the octomap_msgs::Octomap from it. Now my problem is with the conversion: I used the following:

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

But I have problems with the AbstractOcTree header.

HenrySleek gravatar imageHenrySleek ( 2017-03-08 04:17:33 -0600 )edit

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’".

HenrySleek gravatar imageHenrySleek ( 2017-03-08 04:18:55 -0600 )edit

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 have. 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 it?

HenrySleek gravatar imageHenrySleek ( 2017-03-08 04:19:53 -0600 )edit

I would like to manipulate the octomap further rather than saving it. Is there a way to get the octomap at a faster rate than the ros service? Through the planning scene monitor?

hc gravatar imagehc ( 2017-07-27 03:37:58 -0600 )edit
0

answered 2017-03-08 10:53:33 -0600

HenrySleek gravatar image

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.

edit flag offensive delete link more

Comments

Did you use the ros service to get the planning scene or are you using the planning scene monitor?

hc gravatar imagehc ( 2017-07-27 03:32:03 -0600 )edit

It uses a subscriber attached to the /move_group/monitored_planning_scene topic. Service approach is described by @v4hn.

tahsinkose gravatar imagetahsinkose ( 2018-09-09 11:51:04 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-03-06 03:45:33 -0600

Seen: 471 times

Last updated: Mar 08 '17