[hybrid planning] Adjust planning scene locking (moveit#1087)

* Create a copy of the planning scene. const robot state.

* Use LockedPlanningSceneRO over lockSceneRead()

* Use lambda function

moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp

@@ -235,14 +235,13 @@ void LocalPlannerComponent::executeIteration()
     // If the planner received an action request and a global solution it starts to plan locally
     case LocalPlannerState::LOCAL_PLANNING_ACTIVE:
     {
-      // Read current planning scene
       planning_scene_monitor_->updateSceneWithCurrentState();
-      planning_scene_monitor_->lockSceneRead();  // LOCK planning scene
-      planning_scene::PlanningScenePtr planning_scene = planning_scene_monitor_->getPlanningScene();
-      planning_scene_monitor_->unlockSceneRead();  // UNLOCK planning scene
 
-      // Get current state
-      auto current_robot_state = planning_scene->getCurrentStateNonConst();
+      // Read current robot state
+      const moveit::core::RobotState current_robot_state = [this] {
+        planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor_);
+        return ls->getCurrentState();
+      }();
 
       // Check if the global goal is reached
       if (trajectory_operator_instance_->getTrajectoryProgress(current_robot_state) > PROGRESS_THRESHOLD)