RViz segfaults when moving arm into collision with an object (use-after-free in `CollisionEnvFCL`)

[ohrad-robotics]

Description

Description

RViz crashes (SIGSEGV) when you try to move the robot (e.g. drag the goal-state interactive marker) into collision with an object. The crash is triggered by the next live collision check RViz runs as you move the robot's goal/start state.

Root Cause

This is a use-after-free, not an AllowedCollisionMatrix bug (that's just where the corrupted read happens to be observed).

1. collision_detection::World::ensureUnique() (moveit_core/collision_detection/src/world.cpp:131) copy-on-write clones a World::Object whenever it's about to be mutated while something else still holds a shared_ptr to it (!obj.unique()). RViz's MotionPlanningDisplay can transiently hold such a reference while it live-checks collisions for the goal/start state on the Qt GUI timer (PlanningSceneDisplay::executeMainLoopJobs → drawQueryGoalState).
2. CollisionEnvFCL::notifyObjectChange()'s fast path for World::MOVE_SHAPE (moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp, added in Fix: preserve planar joint suffix in Ros2ControlManager (Fixes #3594) #3601 for performance) updates the existing FCL collision object's transform in place, but never refreshes CollisionGeometryData::ptr.obj — a raw, non-owning const World::Object* cached at construction time (see collision_common.hpp:86, getID()).
3. When the pre-move Object is later destructed (once the transient external reference from step 1 goes out of scope), that cached raw pointer becomes dangling.
4. The next collision check dereferences it while reading the moved object's id string (CollisionGeometryData::getID() → ptr.obj->id_), corrupting the read — observed here as a segfault inside AllowedCollisionMatrix::getEntry()'s std::map lookup, but the actual defect is entirely in CollisionEnvFCL.

Notably, createCollisionGeometry() (collision_common.cpp) already has logic for exactly this copy-on-write scenario — when the shape-cache hits but the stored object pointer no longer matches, it calls FCLGeometry::updateCollisionGeometryData() to refresh the pointer. The MOVE_SHAPE fast path in notifyObjectChange() bypasses that safety net entirely.

Regression Provenance

Bisected via git blame: the MOVE_SHAPE branch in notifyObjectChange() was introduced by #3601 ("Optimize MOVE_SHAPE operations for FCL", commit 1d93035fc4, merged 2024-07-19).

Environment

• ROS 2 Jazzy, Ubuntu 24.04 (Noble)
• moveit2 built from source, main branch
• Reproduced against both the ros-jazzy-* apt-installed binaries and a from-source workspace build

Proposed Fix

See linked PR: refresh CollisionGeometryData::ptr.obj via the existing FCLGeometry::updateCollisionGeometryData() helper in the MOVE_SHAPE branch of notifyObjectChange(), alongside the transform update. A regression test is included that deterministically reproduces the stale-pointer condition (via a pointer-identity check, not reliant on the process actually crashing) and fails without the fix / passes with it.

#3797

ROS Distro

Jazzy

OS and version

Ubuntu 24.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

main

Which RMW are you using?

FastRTPS

Steps to Reproduce

1. ros2 launch moveit_resources_panda_moveit_config demo.launch.py
2. In the Scene Objects panel, add a box collision object and publish it into the scene.
3. Move the box.
4. With the Query Goal State marker enabled, try to move the robot itself (drag the goal-state interactive marker) into collision with the collision box object.

Result: rviz2 dies immediately with SIGSEGV (exit code -11) as soon as you try to move the robot into collision.

Note on why collision matters: the underlying defect (see Root Cause) creates a dangling pointer on a MOVE_SHAPE update whenever World::ensureUnique()'s copy-on-write clone fires — which in practice happens on effectively every move while the object is live in the scene (e.g. displayed in the Scene Objects panel). That dangling pointer is only ever dereferenced (crashing the process) once FCL's broad-phase collision manager actually invokes its narrow-phase callback for that object, which only happens when its AABB overlaps the robot's. If the moved object isn't in collision with the robot, the broad-phase pass skips it entirely and the corruption goes unread — which is why proximity alone isn't enough to reproduce, but actual collision reproduces every time.

Expected behavior

You should be able to reposition the arm, even if the current state is in collision with an object in the scene. When the arm is in collision with an object, it should show as red.

Actual behavior

Rviz crashes when trying to move the arm when it is in collision with an object in the scene

Backtrace or Console output

Captured with a RelWithDebInfo build of moveit_core under gdb, reproducing the exact steps above:

Thread 1 "rviz2" received signal SIGSEGV, Segmentation fault.
__memcmp_avx2_movbe ()
#0  __memcmp_avx2_movbe ()
#1  std::char_traits<char>::compare (...)
#2  std::__cxx11::basic_string<...>::compare (this=..., __str=<error reading variable: Cannot create a
    lazy string with address 0x0, and a non-zero length.>)
...
#8  collision_detection::AllowedCollisionMatrix::getEntry (this=..., name1=<garbage/corrupted string>,
    name2="panda_link5", ...) at moveit_core/collision_detection/src/collision_matrix.cpp:130
#9  collision_detection::AllowedCollisionMatrix::getAllowedCollision (...) at collision_matrix.cpp:369
#10 collision_detection::collisionCallback (...) at moveit_core/collision_detection_fcl/src/collision_common.cpp:103
#11 collision_detection::CollisionEnvFCL::checkRobotCollisionHelper (...) at collision_env_fcl.cpp:338
#12 planning_scene::PlanningScene::checkCollision (...) at planning_scene.cpp:443
#13 planning_scene::PlanningScene::getCollidingPairs (...) at planning_scene.cpp:585
#14 planning_scene::PlanningScene::getCollidingLinks (...)
#15 moveit_rviz_plugin::MotionPlanningDisplay::drawQueryGoalState (...) at motion_planning_display.cpp:757
#16 moveit_rviz_plugin::MotionPlanningDisplay::changedQueryGoalState (...) at motion_planning_display.cpp:737
#17 moveit_rviz_plugin::PlanningSceneDisplay::executeMainLoopJobs (...) at planning_scene_display.cpp:283
#18 moveit_rviz_plugin::PlanningSceneDisplay::update (...) at planning_scene_display.cpp:691
#19 rviz_common::DisplayGroup::update(float, float) ()
#20 rviz_common::VisualizationManager::onUpdate() ()
... (Qt event loop) ...

Note name1 in frame #8 fails to print at all (gdb.error / OverflowError reading the string) — a strong signal of reading garbage/freed memory, not a logic bug in the ACM lookup itself.