From 438609fe43fa96f578ba6d58d86353cc9b71a77d Mon Sep 17 00:00:00 2001 From: Aleksey Nogin Date: Sun, 1 Dec 2024 21:59:05 -0500 Subject: [PATCH 1/2] 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; From 1a0ec71f8c864d0a4dd046469752a6311d5686b9 Mon Sep 17 00:00:00 2001 From: Aleksey Nogin Date: Mon, 2 Dec 2024 22:53:12 -0500 Subject: [PATCH 2/2] Fix a linter error --- 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 e5b4abcb2c..21b9b1c670 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 && (!object.subframe_poses.size()) && moveit::core::isEmpty(object.pose)) + if (num_shapes == 1 && object.subframe_poses.empty() && 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;