Custom collision detection allocator is reset to the default FCL implementation

[medvedevigorek]

Description

In our setup we use a custom collision detection logic and for that we have implemented custom collision detector allocator. Upon creation of MoveItCpp instance we call allocateCollisionDetector() on the planning scene to replace the default collision detector (FCL) with our implementation.

auto pMoveIt = std::make_shared<moveit_cpp::MoveItCpp>(nh);
planning_scene_monitor::LockedPlanningSceneRW planningSceneRW(pMoveIt->getPlanningSceneMonitorNonConst());
planningSceneRW->allocateCollisionDetector(collision_detection::CustomCollisionDetectorAllocator::create());

However at some point later the collision detector allocator in the planning scene gets replaced back to FCL and as the result none of our custom logic is getting invoked during the motion planning phase.

We used similar logic in MoveIt1 - call setActiveCollisionDetector to set custom collision detector - where it worked fine.

Going thru the code it comes down to a difference in implementation of clearDiffs() method between MoveIt1 and MoveIt2, the latter replaces collision detector allocator while MoveIt1 keeps the current one.

If I got the sequence right by reading the code then it must be the following:

1. MoveItCpp is instantiated and it creates an instance of PlanningSceneMonitor
2. PlanningSceneMonitor creates an instance of PlanningScene with default collision detector allocator being set to FCL
3. PlanningSceneMonitor records current scene as parent_scene_ and starts scenePublishingThread
4. The user code updates the allocator via allocateCollisionDetector() call, this happens on scene_ object which happens to be a child scene now
5. Scene change happens and scenePublishingThread calls clearDiffs on scene_ instance and this results in the collision detector allocator being set to parent's one from step 2.

And the main questions are:

1. Is the current logic expected new behavior or is it indeed a bug?
2. If it's expected logic then how one is supposed to set a custom collision detection with Moveit2 now?

Your environment

• ROS Distro: Iron
• OS Version: Ubuntu 22.04
• Source or Binary build? Source
• If binary, which release version?
• If source, which branch? iron
• Which RMW (Fast DDS or Cyclone DDS)? Default Iron RMW

Steps to reproduce

See description above

Expected behaviour

I guess the expected behavior is to keep custom collision detection allocator if it was set by the user explicitly.

Actual behaviour

The collision detection allocator is being set back to the default one (FCL) upon scene update handling

Backtrace or Console output

[medvedevigorek]

As alternative option, we tried to set our custom collision detection as the default one via collision_detector parameter and while this approach works there are few drawbacks which make it impractical:

1. You no longer have control over how the collision detector allocator is instantiated as it must be packaged as a plugin so MoveIt2 loads and instantiates it for you.
2. There is no way (at least I couldn't find it yet) to access the instantiated instance to perform necessary initialization

[mikeferguson]

So, I have no idea how this has evolved over the years - but noting that way way back when I added support for custom collision interfaces (edfa221) the intention was that the custom collider would be a plugin (but also, at that point in 2014, everybody pretty much used the ROS MoveGroup interface and so a plugin was the natural way to load custom code, whereas MoveIt2 is certainly leaning more heavily into C++/Python interfaces for bigger applications).

[medvedevigorek]

@mikeferguson, yes, a plugin totally makes sense for MoveGroup interface. In addition to a plugin approach, PlanningScene had a method setActiveCollisionDetector which was useful when using MoveItCpp interface as you can set the active collision detector for a given planning scene. And this seems to be broken now in MoveIt2.

@AndyZe / @henningkayser , could you please look at this one and share your thoughts? Looks to me the things got broken after removing support of multiple collision detectors (#364)

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[henningkayser]

@medvedevigorek thanks for reporting this! I think the observed behavior is in line with the removal of supporting multiple collision backends at the same time #364 (which had its own pitfalls and issues). Aren't you able to allocate (and override) your custom collision checker with each of your MoveItCpp instances? Currently, there is no way to specify custom collision checkers at construction.

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.