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

…t state. (#3265) * Return if requested pause state is already active. Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com> * Reuse logger message with a shared string variable. Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com> --------- Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>
moveit · Jan 30, 2025 · 0af5e0f · 0af5e0f
1 parent 870b23d
commit 0af5e0f
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_)