thread safety in clear octomap & only update geometry (moveit#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.

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<boost::shared_mutex> 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)