Return if requested pause state is already active.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>
moveit · Jan 30, 2025 · e05ad9a · e05ad9a
1 parent 870b23d
commit e05ad9a
Showing 1 changed file with 7 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,13 @@ 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)
+  {
+    RCLCPP_INFO(node_->get_logger(), "Requested pause state is already active.");
+    response->success = true;
+    response->message = "Nothing changed since requested pause state was already active.";
+    return;
+  }
   servo_paused_ = request->data;
   response->success = (servo_paused_ == request->data);
   if (servo_paused_)