Fix bullet constructor not updating world objects #2830

yurirocha15 · 2021-08-20T11:21:33Z

Description

Bullet Collision Env's "copy constructor" does not update the manager with the copied world objects (by calling the notify function). This leads to an empty collision environment when planning a trajectory.

This is the constructor called in https://github.com/ros-planning/moveit/blob/master/moveit_core/planning_scene/src/planning_scene.cpp#L393-L397

which is used by the planning_scene_monitor to keep the move_group scene updated.

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

codecov · 2021-08-20T13:29:47Z

Codecov Report

Merging #2830 (d45a675) into master (e1f6c6a) will increase coverage by 0.07%.
The diff coverage is 100.00%.

❗ Current head d45a675 differs from pull request most recent head 40adcf5. Consider uploading reports for the commit 40adcf5 to get more accurate results

@@            Coverage Diff             @@
##           master    #2830      +/-   ##
==========================================
+ Coverage   60.88%   60.94%   +0.07%     
==========================================
  Files         368      373       +5     
  Lines       31715    31725      +10     
==========================================
+ Hits        19306    19332      +26     
+ Misses      12409    12393      -16

Impacted Files	Coverage Δ
...sion_detection_bullet/src/collision_env_bullet.cpp	`85.12% <100.00%> (+5.48%)`	⬆️
.../ompl_interface/src/detail/constrained_sampler.cpp	`43.25% <0.00%> (-16.21%)`	⬇️
...meterization/work_space/pose_model_state_space.cpp	`80.51% <0.00%> (-2.51%)`	⬇️
...ipulation/pick_place/src/manipulation_pipeline.cpp	`69.45% <0.00%> (-1.85%)`	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	`80.42% <0.00%> (-0.29%)`	⬇️
...lude/moveit/collision_detection/collision_plugin.h	`100.00% <0.00%> (ø)`
...et/src/collision_detector_bullet_plugin_loader.cpp	`100.00% <0.00%> (ø)`
...tection_fcl/collision_detector_fcl_plugin_loader.h	`100.00% <0.00%> (ø)`
...n_bullet/collision_detector_bullet_plugin_loader.h	`100.00% <0.00%> (ø)`
...n_fcl/src/collision_detector_fcl_plugin_loader.cpp	`100.00% <0.00%> (ø)`
... and 4 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 e1f6c6a...40adcf5. Read the comment docs.

moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp

yurirocha15 · 2021-08-21T14:45:49Z

@v4hn I added the tests in the planning_scene folder as I believe they are related to the child scene collision environment not being correctly updated (when using Bullet). I created the same test for FCL as well. I verified that the test fails before this fix and passes after it.

