Add new methods and minor changes for ROS 2 Iron Irwini (#101)

mossmaurice-apexai · web-flow · commit beef9db84283 · 2023-10-17T14:22:26.000+02:00
* Address review findings (#84) Signed-off-by: Simon Hoinkis <simon.hoinkis@apex.ai> * Implement new rmw functions needed for iron (#99) Signed-off-by: Simon Hoinkis <simon.hoinkis@apex.ai> * Change CI to iron (#99) Signed-off-by: Simon Hoinkis <simon.hoinkis@apex.ai> --------- Signed-off-by: Simon Hoinkis <simon.hoinkis@apex.ai>
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
@@ -3,7 +3,7 @@ name: Integration build rmw_iceoryx
 on:
   push:
     branches:
-      - humble
+      - iron
   pull_request:
 
 jobs:
@@ -18,11 +18,11 @@ jobs:
       - name: Setup ROS
         uses: ros-tooling/setup-ros@master
         with:
-          required-ros-distributions: humble
+          required-ros-distributions: iron
       - name: Install Iceoryx Dependencies
         run: sudo apt-get update && sudo apt-get install -y cmake libacl1-dev libncurses5-dev pkg-config
       - name: Build & Test
         uses: ros-tooling/action-ros-ci@master
         with:
           package-name: rmw_iceoryx_cpp
-          target-ros2-distro: humble
+          target-ros2-distro: iron
diff --git a/README.md b/README.md
@@ -15,7 +15,7 @@ To install rmw_iceoryx in a ROS 2 workspace with the latest ROS version, just ex
 ```bash
 mkdir -p ~/iceoryx_ws/src
 cd $_
-# LATEST_ROS_VERSION could be e.g. humble
+# LATEST_ROS_VERSION could be e.g. iron
 git clone --branch LATEST_ROS_VERSION https://github.com/ros2/rmw_iceoryx.git
 ```
 
diff --git a/rmw_iceoryx_cpp/CMakeLists.txt b/rmw_iceoryx_cpp/CMakeLists.txt
@@ -114,6 +114,7 @@ add_library(rmw_iceoryx_cpp SHARED
   src/rmw_trigger_guard_condition.cpp
   src/rmw_wait.cpp
   src/rmw_wait_set.cpp
+  src/rmw_dynamic_type_support.cpp
 )
 ament_target_dependencies(rmw_iceoryx_cpp
   "rcutils"
diff --git a/rmw_iceoryx_cpp/src/iceoryx_generate_gid.hpp b/rmw_iceoryx_cpp/src/iceoryx_generate_gid.hpp
@@ -1,5 +1,5 @@
 // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved.
-// Copyright (c) 2022 by Apex.AI Inc. All rights reserved.
+// Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -19,7 +19,9 @@
 #include "rmw/types.h"
 
 #include "iceoryx_posh/popo/untyped_publisher.hpp"
+#include "iceoryx_posh/popo/untyped_client.hpp"
 
 rmw_gid_t generate_publisher_gid(iox::popo::UntypedPublisher * const publisher);
+rmw_gid_t generate_client_gid(iox::popo::UntypedClient * const client);
 
 #endif  // ICEORYX_GENERATE_GID_HPP_
diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_generate_gid.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_generate_gid.cpp
@@ -1,5 +1,5 @@
 // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved.
-// Copyright (c) 2022 by Apex.AI Inc. All rights reserved.
+// Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -18,6 +18,7 @@
 #include "rmw/impl/cpp/macros.hpp"
 
 #include "iceoryx_posh/popo/untyped_publisher.hpp"
+#include "iceoryx_posh/popo/untyped_client.hpp"
 
 rmw_gid_t generate_publisher_gid(iox::popo::UntypedPublisher * const publisher)
 {
@@ -31,7 +32,27 @@ rmw_gid_t generate_publisher_gid(iox::popo::UntypedPublisher * const publisher)
   size_t size = sizeof(uid);
 
   if (!typed_uid.isValid() || size > RMW_GID_STORAGE_SIZE) {
-    RMW_SET_ERROR_MSG("Could not generated gid");
+    RMW_SET_ERROR_MSG("Could not generated client gid");
+    return gid;
+  }
+  memcpy(gid.data, &uid, size);
+
+  return gid;
+}
+
+rmw_gid_t generate_client_gid(iox::popo::UntypedClient * const client)
+{
+  rmw_gid_t gid;
+  gid.implementation_identifier = rmw_get_implementation_identifier();
+  memset(gid.data, 0, RMW_GID_STORAGE_SIZE);
+
+  iox::popo::UniquePortId typed_uid = client->getUid();
+  iox::popo::UniquePortId::value_type uid =
+    static_cast<iox::popo::UniquePortId::value_type>(typed_uid);
+  size_t size = sizeof(uid);
+
+  if (!typed_uid.isValid() || size > RMW_GID_STORAGE_SIZE) {
+    RMW_SET_ERROR_MSG("Could not generated publisher gid");
     return gid;
   }
   memcpy(gid.data, &uid, size);
diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp
@@ -165,18 +165,19 @@ std::map<std::string, std::string> get_service_names_and_types()
     iox::popo::MessagingPattern::REQ_RES);
 
   for (auto & server : available_servers) {
-    auto name_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description(
+    /// @todo Use structured bindings once all platforms are on C++17
+    std::string name;
+    std::string type;
+    std::tie(name, type) = rmw_iceoryx_cpp::get_name_n_type_from_service_description(
       std::string(server.getServiceIDString().c_str()),
       std::string(server.getInstanceIDString().c_str()),
       std::string(server.getEventIDString().c_str()));
 
-    names_n_types[std::get<0>(name_and_type)] = std::get<1>(name_and_type);
+    names_n_types[name] = type;
     /// @todo There is no API to find out which 'ServiceDescription' is offered by which node,
     /// for now we use 'NodeFoo'..
-    servers_topics[std::string("NodeFoo")].push_back(
-      std::get<0>(
-        name_and_type));
-    topic_servers[std::get<0>(name_and_type)].push_back(std::string("NodeFoo"));
+    servers_topics[std::string("NodeFoo")].push_back(name);
+    topic_servers[name].push_back(std::string("NodeFoo"));
   }
 
   return names_n_types;
diff --git a/rmw_iceoryx_cpp/src/rmw_client.cpp b/rmw_iceoryx_cpp/src/rmw_client.cpp
@@ -203,4 +203,26 @@ rmw_ret_t rmw_client_set_on_new_response_callback(
 
   return RMW_RET_UNSUPPORTED;
 }
+
+rmw_ret_t
+rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT);
+
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    rmw_get_gid_for_client
+    : client, client->implementation_identifier,
+    rmw_get_implementation_identifier(),
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+
+  IceoryxClient * iceoryx_client_abstraction = static_cast<IceoryxClient *>(client->data);
+
+  if (!iceoryx_client_abstraction) {
+    RMW_SET_ERROR_MSG("client info handle is null");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+  *gid = iceoryx_client_abstraction->gid_;
+  return RMW_RET_OK;
+}
 }  // extern "C"
diff --git a/rmw_iceoryx_cpp/src/rmw_dynamic_type_support.cpp b/rmw_iceoryx_cpp/src/rmw_dynamic_type_support.cpp
@@ -0,0 +1,67 @@
+// Copyright (c) 2023 by Apex.AI Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rmw/impl/cpp/macros.hpp"
+#include "rmw/rmw.h"
+
+extern "C"
+{
+rmw_ret_t
+rmw_take_dynamic_message(
+  const rmw_subscription_t * subscription,
+  rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message,
+  bool * taken,
+  rmw_subscription_allocation_t * allocation)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(dynamic_message, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocation, RMW_RET_ERROR);
+
+  RMW_SET_ERROR_MSG("rmw_take_dynamic_message is not supported in iceoryx");
+  return RMW_RET_UNSUPPORTED;
+}
+
+rmw_ret_t
+rmw_take_dynamic_message_with_info(
+  const rmw_subscription_t * subscription,
+  rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message,
+  bool * taken,
+  rmw_message_info_t * message_info,
+  rmw_subscription_allocation_t * allocation)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(dynamic_message, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocation, RMW_RET_ERROR);
+
+  RMW_SET_ERROR_MSG("rmw_take_dynamic_message_with_info is not supported in iceoryx");
+  return RMW_RET_UNSUPPORTED;
+}
+
+rmw_ret_t
+rmw_serialization_support_init(
+  const char * serialization_lib_name,
+  rcutils_allocator_t * allocator,
+  rosidl_dynamic_typesupport_serialization_support_t * serialization_support)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialization_lib_name, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialization_support, RMW_RET_ERROR);
+
+  RMW_SET_ERROR_MSG("rmw_serialization_support_init is not supported in iceoryx");
+  return RMW_RET_UNSUPPORTED;
+}
+}  // extern "C"
diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp
@@ -139,10 +139,10 @@ rmw_take_request(
   iceoryx_server->take()
   .and_then(
     [&](const void * iceoryx_request_payload) {
-      const iox::mepoo::ChunkHeader * chunk_header =
+      const auto * chunk_header =
       iox::mepoo::ChunkHeader::fromUserPayload(
         iceoryx_request_payload);
-      const iox::popo::RequestHeader * iceoryx_request_header =
+      const auto * iceoryx_request_header =
       iox::popo::RequestHeader::fromPayload(
         iceoryx_request_payload);
 
diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp
@@ -98,7 +98,6 @@ rmw_take_response(
             &iceoryx_client_abstraction->type_supports_,
             ros_response);
         }
-        iceoryx_client->releaseResponse(iceoryx_response_payload);
 
         *taken = true;
         ret = RMW_RET_OK;
@@ -107,6 +106,7 @@ rmw_take_response(
         *taken = false;
         ret = RMW_RET_ERROR;
       }
+      iceoryx_client->releaseResponse(iceoryx_response_payload);
     })
   .or_else(
     [&](auto &) {
diff --git a/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp b/rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp
@@ -48,11 +48,7 @@ rmw_service_server_is_available(
     return ret;
   }
 
-  std::this_thread::sleep_for(std::chrono::seconds(1));
-
-  if (iceoryx_client->getConnectionState() == iox::ConnectionState::CONNECTED) {
-    *is_available = true;
-  }
+  *is_available = iceoryx_client->getConnectionState() == iox::ConnectionState::CONNECTED;
 
   ret = RMW_RET_OK;
   return ret;
diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_client.hpp
@@ -15,6 +15,8 @@
 #ifndef TYPES__ICEORYX_CLIENT_HPP_
 #define TYPES__ICEORYX_CLIENT_HPP_
 
+#include "../iceoryx_generate_gid.hpp"
+
 #include "iceoryx_posh/popo/untyped_client.hpp"
 
 #include "rmw/rmw.h"
@@ -30,14 +32,16 @@ struct IceoryxClient
   : type_supports_(*type_supports),
     iceoryx_client_(iceoryx_client),
     is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)),
