[moveit_servo] avoid race condition when calling ~/pause_servo

moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp

@@ -132,6 +132,7 @@ class ServoNode
 
   // Threads used by ServoNode
   std::thread servo_loop_thread_;
+  std::mutex lock_;
 
   // rolling window of joint commands
   std::deque<KinematicState> joint_cmd_rolling_window_;

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -151,6 +151,7 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request
   }
   else
   {
+    std::lock_guard<std::mutex> lock_guard(lock_);
     // Reset the smoothing plugin with the robot's current state in case the robot moved between pausing and unpausing.
     last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */);
     servo_->resetSmoothing(last_commanded_state_);
@@ -319,7 +320,7 @@ void ServoNode::servoLoop()
       servo_frequency.sleep();
       continue;
     }
-
+    std::lock_guard<std::mutex> lock_guard(lock_);
     const bool use_trajectory = servo_params_.command_out_type == "trajectory_msgs/JointTrajectory";
     const auto cur_time = node_->now();