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

pac48 · sjahr · web-flow · commit 50b4337688ee · 2025-02-15T02:18:24.000Z
Signed-off-by: Paul Gesel &lt;paul.gesel@picknik.ai&gt;
Co-authored-by: Sebastian Jahr &lt;sebastian.jahr@picknik.ai&gt;
diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/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_);

Original file line number	Diff line number	Diff line change
`@@ -155,6 +155,7 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request`
`155`	`155`	`response->message = message;`
`156`	`156`	`return;`
`157`	`157`	`}`
	`158`	`+ std::lock_guard<std::mutex> lock_guard(lock_);`
`158`	`159`	`servo_paused_ = request->data;`
`159`	`160`	`response->success = (servo_paused_ == request->data);`
`160`	`161`	`if (servo_paused_)`
`@@ -164,7 +165,6 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request`
`164`	`165`	`}`
`165`	`166`	`else`
`166`	`167`	`{`
`167`		`- std::lock_guard<std::mutex> lock_guard(lock_);`
`168`	`168`	`// Reset the smoothing plugin with the robot's current state in case the robot moved between pausing and unpausing.`
`169`	`169`	`last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */);`
`170`	`170`	`servo_->resetSmoothing(last_commanded_state_);`