-    request_size_(rmw_iceoryx_cpp::iceoryx_get_request_size(type_supports))
+    request_size_(rmw_iceoryx_cpp::iceoryx_get_request_size(type_supports)),
+    gid_(generate_client_gid(iceoryx_client_))
   {}
 
   rosidl_service_type_support_t type_supports_;
   iox::popo::UntypedClient * const iceoryx_client_;
-  bool is_fixed_size_;
-  size_t request_size_;
-  int64_t sequence_id_;
+  bool is_fixed_size_{false};
+  size_t request_size_{0};
+  int64_t sequence_id_{0};
+  rmw_gid_t gid_;
 };
 
 #endif  // TYPES__ICEORYX_CLIENT_HPP_
diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_server.hpp
@@ -35,8 +35,8 @@ struct IceoryxServer
 
   rosidl_service_type_support_t type_supports_;
   iox::popo::UntypedServer * const iceoryx_server_;
-  bool is_fixed_size_;
-  size_t response_size_;
+  bool is_fixed_size_{false};
+  size_t response_size_{0};
   /// @todo The request payload pointer could also be added to 'rmw_request_id_t' if accepeted in
   /// ros2/rmw. For now, the limitation exists to always call 'rmw_take_request' followed by
   /// 'rmw_send_response' and not call e.g. 2x times 'rmw_take_request' and then

