Fix Solution::fillMessage() #432
Conversation
There was a problem hiding this comment.
Need to generate full planning scene message if planning progressed backwards
That's a cheap (in terms of lines of code) solution indeed. 👍 It is quite costly with meshes in the scene, but it's definitely better than the current broken state. An improved implementation could still implement a generic "diff" for independent scene objects. Please consider creating a new issue for this as future work when merging this PR.
CI doesn't like it yet. Seems to be related to scene diff handling during execution:
[ INFO] [1677622610.740722501]: Found a contact between 'object' (type 'Object') and 'panda_leftfinger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1677622610.740764900]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1677622610.740791500]: Trajectory component '7/19' is invalid after scene update
This reverts commit 5382338.
rhaschke
force-pushed
the
fix-solution-message
branch
from
cae81ca to
eee9989
Compare
This reverts commit 5382338.
rhaschke
force-pushed
the
fix-solution-message
branch
from
eee9989 to
351a30c
Compare
This reverts commit 5382338.
This reverts commit 5382338.
rhaschke
force-pushed
the
fix-solution-message
branch
from
351a30c to
1246d25
Compare
Need to generate full planning scene message if planning progressed backwards: The scene_diff field describes PlanningScene changes of the end scene relative to the start scene, while during planning it might be inversed.
This reverts commit 5382338.
Joints are handled in trajectories. Scene diffs should not modify joints during execution. Fixes moveit#353. Alternative to moveit#504. The previous solution, to always clear the joint states during message generation, broke the visualization in rviz.
rhaschke
force-pushed
the
fix-solution-message
branch
from
1246d25 to
2450127
Compare
Codecov ReportAll modified and coverable lines are covered by tests ✅
Additional details and impacted files@@ Coverage Diff @@
## master #432 +/- ##
==========================================
+ Coverage 58.70% 58.83% +0.14%
==========================================
Files 91 91
Lines 8614 8617 +3
==========================================
+ Hits 5056 5069 +13
+ Misses 3558 3548 -10 ☔ View full report in Codecov by Sentry. |
The scene_diff field usually describes PlanningScene changes of the end scene relative to the start scene. For backwards planning, this direction is reversed: the start scene is derived from the end scene. Thus, we need to generate a full planning scene message for the end scene if planning progressed backwards. Fixes moveit#405.
This reverts commit 5382338.
Need to generate full planning scene message if planning progressed backwards: The scene_diff field describes PlanningScene changes of the end scene relative to the start scene, while during planning it might be inversed.
Seemingly fixes #405. I would like to add some unit tests before merging.
I also noticed that
ModifyPlanningScene::(add|remove)Objectdoesn't yet work in backwards mode. Shall we support this or just issue an error? To reverse,removeObject()would require the fullCollisionObjectdescription (to actually create the object), while currently (for the forward pass) the name suffices.