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

ensure thread safety in clear octomap #2500

Merged
merged 2 commits into from Mar 3, 2021

Conversation

simonschmeisser
Copy link
Contributor

@simonschmeisser simonschmeisser commented Jan 26, 2021

Description

As commented in #2320 (comment) I recently encountered some crashes when clearOctomap is being called while some planners still run. Even though that is a weird thing to do, it still shouldn't crash.

Also #2320 uses UPDATE_SCENE which causes a full scene to be transmitted while UPDATE_GEOMETRY should be sufficient.

Checklist

@codecov
Copy link

codecov bot commented Jan 26, 2021

Codecov Report

Merging #2500 (8797348) into master (0e59113) will decrease coverage by 0.05%.
The diff coverage is 65.86%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master    #2500      +/-   ##
==========================================
- Coverage   60.26%   60.21%   -0.04%     
==========================================
  Files         351      351              
  Lines       26476    26485       +9     
==========================================
- Hits        15952    15945       -7     
- Misses      10524    10540      +16     
Impacted Files Coverage Δ
...ene/include/moveit/planning_scene/planning_scene.h 46.16% <ø> (ø)
...bot_model/include/moveit/robot_model/robot_model.h 84.22% <ø> (ø)
...bot_state/include/moveit/robot_state/robot_state.h 91.12% <ø> (ø)
...anner/src/trajectory_blender_transition_window.cpp 100.00% <ø> (ø)
...strial_motion_planner/src/trajectory_generator.cpp 100.00% <ø> (ø)
..._motion_planner_testutils/cartesianconfiguration.h 44.45% <ø> (ø)
...nning_scene_monitor/src/planning_scene_monitor.cpp 67.62% <0.00%> (-0.53%) ⬇️
...obot_interface/src/wrap_python_robot_interface.cpp 26.38% <25.00%> (-0.21%) ⬇️
.../move_group_interface/src/move_group_interface.cpp 48.38% <66.67%> (-0.05%) ⬇️
moveit_core/kinematic_constraints/src/utils.cpp 30.04% <100.00%> (-0.50%) ⬇️
... and 14 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 0e59113...4153df0. Read the comment docs.

@simonschmeisser
Copy link
Contributor Author

@felixvd friendly ping as you were the last one to touch this code

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. Awaits a second review.

@v4hn v4hn added the awaits 2nd review one maintainer approved this request label Mar 2, 2021
Copy link
Contributor

@felixvd felixvd left a comment

Choose a reason for hiding this comment

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

My bad or introducing the bug. LGTM. Thanks for the fix of UPDATE_GEOMETRY, too.

Will merge after you decide about the proposed comment. I'm good either way.

else
{
ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized");
boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
Copy link
Contributor

Choose a reason for hiding this comment

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

It wasn't hard to figure out how this works, but I had to think for a second because I don't use mutexes a lot. Do you want to add a comment to the effect of // Lock scene using mutex before removing the octomap. Lock expires after the code block for the less familiar? I'm on the fence.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I should probably add a comment about the extra brackets that are there so that the scoped lock gets lifted before notifying consumers (triggerSceneUpdateEvent) in order to avoid a deadlock ...

Copy link
Contributor

@felixvd felixvd Mar 3, 2021

Choose a reason for hiding this comment

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

I didn't know that the lock needs to be lifted before that call. How about adding // Lift the scoped lock before calling triggerSceneUpdateEvent to avoid deadlock at the end of the block?

I'm fine with not adding the other comment because I suppose it's not "nonobvious to a reader who understands C++ well".

Co-authored-by: Felix von Drigalski <FvDrigalski@gmail.com>
@v4hn v4hn merged commit 30a170f into moveit:master Mar 3, 2021
@v4hn
Copy link
Contributor

v4hn commented Mar 3, 2021

Thanks @simonschmeisser . Congratulations on getting pull request number #2500. 🤡
We sure deal with a lot of patches as time passes...

@felixvd
Copy link
Contributor

felixvd commented Mar 3, 2021

I hope we don't have to wait until #12345 to merge this

@simonschmeisser
Copy link
Contributor Author

It took me a long time to actually find something to fix just so I could get this PR number ... 😉

Thanks for reviewing and merging ... I suspect there's going to be another one as we had crashes in the octree->clear part as well that should be locked by themselves

@tylerjw tylerjw mentioned this pull request Apr 9, 2021
tylerjw pushed a commit to tylerjw/moveit that referenced this pull request Apr 29, 2021
The method used UPDATE_SCENE which caused a full scene to be transmitted.
But only the octomap is modified, so UPDATE_GEOMETRY suffices.

Also add comment about necessary scoping of non-recursive lock.
@tylerjw tylerjw mentioned this pull request Apr 29, 2021
tylerjw pushed a commit to tylerjw/moveit that referenced this pull request May 3, 2021
The method used UPDATE_SCENE which caused a full scene to be transmitted.
But only the octomap is modified, so UPDATE_GEOMETRY suffices.

Also add comment about necessary scoping of non-recursive lock.
tylerjw pushed a commit that referenced this pull request May 3, 2021
The method used UPDATE_SCENE which caused a full scene to be transmitted.
But only the octomap is modified, so UPDATE_GEOMETRY suffices.

Also add comment about necessary scoping of non-recursive lock.
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

3 participants