diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 05fd6f4d64..219fc75490 100644 --- 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& request, const std::shared_ptr& 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_)