add Reject Consecutive Identical Request option (#153)

rapyuta-robotics · Dec 2, 2024 · 4785922 · 4785922
1 parent d6cb53a
commit 4785922
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 0 deletions.
diff --git 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
@@ -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
@@ -271,6 +271,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;