From 438609fe43fa96f578ba6d58d86353cc9b71a77d Mon Sep 17 00:00:00 2001 From: Aleksey Nogin Date: Sun, 1 Dec 2024 21:59:05 -0500 Subject: [PATCH] Do not switch object and shape poses of attached objects with subframes --- moveit_core/planning_scene/src/planning_scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index bf8ea6df13..e5b4abcb2c 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1817,7 +1817,7 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: shape_poses.reserve(num_shapes); bool switch_object_pose_and_shape_pose = false; - if (num_shapes == 1 && moveit::core::isEmpty(object.pose)) + if (num_shapes == 1 && (!object.subframe_poses.size()) && moveit::core::isEmpty(object.pose)) { // If the object pose is not set but the shape pose is, use the shape's pose as the object pose. switch_object_pose_and_shape_pose = true;