Fix bug in applying planning scene diffs that have attached collision objects#3124
Fix bug in applying planning scene diffs that have attached collision objects#3124AndyZe merged 7 commits intomoveit:masterfrom
Conversation
Codecov Report
@@ Coverage Diff @@
## master #3124 +/- ##
==========================================
+ Coverage 61.55% 61.60% +0.05%
==========================================
Files 376 376
Lines 33308 33314 +6
==========================================
+ Hits 20501 20521 +20
+ Misses 12807 12793 -14
Continue to review full report at Codecov.
|
AndyZe
left a comment
There was a problem hiding this comment.
I've tested this now with the Panda tutorial and clearing collision scene objects. It seems to work great! But, I still think you need to check if parent_ is a nullptr.
tahsinkose
left a comment
There was a problem hiding this comment.
Hey there. Wanted to give a little review, which is just a nit 🙂
…ld cause them to override the aco
Co-authored-by: AndyZe <andyz@utexas.edu>
Co-authored-by: AndyZe <andyz@utexas.edu>
Co-authored-by: AndyZe <andyz@utexas.edu>
042d61e to
fed689b
Compare
| scene_msg.robot_state = moveit_msgs::RobotState(); | ||
| scene_msg.robot_state.is_diff = true; | ||
| } | ||
| scene_msg.robot_state.is_diff = true; |
There was a problem hiding this comment.
I think, this is correct. However, @v4hn explicitly had a different opinion, when he introduced this code line: #939 (comment)
| } | ||
|
|
||
| // Returns a planning scene diff message | ||
| moveit_msgs::PlanningScene create_planning_scene_diff(planning_scene::PlanningScene& ps, const std::string& object_name, |
There was a problem hiding this comment.
Here PlanningScene should be a const reference, shouldn't it?
| getOctomapMsg(scene_msg.world.octomap); | ||
| } | ||
|
|
||
| // Ensure any detached collision objects get detached from the parent planning scene, too |
There was a problem hiding this comment.
This comment is misleading: The parent planning scene is not (and should not get) modified.
There was a problem hiding this comment.
Agreed, I had a to-do to update the comment, but it got resolved somehow
…nts to the original PR (#3142)
… objects (moveit#3124) (moveit#1251) * Add test that trigger the bug in applying scene that have robot state diffs * Fix a bug in planning scene diffs where having applying two diffs would cause them to override the aco * Fix a bug where removing an attached collision object would keep it in the scene * Update moveit_core/planning_scene/test/test_planning_scene.cpp Co-authored-by: AndyZe <andyz@utexas.edu> * Update moveit_core/planning_scene/test/test_planning_scene.cpp Co-authored-by: AndyZe <andyz@utexas.edu> * Update moveit_core/planning_scene/src/planning_scene.cpp Co-authored-by: AndyZe <andyz@utexas.edu> * Check parent pointer Co-authored-by: AndyZe <andyz@utexas.edu> Co-authored-by: AndyZe <andyz@utexas.edu>
Description
This fixes a bug we had while executing an MTC task where having two different planning scenes diffs that add a collision object to the scene, the first commit is just a test that trigger the bug, the second commit sets
robot_state.is_diffto true when callinggetPlanningSceneDiffMsgwhich prevent conversions.cpp#L369-L372 from deleting the aco from the robot state when applying the diffs, but now when calling PlanningScene::setCurrentState while detaching an object that object is staying in the scene so the last commit fixes it by explicitly removing the aco in the case of scene diffThis work is sponsored by RE2 Robotics