Servo Node - pause service: check if request is different than curren…

…t state. (#3265) (#3348) Signed-off-by: Jelmer de Wolde <[email protected]> (cherry picked from commit 0af5e0f) Co-authored-by: Jelmer de Wolde <[email protected]>
moveit · Feb 15, 2025 · 6da4b25 · 6da4b25
1 parent 7e1fc53
commit 6da4b25
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp
@@ -147,6 +147,14 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options)
 void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                            const std::shared_ptr<std_srvs::srv::SetBool::Response>& response)
 {
+  if (servo_paused_ == request->data)
+  {
+    std::string message = "Requested pause state is already active.";
+    RCLCPP_INFO(node_->get_logger(), "%s", message.c_str());
+    response->success = true;
+    response->message = message;
+    return;
+  }
   servo_paused_ = request->data;
   response->success = (servo_paused_ == request->data);
   if (servo_paused_)