Update current state even if servo is paused (#3341)

Signed-off-by: Paul Gesel <[email protected]>
Co-authored-by: Sebastian Jahr <[email protected]>

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -155,6 +155,7 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request
     response->message = message;
     return;
   }
+  std::lock_guard<std::mutex> lock_guard(lock_);
   servo_paused_ = request->data;
   response->success = (servo_paused_ == request->data);
   if (servo_paused_)
@@ -164,7 +165,6 @@ 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_);