Fix pose-not-set-bug #2852
Fix pose-not-set-bug #2852
Conversation
…erify all pose fields are 0
|
Thanks for helping in improving MoveIt and open source robotics! |
There was a problem hiding this comment.
I think this check is problematic because if the pose is not set, all pose fields are zero.
Yes, but object_pose is obtained through PlanningScene::poseMsgToEigen(object.pose, object_pose); and this message prints a warning and assumes identity when the pose is empty. So that case works as expected.
even if the object pose is set to identity, the shape pose will be used instead
That is an actual bug in a (probably not uncommon) edge-case! Thank you for pointing it out and providing a patch.
I added some simplification.
v4hn
force-pushed
the
fix_pose_not_set_bug
branch
from
a9a768a
to
a465501
Compare
Codecov Report
@@ Coverage Diff @@
## master #2852 +/- ##
==========================================
+ Coverage 60.84% 60.87% +0.03%
==========================================
Files 366 366
Lines 31709 31710 +1
==========================================
+ Hits 19290 19299 +9
+ Misses 12419 12411 -8
Continue to review full report at Codecov.
|
| @@ -1718,9 +1715,11 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: | |||
|
|
|||
| bool switch_object_pose_and_shape_pose = false; | |||
| if (num_shapes == 1) | |||
| if (object_pose.isApprox(Eigen::Isometry3d::Identity())) | |||
| if (moveit::core::isEmpty(object.pose)) | |||
There was a problem hiding this comment.
I had just accepted that identity object poses will always be overwritten by shape poses, but it's true that you might want to e.g. shift your mesh without changing the object pose itself. Nice!
| object.object.pose.position.z == 0 && object.object.pose.orientation.x == 0 && | ||
| object.object.pose.orientation.y == 0 && object.object.pose.orientation.z == 0 && | ||
| object.object.pose.orientation.w == 0) | ||
| if (moveit::core::isEmpty(object.object.pose)) |
There was a problem hiding this comment.
This looks a lot prettier now.
|
Congrats on getting your first MoveIt pull request merged and improving open source robotics! |
Description
In #2816 , there was a fix for backwards compatibility for specifying a pose for a single collision shape. In that fix, I think there may be a bug. There is a check here to determine if an object's pose has been set:
if (object_pose.isApprox(Eigen::Isometry3d::Identity()))I think this check is problematic because if the pose is not set, all pose fields are zero. Checking if the pose is Identity assumes the unit quaternion which means that the "w" component is equal to 1. The way the code is currently, even if the object pose is set to identity, the shape pose will be used instead (which may not be aligned the same way as the object pose intended). This PR fixes this by explicitly checking that all fields of the object pose are zero.
Checklist