Fix PlanningScene CollisionDetector diff handling #464
Conversation
| if (have_previous_coll_detector) | ||
| // If parent_detector is specified, copy-construct collision environments (copies link shapes and attached objects) | ||
| // Otherwise, construct new collision environment from world and robot model | ||
| if (parent_detector) |
There was a problem hiding this comment.
This is the key part of this fix. The collision environment of the parent is used to copy-construct the child environment so that attached objects are available as well.
| collision_detector_->copyPadding(*parent_->collision_detector_); | ||
| } | ||
| collision_detector_->cenv_const_ = collision_detector_->cenv_; | ||
| // Reset collision detector to the the parent's version |
There was a problem hiding this comment.
clearDiffs() now also just allocates a fresh collision detector from parent
Codecov Report
@@ Coverage Diff @@
## main #464 +/- ##
==========================================
+ Coverage 51.41% 51.42% +0.01%
==========================================
Files 223 223
Lines 23343 23343
==========================================
+ Hits 12000 12001 +1
+ Misses 11343 11342 -1
Continue to review full report at Codecov.
|
moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
Show resolved
Hide resolved
|
Tested with the move_group_interface tutorial. Looks good to me! |
henningkayser
force-pushed
the
henning/fix-scene-diffs
branch
from
d64fecd
to
3b6e0d1
Compare
|
@henningkayser I already test this with octomap . It can fixed #444 |
This fixes how PlanningScene diffs are created and managed (replaces #463 and #456).
#364 introduced a bug where a child scene would not copy-construct the collision environments so that it wouldn't include any of the parent's link geometries or attached objects. The link geometries aren't that bad because they would be constructed from the robot model anyway, but there was no way to sync back the attached objects. This PR fixes that by providing a private allocate function that allows passing the parent detector which implements the old behavior. I tested this and it fixes #429 and #407 for me. I didn't test #444 yet.
However, there is still a bug (or irritation) where the start state of the MotionPlanningPanel doesn't include the attached object right away after publishing the collision objects. The reason is that the internal start state of the panel is not updated automatically which is also exactly the case for MoveIt 1. The requested plan then solves a trajectory which is then being rejected right away because it's in collision. To make the plan succeed, the start state needs to be updated either by selecting "<current>" again or by changing it to a different state. I actually don't think that this needs fixing and it's definitely out of scope of this PR.