Collision object MOVE modifies object pose before validating shape pose count

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

Maintainers usually reply within 1 day

Nobody has claimed this yet.

  • #3956 by @paneendrakumar0 — closed without merging

Assessment

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

Research direction

Read moveit_core/planning_scene/src/planning_scene.cpp around processCollisionObjectMove() and trace the MOVE validation and world updates. Add or locate a regression test for a shape pose count mismatch, then run it. Done means the request returns false and leaves the object's global pose and individual shape poses unchanged.

Written by the indexing model from the issue text.

Description

bug
Description

In PlanningScene::processCollisionObjectMove(), the collision object's global pose is updated before validating the number of shape poses provided in the MOVE message.

Affected file:

moveit_core/planning_scene/src/planning_scene.cpp

The object pose is updated first (around line 1976):

const Eigen::Isometry3d object_frame_transform =
world_to_object_header_transform * header_to_pose_transform;

world_->setObjectPose(object.id, object_frame_transform);

However, the number of supplied shape poses is validated only afterward (around lines 1979–1989):

if (!object.primitive_poses.empty() ||
!object.mesh_poses.empty() ||
!object.plane_poses.empty())
{
auto world_object = world_->getObject(object.id);

std::size_t shape_size =
object.primitive_poses.size() +
object.mesh_poses.size() +
object.plane_poses.size();

if (shape_size != world_object->shape_poses_.size())
{
RCLCPP_ERROR(
getLogger(),
"Move operation for object '%s' must have same number of geometry poses. Cannot move.",
object.id.c_str());
return false;
}
}

Consequently, if the shape pose count does not match, the function returns false, but the global object pose has already been modified.

This creates a partial state update: the operation reports failure while the PlanningScene may retain a changed collision object pose.

This can cause inconsistencies between the caller's understanding of the operation result and the collision world state.

Suggested Fix

Validate the supplied shape pose count before calling world_->setObjectPose()

Ideally, all MOVE request validation should be completed before applying any modification to the collision world.

A regression test should verify that a rejected MOVE request leaves both the global object pose and the individual shape poses unchanged.

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 with a valid robot model.
  2. Add a collision object named test_object containing two primitive shapes.
  3. Record the object's initial global pose.
  4. Construct a moveit_msgs::msg::CollisionObject with:
    • id = "test_object"
    • operation = CollisionObject::MOVE
    • A new global object pose different from the original pose.
    • Only one entry in primitive_poses, despite the existing object containing two shapes.
  5. Submit the MOVE message through PlanningScene::processCollisionObjectMsg().
  6. Check the returned status.
  7. Retrieve test_object from the collision world and compare its global pose with the original pose.

The shape pose count mismatch should trigger the validation failure in processCollisionObjectMove().

Expected behavior

When a MOVE request contains an invalid number of shape poses, the operation should be rejected without modifying the existing collision object.

The function should return false, and both the global object pose and individual shape poses should remain unchanged.

Failed MOVE operations should not leave the PlanningScene in a partially modified state.

Actual behavior

Source inspection shows that processCollisionObjectMove() updates the global object pose before checking whether the number of supplied shape poses matches the number of shapes in the existing object.

When the shape pose count does not match, the function logs an error and returns false. However, the earlier call to world_->setObjectPose() has already been executed.

Therefore, the function can report failure while leaving the collision object's global pose modified.

This behavior is supported by the current source code, but a runtime regression test has not yet been executed.

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.