0
$\begingroup$

Rosanswers logo

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.


Originally posted by HenrySleek on ROS Answers with karma: 53 on 2017-03-06

Post score: 0

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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.


Originally posted by v4hn with karma: 2950 on 2017-03-08

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by HenrySleek on 2017-03-08:
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.

Comment by HenrySleek on 2017-03-08:
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’".

Comment by HenrySleek on 2017-03-08:
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?

Comment by hc on 2017-07-27:
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?

$\endgroup$
0
$\begingroup$

Rosanswers logo

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.


Originally posted by HenrySleek with karma: 53 on 2017-03-08

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by hc on 2017-07-27:
Did you use the ros service to get the planning scene or are you using the planning scene monitor?

Comment by tahsinkose on 2018-09-09:
It uses a subscriber attached to the /move_group/monitored_planning_scene topic. Service approach is described by @v4hn.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.