Providing MoveIt! with (external) octomap data
I'm aware of the possibility of quickly setting up MoveIt! to use point cloud sensors and generate octomap data from their clouds, but what I'd like to do is retrieving a area of interest from another node providing a octomap and using that inside MoveIt!. I have a custom MoveGroup plugin that listens to requests for planning. If one comes in, it should retrieve the relevant octomap via a service call and then plan. Apart from the collision avoidance step the plugin already works as intended. Any ideas/suggestions are much appreciated.
/edit: So the first idea I have is instantiating a planning scene, keeping track of it manually and using the planning scene monitor constructor taking a planning scene as argument (for getting the planning scene monitor benefits like tf monitoring etc.). I'll then try updating the planning scene with a octomap manually, locking the planning scene monitor afterwards and then starting planning.
/edit2: After taking a close look I noted that I can indeed retrieve the planning_scene from the planning_scene_monitor and add the octomap data. Normally, my octomap data are in a frame not belonging to the robot ("/map"), thus when trying to add the octomap message to the scene I get:
[ERROR] [1367584563.653419864, 80.883000000]: Transform from frame 'map' to frame 'pelvis' is not known ('map' should be a link name or an attached body id).
Is there any way for working around this? Transforming octomap data between frames isn't possible right now, so from what I'm seeing my options are:
- Changing internals of MoveIt! so it also can use transforms outside of those belonging to the robot
- Providing a octomap already in the "pelvis" frame
Did you get an answer?
Do you have the code for publishing the octomap?