Runtime Interface Reflection: rcl (#1025)

* Implement runtime type typesupport struct Signed-off-by: methylDragon <methylDragon@gmail.com> * Implement first cut Signed-off-by: methylDragon <methylDragon@gmail.com> * Migrate to rosidl_dynamic_typesupport and update field IDs Signed-off-by: methylDragon <methylDragon@gmail.com> * Migrate to type description interfaces Signed-off-by: methylDragon <methylDragon@gmail.com> * Fix const Signed-off-by: methylDragon <methylDragon@gmail.com> * Refine signatures and use return types Signed-off-by: methylDragon <methylDragon@gmail.com> * Fix C linkage Signed-off-by: methylDragon <methylDragon@gmail.com> * Migrate methods to use return types Signed-off-by: methylDragon <methylDragon@gmail.com> * Lint Signed-off-by: methylDragon <methylDragon@gmail.com> * Support type hashes Signed-off-by: methylDragon <methylDragon@gmail.com> * Remove identifier alias Signed-off-by: methylDragon <methylDragon@gmail.com> * Change fini to destroy Signed-off-by: methylDragon <methylDragon@gmail.com> * Use create instead of init Signed-off-by: methylDragon <methylDragon@gmail.com> * Add rcutils conversion function and remove rmw interfaces Signed-off-by: methylDragon <methylDragon@gmail.com> * Clean up some comments Signed-off-by: methylDragon <methylDragon@gmail.com> --------- Signed-off-by: methylDragon <methylDragon@gmail.com>
ros2 · Apr 8, 2023 · 67919d5 · 67919d5
1 parent cf6fafa
commit 67919d5
Show file tree

Hide file tree

Showing 8 changed files with 282 additions and 0 deletions.
diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt
@@ -43,6 +43,7 @@ set(${PROJECT_NAME}_sources
   src/rcl/context.c
   src/rcl/discovery_options.c
   src/rcl/domain_id.c
+  src/rcl/dynamic_message_type_support.c
   src/rcl/event.c
   src/rcl/expand_topic_name.c
   src/rcl/graph.c

diff --git a/rcl/include/rcl/dynamic_message_type_support.h b/rcl/include/rcl/dynamic_message_type_support.h
@@ -0,0 +1,50 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#ifndef RCL__DYNAMIC_MESSAGE_TYPE_SUPPORT_H_
+#define RCL__DYNAMIC_MESSAGE_TYPE_SUPPORT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <rosidl_runtime_c/type_description/type_description__struct.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+
+#include "rcl/macros.h"
+#include "rcl/types.h"
+#include "rcl/visibility_control.h"
+
+/// Does not take ownership of description (copies)
+/// Allocates the `ts` arg. The caller takes ownership of the `ts` arg.
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_dynamic_message_type_support_handle_create(
+  const char * serialization_lib_name,
+  const rosidl_runtime_c__type_description__TypeDescription * desc,
+  rosidl_message_type_support_t ** ts);  // OUT
+
+/// Finalize a rosidl_message_type_support_t obtained with
+/// `rcl_dynamic_message_type_support_handle_create()`
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_dynamic_message_type_support_handle_destroy(rosidl_message_type_support_t * ts);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RCL__DYNAMIC_MESSAGE_TYPE_SUPPORT_H_
diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h
@@ -29,6 +29,7 @@ extern "C"
 #include "rcl/node.h"
 #include "rcl/visibility_control.h"
 
+#include "rmw/dynamic_message_type_support.h"
 #include "rmw/message_sequence.h"
 
 /// Internal rcl implementation struct.
@@ -599,6 +600,44 @@ rcl_take_serialized_message(
   rmw_message_info_t * message_info,
   rmw_subscription_allocation_t * allocation);
 
+/// Take a dynamic type message from a topic using a rcl subscription.
+/**
+ * In contrast to rcl_take(), this function takes a dynamic type message with dynamic data taken
+ * directly from the middleware.
+ * It is the job of the caller to ensure that the type associated with the subscription
+ * matches, and that the subscription uses the dynamic type rosidl_message_type_support_t.
+ *
+ * Apart from the differences above, this function behaves like rcl_take().
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] subscription the handle to the subscription from which to take
+ * \param[inout] dynamic_message pointer to a (pre-allocated) dynamic type message.
+ * \param[out] message_info rmw struct which contains meta-data for the message
+ * \param[in] allocation structure pointer used for memory preallocation (may be NULL)
+ * \return #RCL_RET_OK if the message was taken, or
+ * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or
+ * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or
+ * \return #RCL_RET_SUBSCRIPTION_TAKE_FAILED if take failed but no error
+ *         occurred in the middleware, or
+ * \return #RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_take_dynamic_message(
+  const rcl_subscription_t * subscription,
+  rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message,
+  rmw_message_info_t * message_info,
+  rmw_subscription_allocation_t * allocation);
+
 /// Take a loaned message from a topic using a rcl subscription.
 /**
  * Depending on the middleware, incoming messages can be loaned to the user's callback

diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h
@@ -18,6 +18,7 @@
 #define RCL__TYPES_H_
 
 #include <rmw/types.h>
+#include <rcutils/types.h>
 
 /// The type that holds an rcl return code.
 typedef rmw_ret_t rcl_ret_t;

diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c
@@ -43,6 +43,25 @@ rcl_convert_rmw_ret_to_rcl_ret(rmw_ret_t rmw_ret)
   }
 }
 
+rcl_ret_t
+rcl_convert_rcutils_ret_to_rcl_ret(rcutils_ret_t rcutils_ret)
+{
+  switch (rcutils_ret) {
+    case RCUTILS_RET_OK:
+      return RCL_RET_OK;
+    case RCUTILS_RET_ERROR:
+      return RCL_RET_ERROR;
+    case RCUTILS_RET_BAD_ALLOC:
+      return RCL_RET_BAD_ALLOC;
+    case RCUTILS_RET_INVALID_ARGUMENT:
+      return RCL_RET_INVALID_ARGUMENT;
+    case RCUTILS_RET_NOT_INITIALIZED:
+      return RCL_RET_NOT_INIT;
+    default:
+      return RCL_RET_ERROR;
+  }
+}
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h
@@ -26,6 +26,10 @@ extern "C"
 rcl_ret_t
 rcl_convert_rmw_ret_to_rcl_ret(rmw_ret_t rmw_ret);
 
+/// Convenience function for converting common rcutils_ret_t return codes to rcl.
+rcl_ret_t
+rcl_convert_rcutils_ret_to_rcl_ret(rcutils_ret_t rcutils_ret);
+
 #ifdef __cplusplus
 }
 #endif

diff --git a/rcl/src/rcl/dynamic_message_type_support.c b/rcl/src/rcl/dynamic_message_type_support.c
@@ -0,0 +1,134 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include <rcutils/logging_macros.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+#include <rosidl_runtime_c/type_description/type_description__struct.h>
+
+#include "rmw/dynamic_message_type_support.h"
+
+#include "rcl/common.h"
+#include "rcl/error_handling.h"
+#include "rcl/dynamic_message_type_support.h"
+#include "rcl/type_hash.h"
+#include "rcl/types.h"
+
+
+/// Create a rosidl_message_type_support_t from a TypeDescription message
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_dynamic_message_type_support_handle_create(
+  const char * serialization_lib_name,
+  const rosidl_runtime_c__type_description__TypeDescription * description,
+  rosidl_message_type_support_t ** ts)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(ts, RCUTILS_RET_INVALID_ARGUMENT);
+
+  // TODO(methylDragon): Remove if and when the deferred description path is supported
+  if (description == NULL) {
+    RCUTILS_SET_ERROR_MSG(
+      "Deferred type description is not currently supported. You must provide a type description.");
+    return RCUTILS_RET_INVALID_ARGUMENT;
+  }
+
+  bool middleware_supports_type_discovery = rmw_feature_supported(
+    RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY);
+  if (!middleware_supports_type_discovery && description == NULL) {
+    RCL_SET_ERROR_MSG(
+      "Middleware does not support type discovery. Deferred dynamic type message type support will "
+      "never be populated. You must provide a type description.");
+    return RCUTILS_RET_INVALID_ARGUMENT;
+  }
+  // TODO(methylDragon): Remove if and when the deferred description path is supported
+  if (description == NULL) {
+    RCL_SET_ERROR_MSG(
+      "Deferred type description is not currently supported. You must provide a type description.");
+    return RCUTILS_RET_INVALID_ARGUMENT;
+  }
+
+  rosidl_dynamic_typesupport_serialization_support_t * serialization_support = NULL;
+  rcl_ret_t ret = rcl_convert_rmw_ret_to_rcl_ret(
+    rmw_get_serialization_support(serialization_lib_name, &serialization_support));
+  if (ret != RCL_RET_OK || serialization_support == NULL) {
+    RCL_SET_ERROR_MSG("failed to get serialization support");
+    if (ret == RCL_RET_OK) {  // It means serialization support was NULL
+      return RCL_RET_ERROR;
+    } else {
+      return ret;
+    }
+  }
+
+  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  rosidl_type_hash_t * type_hash = allocator.zero_allocate(
+    1, sizeof(rosidl_type_hash_t), &allocator.state);
+  if (!type_hash) {
+    RCUTILS_SET_ERROR_MSG("Could not allocate type hash");
+    return RCL_RET_ERROR;
+  }
+
+  ret = rcl_calculate_type_hash(
+    // TODO(methylDragon): Replace this cast with the conversion function when it is ready
+    //  Either a custom function, or from https://github.com/ros2/rcl/pull/1052
+    (const type_description_interfaces__msg__TypeDescription *) description, type_hash);
+  if (ret != RCL_RET_OK || type_hash == NULL) {
+    RCL_SET_ERROR_MSG("failed to get type hash");
+    allocator.deallocate(type_hash, &allocator.state);
+    if (ret == RCL_RET_OK) {
+      return RCL_RET_ERROR;
+    } else {
+      return ret;
+    }
+  }
+
+  ret = rcl_convert_rcutils_ret_to_rcl_ret(
+    rosidl_dynamic_message_type_support_handle_create(
+      serialization_support,
+      type_hash,    // type_hash
+      description,  // type_description
+      NULL,         // type_description_sources
+      ts
+    )
+  );
+
+  if (!ts) {
+    RCL_SET_ERROR_MSG("failed to init rosidl_message_type_support");
+    allocator.deallocate(type_hash, &allocator.state);
+    if (ret == RCL_RET_OK) {
+      return RCL_RET_ERROR;
+    } else {
+      return ret;
+    }
+  }
+  return RCL_RET_OK;
+}
+
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_dynamic_message_type_support_handle_destroy(rosidl_message_type_support_t * ts)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(ts, RCL_RET_INVALID_ARGUMENT);
+  return rcl_convert_rcutils_ret_to_rcl_ret(rosidl_dynamic_message_type_support_handle_destroy(ts));
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c
@@ -27,8 +27,10 @@ extern "C"
 #include "rcutils/strdup.h"
 #include "rcutils/types/string_array.h"
 #include "rmw/error_handling.h"
+#include "rmw/dynamic_message_type_support.h"
 #include "rmw/subscription_content_filter_options.h"
 #include "rmw/validate_full_topic_name.h"
+#include "rosidl_dynamic_typesupport/identifier.h"
 #include "tracetools/tracetools.h"
 
 #include "./common.h"
@@ -620,6 +622,38 @@ rcl_take_serialized_message(
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+rcl_take_dynamic_message(
+  const rcl_subscription_t * subscription,
+  rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message,
+  rmw_message_info_t * message_info,
+  rmw_subscription_allocation_t * allocation)
+{
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking dynamic message");
+  if (!rcl_subscription_is_valid(subscription)) {
+    return RCL_RET_SUBSCRIPTION_INVALID;  // error already set
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(dynamic_message, RCL_RET_INVALID_ARGUMENT);
+  // If message_info is NULL, use a place holder which can be discarded.
+  rmw_message_info_t dummy_message_info;
+  rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
+  *message_info_local = rmw_get_zero_initialized_message_info();
+  // Call take with info
+  bool taken = false;
+  rmw_ret_t ret = rmw_take_dynamic_message_with_info(
+    subscription->impl->rmw_handle, dynamic_message, &taken, message_info_local, allocation);
+  if (ret != RMW_RET_OK) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    return rcl_convert_rmw_ret_to_rcl_ret(ret);
+  }
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Subscription dynamic take succeeded: %s", taken ? "true" : "false");
+  if (!taken) {
+    return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
+  }
+  return RCL_RET_OK;
+}
+
 rcl_ret_t
 rcl_take_loaned_message(
   const rcl_subscription_t * subscription,