[moveit_servo] avoid race condition when calling ~/pause_servo (#3059)

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp

@@ -133,6 +133,9 @@ class ServoNode
   // Threads used by ServoNode
   std::thread servo_loop_thread_;
 
+  // Locks for threads safety
+  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_);
@@ -320,6 +321,7 @@ void ServoNode::servoLoop()
       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();