Revert locked planning scene change

moveit · Nov 3, 2024 · f7c95c4 · f7c95c4
1 parent 8ef27b9
commit f7c95c4
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp
@@ -98,9 +98,6 @@ void CollisionMonitor::checkCollisions()
   double self_collision_scale, scene_collision_scale;
   const double log_val = -log(0.001);
 
-  // Get a read-only copy of the planning scene.
-  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
-
   while (rclcpp::ok() && !stop_requested_)
   {
     const double self_velocity_scale_coefficient{ log_val / servo_params_.self_collision_proximity_threshold };
@@ -114,6 +111,9 @@ void CollisionMonitor::checkCollisions()
       // This must be called before doing collision checking.
       robot_state_.updateCollisionBodyTransforms();
 
+      // Get a read-only copy of the planning scene.
+      planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
+
       // Check collision with environment.
       scene_collision_result_.clear();
       locked_scene->getCollisionEnv()->checkRobotCollision(scene_collision_request_, scene_collision_result_,