Failed collision object ADD replacement removes the existing object before validation

Open Beginner friendly
  • Is something wrong?
#3,942 2 comments 0 reactions 0 assignees View on GitHub

Maintainers usually reply within 1 day

@paneendrakumar0 is already working on this.

Since Oct 29, 2026.

  • #3951 by @paneendrakumar0 — open

Assessment

Difficulty
2/5
Estimated time
1-3 hours
Newbie friendliness
76/100
Issue type
Bug
Clarity
Clearly specified
Activity status
Active
Tech stack
cpp
Domain
robotics

Research direction

Start in moveit_core/planning_scene/src/planning_scene.cpp at PlanningScene::processCollisionObjectAdd() and trace how replacement ADD messages are converted and added to the world. Then find the PlanningScene collision object tests and add a case with mismatched primitive and pose counts. Done means the replacement is rejected and the original object's geometry and pose remain unchanged.

Written by the indexing model from the issue text.

Description

bug
Description

In PlanningScene::processCollisionObjectAdd(), an existing collision object is removed before the replacement object's geometry is validated.

Affected file:

moveit_core/planning_scene/src/planning_scene.cpp

When an ADD operation targets an existing collision object, the following code removes the original object:

if (object.operation == moveit_msgs::msg::CollisionObject::ADD && world_->hasObject(object.id))
world_->removeObject(object.id);

However, the replacement geometry is only parsed and validated afterward:

const Eigen::Isometry3d& world_to_object_header_transform =
getFrameTransform(object.header.frame_id);

Eigen::Isometry3d header_to_pose_transform;
std::vectorshapes::ShapeConstPtr shapes;
EigenSTL::vector_Isometry3d shape_poses;

if (!shapesAndPosesFromCollisionObjectMessage(
object, header_to_pose_transform, shapes, shape_poses))
return false;

const Eigen::Isometry3d object_frame_transform =
world_to_object_header_transform * header_to_pose_transform;

world_->addToObject(object.id, object_frame_transform, shapes, shape_poses);

The geometry conversion function can return false when the input contains an invalid number of shape poses.

For example, if primitive_poses.size() exceeds primitives.size(), the conversion fails.

As a result, a malformed replacement ADD request can remove an existing valid collision object and then return false without restoring it.

This creates a partial state update and may cause an obstacle to disappear from the collision world even though the replacement operation failed.

In applications that rely on the PlanningScene for collision checking, this could lead to an incomplete representation of the environment.

Suggested Fix

Validate and construct the replacement geometry before removing the existing collision object.

The original object should remain unchanged if validation fails.

A regression test should verify that a rejected replacement ADD operation preserves the existing collision object and its geometry.

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

No response

Which RMW are you using?

None

Steps to Reproduce

Proposed reproduction steps (not yet executed):

  1. Create a PlanningScene instance with a valid robot model.
  2. Add a valid collision object with the ID test_object, containing one primitive shape.
  3. Verify that test_object exists in the collision world.
  4. Construct a new moveit_msgs::msg::CollisionObject message with:
    • id = "test_object"
    • operation = CollisionObject::ADD
    • The same valid header.frame_id as the original object.
    • One primitive shape.
    • Two entries in primitive_poses, intentionally creating a mismatch between the number of shapes and poses.
  5. Submit the replacement message using PlanningScene::processCollisionObjectMsg().
  6. Check the return value of the operation.
  7. Query the collision world using getWorld()->hasObject("test_object").

The mismatched shape and pose counts should cause
shapesAndPosesFromCollisionObjectMessage() to return false after the existing object has already been removed.

Expected behavior

When a replacement ADD message contains invalid geometry data, the operation should fail without modifying the existing collision object.

The original object should remain in the collision world with its geometry, pose, and associated state unchanged.

A failed replacement operation should not remove an existing valid obstacle from the PlanningScene.

Actual behavior

Source inspection shows that processCollisionObjectAdd() removes the existing collision object before validating the replacement geometry.

If shapesAndPosesFromCollisionObjectMessage() detects an invalid shape/pose count, it returns false, causing processCollisionObjectAdd() to terminate without adding the replacement object.

Consequently, the original collision object can be removed even though the replacement ADD operation reports failure.

This behavior is supported by the source-level control flow but has not yet been verified by a runtime regression test.

Backtrace or Console output

No response

Dominant language
C++
Stars
2.1k
Forks
816
Avg merge
1d 2h
Merged PRs (30d)
4

Getting set up

First steps

  1. Read the whole issue, then the project's contributing guide.
  2. Comment on the issue to say you are picking it up — it saves two people doing the same work.
  3. Fork the repository and make your change on a branch.
  4. Open a pull request that references the issue number.

More from moveit/moveit2

All issues in moveit/moveit2

Similar issues

More C++ issues

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.