Description

tl;dr:

The attached collision objects at each waypoint along a RobotTrajectory are not aliased, and so the FCLShapeCache misses the cache for each new waypoint, and a huge amount of processing time is spent in the fcl collision detection algorithm creating an fcl::BVHModel for each object.

Longer context:

I am planning trajectories using pilz_industrial_motion_planner's plan_sequence_path service. I am using relatively large attached collision meshes (~100k faces). These trajectories take a very long time to plan (~1s per waypoint), and more importantly the move_group node leaks memory continuously (~10Mb per waypoint), which is not recovered when the plan is finished.

I have been profiling the move_group, to see if what I'm wanting to do is possible, and have found that almost all (>99%) of the processing time is spent in the FCLGeometryConstPtr::createCollisionGeometry method. I believe (although correct me if I'm wrong) that the vast majority of this time would be saved if the FCLShapeCache was hit, but instead it is missed every time. This is happening because the attached collision objects associated with each waypoint along the RobotTrajectory are different objects (even though they represent the same objects, just moved slightly).

Your environment

• ROS Distro: Noetic
• OS Version: 20.04
• Source or Binary build? Both; first the binary build, and then the source code when debugging.
• If binary, which release version? 1.1.12
• If source, which branch? master: b4b1a27f605031c54e80a9b3465f7a3cabe6cc88

Steps to reproduce

I can set this up if it would help, but I don't think it's that kind of bug.

Expected behaviour

I would assume the attached collision objects along a single trajectory would be cached, so they didn't have to be processed for every step along the trajectory.

Actual behaviour

Planning takes a long time, and the memory usage of the move_group nodes increases quickly, and does not go down again when planning has finished.

Backtrace or Console output

N/A