diff --git a/Source/rclUE/Private/ROS2ServiceServer.cpp b/Source/rclUE/Private/ROS2ServiceServer.cpp index 1208dbe9..cf93d7e1 100644 --- a/Source/rclUE/Private/ROS2ServiceServer.cpp +++ b/Source/rclUE/Private/ROS2ServiceServer.cpp @@ -53,6 +53,13 @@ void UROS2ServiceServer::ProcessReady() void* data = Service->GetRequest(); RCSOFTCHECK(rcl_take_request_with_info(&rcl_service, &req_info, data)); + if (bRejectConsecutiveIdenticalRequest && UROS2Utils::IsEqualWMSrvInfo(req_info, LastReqInfo, true)) + { + UE_LOG_WITH_INFO(LogROS2Srv, Warning, TEXT("Rejecting consecutive identical request")); + return; + } + LastReqInfo = req_info; + UE_LOG_WITH_INFO_NAMED(LogROS2Node, Log, TEXT("ROS2Node Executing Service server callback")); SrvCallback.ExecuteIfBound(Service); diff --git a/Source/rclUE/Public/ROS2ServiceServer.h b/Source/rclUE/Public/ROS2ServiceServer.h index b7308773..ad7cd7fe 100644 --- a/Source/rclUE/Public/ROS2ServiceServer.h +++ b/Source/rclUE/Public/ROS2ServiceServer.h @@ -82,6 +82,12 @@ class RCLUE_API UROS2ServiceServer : public UROS2Service * */ virtual void InitializeServiceComponent() override; + + rmw_service_info_t LastReqInfo; + + //! Rejct consecutive identical requests + UPROPERTY(EditAnywhere, BlueprintReadWrite) + bool bRejectConsecutiveIdenticalRequest = true; }; /** diff --git a/Source/rclUE/Public/rclcUtilities.h b/Source/rclUE/Public/rclcUtilities.h index 6293b522..4d4ab80a 100644 --- a/Source/rclUE/Public/rclcUtilities.h +++ b/Source/rclUE/Public/rclcUtilities.h @@ -272,6 +272,39 @@ class UROS2Utils : public UBlueprintFunctionLibrary GENERATED_BODY() public: + static bool IsEqualRWMRequestId(rmw_request_id_t service1, rmw_request_id_t service2) + { + if (service1.sequence_number != service2.sequence_number) + { + return false; + } + for (int i = 0; i < 16; i++) + { + if (service1.writer_guid[i] != service2.writer_guid[i]) + { + return false; + } + } + return true; + } + + static bool IsEqualWMSrvInfo(rmw_service_info_t service1, rmw_service_info_t service2, bool check_received_timestamp = true) + { + if (!UROS2Utils::IsEqualRWMRequestId(service1.request_id, service2.request_id)) + { + return false; + } + if (service1.source_timestamp != service2.source_timestamp) + { + return false; + } + if (check_received_timestamp && service1.received_timestamp != service2.received_timestamp) + { + return false; + } + return true; + } + static builtin_interfaces__msg__Time FloatToROSStamp(const float InTimeSec) { builtin_interfaces__msg__Time stamp;