Planning with attached collision objects and custom start state using move group c++ interface

edit

The Problem

I am attempting to use moveit to perform motion planning for a robot with an attached collision object.

The object has been attached from a different node using the /attached_collision_object topic. That is, not through the c++ move group interface.

I am providing a custom start state for the planner rather than using the current robot state.

The issue is: When planning moveit does not take collisions between the attached object and the robot into account.

My Understanding of the Issue

I am not 100% sure, but I think that the issue is that when I set a start state, the planning interface will send that state to the move_group node with is_diff set to false such that it completely overwrites the robot state maintained by the move group.

The move group c++ interface is supposed to be able to keep track of the current robot state.

So it seems logical to me that I should be able to get the current robot state from the move group c++ interface, overwrite the parts I wish to change, and then use it as the start state. Then only the things I overwrite should be different from the actual current robot state (ignoring any concurrency/synchronization issues).

But as far as I can tell from the move group c++ interface sources, it is actually monitoring joint state and tf topics, so it does not keep track of attached collision objects. The attached collision objects are however a part of the message send to the move_group, so they will be overwritten (that is deleted because according to the move group interface there are none, because it is not monitoring them).

This almost seems like an incompatibility between the move_group node and the move group c++ interface. The move_group node considers the attached collision objects part of the robot state, but the c++ interface does not. Well.. It does because the data structure can represent them, but the component which is supposed to monitor the robot state does not monitor them, so unless you manually fill them out there never are any.

Possible Solutions?

It seems to me that to solve this issue one of two things should happen

1. Set is_diff to true in the motion plan request, or
2. Obtain the attached collision objects and put them into the start state.

It appears (1) is not possible using the move group c++ interface. That value is set deep down in some function with no way of controlling it.

For (2), I can get the attached collision objects using a planning scene interface without too much trouble. To add them to the robot state they need to be turned into moveit AttachedBody object. I can find a conversion function going in the other direction, but not the direction I need. I could probably implement this myself but it seems a bit awkward.

My Question

I think I can probably get it to work using approach 2 (otherwise I will have to ask another ... (more)