Remove out-commented code and implement setting the GUID (ros2#76)

Signed-off-by: Simon Hoinkis <[email protected]>
ApexAI · Jan 11, 2023 · bb005ce · bb005ce
1 parent 8b67b38
commit bb005ce
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 20 deletions.
diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp
@@ -170,8 +170,22 @@ rmw_take_request(
   }
   request_header->source_timestamp = 0;  // Unsupported until needed
   ret = rcutils_system_time_now(&request_header->received_timestamp);
+  if (ret != RMW_RET_OK) {
+    return ret;
+  }
   request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId();
-  request_header->request_id.writer_guid[0] = 42;  /// @todo
+
+  auto typed_guid = iceoryx_server->getUid();
+  iox::popo::UniquePortId::value_type guid =
+    static_cast<iox::popo::UniquePortId::value_type>(typed_guid);
+  size_t size = sizeof(guid);
+  auto max_rmw_storage = sizeof(request_header->request_id.writer_guid);
+  if (!typed_guid.isValid() || size > max_rmw_storage) {
+    RMW_SET_ERROR_MSG("Could not write server guid");
+    ret = RMW_RET_ERROR;
+    return ret;
+  }
+  memcpy(request_header->request_id.writer_guid, &guid, size);
 
   // Hold the loaned request till we send the response in 'rmw_send_response'
   iceoryx_server_abstraction->request_payload_ = user_payload;

diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp
@@ -66,16 +66,30 @@ rmw_take_response(
     [&](const void * iceoryx_response_payload) {
       auto iceoryx_response_header = iox::popo::ResponseHeader::fromPayload(
         iceoryx_response_payload);
-      /// @todo check writer guid?
-      request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId();
-      request_header->source_timestamp = 0;  // Unsupported until needed
-      ret = rcutils_system_time_now(&request_header->received_timestamp);
 
       if (iceoryx_response_header->getSequenceId() ==
       iceoryx_client_abstraction->sequence_id_ - 1)
       {
         user_payload = iceoryx_response_payload;
         chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload);
+
+        auto typed_guid = chunk_header->originId();
+        iox::popo::UniquePortId::value_type guid =
+          static_cast<iox::popo::UniquePortId::value_type>(typed_guid);
+        size_t size = sizeof(guid);
+        auto max_rmw_storage = sizeof(request_header->request_id.writer_guid);
+        if (!typed_guid.isValid() || size > max_rmw_storage) {
+          RMW_SET_ERROR_MSG("Could not write server guid");
+          ret = RMW_RET_ERROR;
+          return;
+        }
+        memcpy(request_header->request_id.writer_guid, &guid, size);
+        request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId();
+        request_header->source_timestamp = 0;  // Unsupported until needed
+        ret = rcutils_system_time_now(&request_header->received_timestamp);
+        if (ret != RMW_RET_OK) {
+          return;
+        }
         ret = RMW_RET_OK;
       } else {
         RMW_SET_ERROR_MSG("Got response with outdated sequence number!");
@@ -145,10 +159,6 @@ rmw_send_response(
 
   auto * iceoryx_request_header = iox::popo::RequestHeader::fromPayload(
     iceoryx_server_abstraction->request_payload_);
-  /// @todo Why is it not possible to set the sequence id? Is this automatically done? If so,
-  /// we need to compare the user-provided sequence id with the one from the
-  /// 'iceoryx_request_header'
-  // iceoryx_request_header->setSequenceId(request_header->sequence_number);
 
   iceoryx_server->loan(
     iceoryx_request_header, iceoryx_server_abstraction->response_size_,

diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp
@@ -20,8 +20,6 @@
 
 #include "./types/iceoryx_client.hpp"
 
-// #include "iceoryx_posh/runtime/service_discovery.hpp"
-
 extern "C"
 {
 rmw_ret_t
@@ -56,15 +54,6 @@ rmw_service_server_is_available(
     *is_available = true;
   }
 
-  /// @todo Better to go through service discovery?
-  // iox::runtime::ServiceDiscovery serviceDiscovery;
-  // auto& searchItem = iceoryx_client->getServiceDescription();
-  // serviceDiscovery.findService( searchItem.getServiceIDString(),
-  //                               searchItem.getInstanceIDString(),
-  //                               searchItem.getEventIDString(),
-  //                               [&](auto&){ *is_available = true; },
-  //                               iox::popo::MessagingPattern::REQ_RES);
-
   ret = RMW_RET_OK;
   return ret;
 }