From bc81091cce3c4f9d5845f822a7979c8ad40a92d9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 28 May 2023 12:24:51 +0200 Subject: [PATCH] Silent "empty quaternion" warning from poseMsgToEigen() (#3435) --- .../planning_scene/src/planning_scene.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 72f89a6802..8eee7b1ec2 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1755,15 +1755,15 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: shapes.reserve(num_shapes); shape_poses.reserve(num_shapes); - PlanningScene::poseMsgToEigen(object.pose, object_pose); - bool switch_object_pose_and_shape_pose = false; - if (num_shapes == 1) - if (moveit::core::isEmpty(object.pose)) - { - switch_object_pose_and_shape_pose = true; // If the object pose is not set but the shape pose is, - // use the shape's pose as the object pose. - } + if (num_shapes == 1 && 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; + object_pose.setIdentity(); + } + else + PlanningScene::poseMsgToEigen(object.pose, object_pose); auto append = [&object_pose, &shapes, &shape_poses, &switch_object_pose_and_shape_pose](shapes::Shape* s, const geometry_msgs::Pose& pose_msg) {