From ac0ac4be0d99e30202041ff5b9962a6de35d3658 Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Wed, 3 Mar 2021 11:43:12 +0100 Subject: [PATCH] thread safety in clear octomap & only update geometry (#2500) 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. --- .../src/planning_scene_monitor.cpp | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 2c7c1167804..8bbcdda8efe 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -537,18 +537,25 @@ void PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningS void PlanningSceneMonitor::clearOctomap() { - if (scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS)) - triggerSceneUpdateEvent(UPDATE_SCENE); - if (octomap_monitor_) - { - octomap_monitor_->getOcTreePtr()->lockWrite(); - octomap_monitor_->getOcTreePtr()->clear(); - octomap_monitor_->getOcTreePtr()->unlockWrite(); - } - else + bool removed = false; { - ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized"); + boost::unique_lock ulock(scene_update_mutex_); + removed = scene_->getWorldNonConst()->removeObject(scene_->OCTOMAP_NS); + + if (octomap_monitor_) + { + octomap_monitor_->getOcTreePtr()->lockWrite(); + octomap_monitor_->getOcTreePtr()->clear(); + octomap_monitor_->getOcTreePtr()->unlockWrite(); + } + else + { + ROS_WARN_NAMED(LOGNAME, "Unable to clear octomap since no octomap monitor has been initialized"); + } // Lift the scoped lock before calling triggerSceneUpdateEvent to avoid deadlock } + + if (removed) + triggerSceneUpdateEvent(UPDATE_GEOMETRY); } bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)