Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

prevent collision checking segfault if octomap has NULL root pointer #2104

Merged
merged 5 commits into from
May 26, 2020

Conversation

JStech
Copy link
Contributor

@JStech JStech commented May 23, 2020

Description

After clearing the octomap, the root node is set to NULL, which then causes FCL to segfault in collision checking. Maybe it's actually an FCL bug, but this was the most straightforward way I found to work around it.

Checklist

  • Required by CI: Code is auto formatted using clang-format
  • Extend the tutorials / documentation reference
  • Document API changes relevant to the user in the MIGRATION.md notes
  • Create tests, which fail without this PR reference
  • Include a screenshot if changing a GUI
  • While waiting for someone to review your request, please help review another open pull request to support the maintainers

Copy link
Contributor

@v4hn v4hn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

  • Please use static_piointer_cast, the type is ensured by getObjectType.
  • Why would you return any distance here? I guess you could just check and return false; as in the other preliminary tests above.
  • use nullptr or boolean check, not NULL
  • CI fails because your patch is incompatible with the old version of FCL. Please have a look at our compat header

I'm fine with catching this issue here, but please also file an issue in the fcl project and reference this request. Either we should never clear the root or they should fix it upstream.

@JStech
Copy link
Contributor Author

JStech commented May 23, 2020

Thanks for the guidance. I think I've made the modifications you described.

@codecov-commenter
Copy link

codecov-commenter commented May 23, 2020

Codecov Report

Merging #2104 into master will increase coverage by 3.28%.
The diff coverage is 75.00%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master    #2104      +/-   ##
==========================================
+ Coverage   54.49%   57.77%   +3.28%     
==========================================
  Files         328      328              
  Lines       25661    25664       +3     
==========================================
+ Hits        13983    14827     +844     
+ Misses      11678    10837     -841     
Impacted Files Coverage Δ
...e/collision_detection_fcl/src/collision_common.cpp 79.33% <75.00%> (+1.17%) ⬆️
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp 72.97% <0.00%> (-0.67%) ⬇️
...pl/ompl_interface/src/planning_context_manager.cpp 70.94% <0.00%> (-0.17%) ⬇️
.../distance_field/src/propagation_distance_field.cpp 94.46% <0.00%> (-0.09%) ⬇️
...kinematic_constraints/src/kinematic_constraint.cpp 72.69% <0.00%> (-0.05%) ⬇️
moveit_core/distance_field/src/distance_field.cpp 26.02% <0.00%> (ø)
...veit_core/robot_model/src/floating_joint_model.cpp 48.35% <0.00%> (ø)
...moveit/distance_field/propagation_distance_field.h 85.71% <0.00%> (ø)
...nning_scene_monitor/src/planning_scene_monitor.cpp 70.70% <0.00%> (+0.19%) ⬆️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp 79.37% <0.00%> (+0.44%) ⬆️
... and 35 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 442c0fe...73eedce. Read the comment docs.

if ((o1->getObjectType() == fcl::OT_OCTREE &&
!std::static_pointer_cast<const fcl::OcTreed>(o1->collisionGeometry())->getRoot()) ||
(o1->getObjectType() == fcl::OT_OCTREE &&
!std::static_pointer_cast<const fcl::OcTreed>(o1->collisionGeometry())->getRoot()))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess the second check should be about o2, right?
Also, please add a comment explaining that fcl::distance misbehaves for this in the version you use.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah yes, I copy-pasted that and failed to change o1 to o2. Done.

Copy link
Contributor

@v4hn v4hn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

looks good to me. Thank you for creating the FCL issue too. 👍

@v4hn v4hn added the awaits 2nd review one maintainer approved this request label May 24, 2020
@rhaschke rhaschke merged commit 7aa1699 into moveit:master May 26, 2020
JStech pushed a commit to JStech/moveit that referenced this pull request Jun 2, 2020
@JStech JStech mentioned this pull request Jun 2, 2020
6 tasks
pradeepr-roboticist pushed a commit to pradeepr-roboticist/moveit that referenced this pull request Jun 3, 2020
v4hn pushed a commit to v4hn/moveit that referenced this pull request Jul 3, 2020
@tylerjw tylerjw mentioned this pull request Jul 18, 2020
20 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
awaits 2nd review one maintainer approved this request
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

5 participants