Original file line number	Diff line number	Diff line change
`@@ -114,6 +114,7 @@ add_library(rmw_iceoryx_cpp SHARED`
`114`	`114`	`src/rmw_trigger_guard_condition.cpp`
`115`	`115`	`src/rmw_wait.cpp`
`116`	`116`	`src/rmw_wait_set.cpp`
	`117`	`+ src/rmw_dynamic_type_support.cpp`
`117`	`118`	`)`
`118`	`119`	`ament_target_dependencies(rmw_iceoryx_cpp`
`119`	`120`	`"rcutils"`
Original file line number	Diff line number	Diff line change
`@@ -98,7 +98,6 @@ rmw_take_response(`
`98`	`98`	`&iceoryx_client_abstraction->type_supports_,`
`99`	`99`	`ros_response);`
`100`	`100`	`}`
`101`		`- iceoryx_client->releaseResponse(iceoryx_response_payload);`
`102`	`101`
`103`	`102`	`*taken = true;`
`104`	`103`	`ret = RMW_RET_OK;`
`@@ -107,6 +106,7 @@ rmw_take_response(`
`107`	`106`	`*taken = false;`
`108`	`107`	`ret = RMW_RET_ERROR;`
`109`	`108`	`}`
	`109`	`+ iceoryx_client->releaseResponse(iceoryx_response_payload);`
`110`	`110`	`})`
`111`	`111`	`.or_else(`
`112`	`112`	`[&](auto &) {`