As I had to add the bullet dependency to the tests, I also replicated the FCL multithreaded test (as #2598 made Bullet thread-safe).

yurirocha15 · 2021-08-22T14:31:18Z

RoboStack's Windows build cannot find bullet:

-- Checking for module 'bullet'
--   No package 'bullet' found
-- Could NOT find Bullet: Found unsuitable version "", but required is at least "2.87" (found )
-- Version of Bullet too old or not available: disabling Bullet collision detection plugin. Try using Ubuntu 18.04 or later.

Because of this BULLET_INC is not being set which leads to a build error:

fatal error C1083: Cannot open include file: 'moveit/collision_detection_bullet/collision_detector_allocator_bullet.h': No such file or directory

Any suggestions on how to proceed from here?

rhaschke

The Bullet code should be disregarded if Bullet was not found.

rhaschke · 2021-08-22T15:18:41Z

moveit_core/planning_scene/CMakeLists.txt

@@ -14,6 +14,7 @@ target_link_libraries(${MOVEIT_LIB_NAME}
  moveit_exceptions
  moveit_transforms
  moveit_collision_detection_fcl
+  moveit_collision_detection_bullet


This lib shouldn't be added explicitly here as it is a plugin!

If I remove it, the tests fail to compile as they cannot find the collision_detection::CollisionDetectorAllocatorBullet definition. I thought it was ok to link bullet here as there is a link for fcl already, but I guess that this plugin wouldn't exist if Bullet was not available.

I changed it (c7df778) to a conditional based on whether -DBULLET_ENABLE was defined or not.

if(BULLET_ENABLE) set(BULLET_LIB moveit_collision_detection_bullet) endif() target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_robot_state moveit_exceptions moveit_transforms moveit_collision_detection_fcl ${BULLET_LIB} moveit_kinematic_constraints ...

Ideally, you would load the plugin via the plugin mechanism instead of instantiating it directly ;-)

rhaschke · 2021-08-22T15:25:28Z

moveit_core/planning_scene/test/test_multi_threaded.cpp

+#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
+#include <moveit/collision_detection_bullet/collision_env_bullet.h>


These includes are not available when Bullet is not available. You need to conditionally include them!
My suggestion is to add add_definitions(-DBULLET_ENABLE) here and in c++ code use:

#ifdef BULLET_ENABLE ... #endif

Thank you for the suggestion! Added in 2433753

rhaschke · 2021-08-23T09:41:18Z

I will have a look into the remaining CI failure. Seems to be related to clang being used as the compiler...

yurirocha15 · 2021-08-23T10:45:47Z

I will have a look into the remaining CI failure. Seems to be related to clang being used as the compiler...

I ran industrial_ci locally and was able to fix it. I believe it was related to the compiler resetting the class_loader ptr first and then resetting the planning_scene ptrs (maybe trying to access the deleted BulletPlugin at destruction?). After explicitly setting the destruction order the segmentation fault disappeared (at least when running locally), Hopefully, it will pass now.

rhaschke

To avoid code duplication in tests, I provided https://github.com/makinarocks/moveit/pull/2 with more simplifications. However, this relies on #2834.
Otherwise, I approve.

yurirocha15 · 2021-08-31T15:41:49Z

@rhaschke as #2834 was merged, I cherry-picked the commits from https://github.com/makinarocks/moveit/pull/2 and rebased the branch on top of master.

rhaschke · 2021-09-01T07:29:51Z

I pushed another commit to https://github.com/makinarocks/moveit/pull/2 to fix the gtest compatibility issues: https://github.com/makinarocks/moveit/pull/2/commits/893e63f2a1b1769343532d353f5c5b84c1de742a

yurirocha15 requested review from rhaschke and v4hn as code owners August 20, 2021 11:21

v4hn reviewed Aug 20, 2021

View reviewed changes

moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp Show resolved Hide resolved

moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp Show resolved Hide resolved

yurirocha15 requested a review from felixvd as a code owner August 21, 2021 14:40

rhaschke reviewed Aug 22, 2021

View reviewed changes

rhaschke mentioned this pull request Aug 23, 2021

Split CollisionPluginLoader #2834

Merged

rhaschke requested changes Aug 23, 2021

View reviewed changes

yurirocha15 and others added 13 commits August 31, 2021 21:46

notify object changes

628160d

add unit test

e0598af

replicate multithreaded test

a6196c3

remove TODO

5ecd518

only run bullet tests if bullet is enabled

19301a2

conditionally link bullet plugin

1d9cb48

load bullet using pluginlib

d16cb8a

destroy class loaders last

2441363

Use CollisionPluginLoader

9b5c576

Use parameterized tests

204fec2

fixup! Use CollisionPluginLoader

92c1245

Add SCOPED_TRACE to write plugin_name on any error

02e85a1

update class name

d45a675

backwards compatibility to older gtest

40adcf5

rhaschke approved these changes Sep 1, 2021

View reviewed changes

rhaschke merged commit e808fb0 into moveit:master Sep 1, 2021

yurirocha15 deleted the bugfix/fix-bullet-constructor branch September 1, 2021 10:10

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix bullet constructor not updating world objects #2830

Fix bullet constructor not updating world objects #2830

yurirocha15 commented Aug 20, 2021 •

edited

codecov bot commented Aug 20, 2021 •

edited

yurirocha15 commented Aug 21, 2021 •

edited

yurirocha15 commented Aug 22, 2021 •

edited

rhaschke left a comment

rhaschke Aug 22, 2021

yurirocha15 Aug 22, 2021 •

edited

yurirocha15 Aug 22, 2021 •

edited

rhaschke Aug 22, 2021

yurirocha15 Aug 23, 2021 •

edited

rhaschke Aug 22, 2021

yurirocha15 Aug 22, 2021

rhaschke commented Aug 23, 2021

yurirocha15 commented Aug 23, 2021 •

edited

rhaschke left a comment •

edited

yurirocha15 commented Aug 31, 2021

rhaschke commented Sep 1, 2021

		#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
		#include <moveit/collision_detection_bullet/collision_env_bullet.h>

Fix bullet constructor not updating world objects #2830

Fix bullet constructor not updating world objects #2830

Conversation

yurirocha15 commented Aug 20, 2021 • edited

Description

Checklist

codecov bot commented Aug 20, 2021 • edited

Codecov Report

yurirocha15 commented Aug 21, 2021 • edited

yurirocha15 commented Aug 22, 2021 • edited

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Aug 22, 2021

Choose a reason for hiding this comment

yurirocha15 Aug 22, 2021 • edited

Choose a reason for hiding this comment

yurirocha15 Aug 22, 2021 • edited

Choose a reason for hiding this comment

rhaschke Aug 22, 2021

Choose a reason for hiding this comment

yurirocha15 Aug 23, 2021 • edited

Choose a reason for hiding this comment

rhaschke Aug 22, 2021

Choose a reason for hiding this comment

yurirocha15 Aug 22, 2021

Choose a reason for hiding this comment

rhaschke commented Aug 23, 2021

yurirocha15 commented Aug 23, 2021 • edited

rhaschke left a comment • edited

Choose a reason for hiding this comment

yurirocha15 commented Aug 31, 2021

rhaschke commented Sep 1, 2021

yurirocha15 commented Aug 20, 2021 •

edited

codecov bot commented Aug 20, 2021 •

edited

yurirocha15 commented Aug 21, 2021 •

edited

yurirocha15 commented Aug 22, 2021 •

edited

yurirocha15 Aug 22, 2021 •

edited

yurirocha15 Aug 22, 2021 •

edited

yurirocha15 Aug 23, 2021 •

edited

yurirocha15 commented Aug 23, 2021 •

edited

rhaschke left a comment •

edited