From 50f61d9f5f89dac450bccf514451932f5e1e6d0a Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 11 Apr 2023 15:20:18 -0700 Subject: [PATCH 1/6] Implement subscription base changes Signed-off-by: methylDragon --- .../include/rclcpp/generic_subscription.hpp | 28 ++- rclcpp/include/rclcpp/subscription.hpp | 53 ++++- rclcpp/include/rclcpp/subscription_base.hpp | 68 ++++++- rclcpp/package.xml | 1 + rclcpp/src/rclcpp/executor.cpp | 181 ++++++++++++------ rclcpp/src/rclcpp/generic_subscription.cpp | 54 +++++- rclcpp/src/rclcpp/subscription_base.cpp | 45 ++++- 7 files changed, 355 insertions(+), 75 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 12a1c79f8f..231f4d5f9e 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -84,7 +84,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase options.to_rcl_subscription_options(qos), options.event_callbacks, options.use_default_callbacks, - true), + SubscriptionType::SERIALIZED_MESSAGE), callback_(callback), ts_lib_(ts_lib) {} @@ -123,6 +123,32 @@ class GenericSubscription : public rclcpp::SubscriptionBase RCLCPP_PUBLIC void return_serialized_message(std::shared_ptr & message) override; + + // DYNAMIC TYPE ================================================================================== + // TODO(methylDragon): Reorder later + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type() + override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override; + + RCLCPP_PUBLIC + void return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override; + + RCLCPP_PUBLIC + void handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) override; + private: RCLCPP_DISABLE_COPY(GenericSubscription) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index d9e84b29f8..4111d17af9 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -144,7 +144,7 @@ class Subscription : public SubscriptionBase // NOTE(methylDragon): Passing these args separately is necessary for event binding options.event_callbacks, options.use_default_callbacks, - callback.is_serialized_message_callback()), + callback.is_serialized_message_callback() ? SubscriptionType::SERIALIZED_MESSAGE : SubscriptionType::ROS_MESSAGE), // NOLINT any_callback_(callback), options_(options), message_memory_strategy_(message_memory_strategy) @@ -388,6 +388,57 @@ class Subscription : public SubscriptionBase return any_callback_.use_take_shared_method(); } + // DYNAMIC TYPE ================================================================================== + // TODO(methylDragon): Reorder later + // TODO(methylDragon): Implement later... + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr + get_shared_dynamic_message_type() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message_type is not implemented for Subscription"); + } + + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + get_shared_dynamic_message() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message is not implemented for Subscription"); + } + + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_serialization_support is not implemented for Subscription"); + } + + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + create_dynamic_message() override + { + throw rclcpp::exceptions::UnimplementedError( + "create_dynamic_message is not implemented for Subscription"); + } + + void + return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override + { + (void) message; + throw rclcpp::exceptions::UnimplementedError( + "return_dynamic_message is not implemented for Subscription"); + } + + void + handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) override + { + (void) message; + (void) message_info; + throw rclcpp::exceptions::UnimplementedError( + "handle_dynamic_message is not implemented for Subscription"); + } + private: RCLCPP_DISABLE_COPY(Subscription) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 52057a39d2..20fb697e9b 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -31,6 +31,9 @@ #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/detail/cpp_callback_trampoline.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/macros.hpp" @@ -60,6 +63,14 @@ namespace experimental class IntraProcessManager; } // namespace experimental +enum class SubscriptionType : uint8_t +{ + ROS_MESSAGE = 1, // take message as ROS message and handle as ROS message + SERIALIZED_MESSAGE = 2, // take message as serialized and handle as serialized + DYNAMIC_MESSAGE_DIRECT = 3, // take message as DynamicMessage and handle as DynamicMessage + DYNAMIC_MESSAGE_FROM_SERIALIZED = 4 // take message as serialized and handle as DynamicMessage +}; + /// Virtual base class for subscriptions. This pattern allows us to iterate over different template /// specializations of Subscription, among other things. class SubscriptionBase : public std::enable_shared_from_this @@ -76,7 +87,7 @@ class SubscriptionBase : public std::enable_shared_from_this * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. * \param[in] subscription_options Options for the subscription. - * \param[in] is_serialized is true if the message will be delivered still serialized + * \param[in] subscription_type Enum flag to change how the message will be received and delivered */ RCLCPP_PUBLIC SubscriptionBase( @@ -86,7 +97,7 @@ class SubscriptionBase : public std::enable_shared_from_this const rcl_subscription_options_t & subscription_options, const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks, - bool is_serialized = false); + SubscriptionType subscription_type = SubscriptionType::ROS_MESSAGE); /// Destructor. RCLCPP_PUBLIC @@ -227,13 +238,13 @@ class SubscriptionBase : public std::enable_shared_from_this const rosidl_message_type_support_t & get_message_type_support_handle() const; - /// Return if the subscription is serialized + /// Return the type of the subscription. /** - * \return `true` if the subscription is serialized, `false` otherwise + * \return `SubscriptionType`, which adjusts how messages are received and delivered. */ RCLCPP_PUBLIC - bool - is_serialized() const; + SubscriptionType + get_subscription_type() const; /// Get matching publisher count. /** \return The number of publishers on this topic. */ @@ -535,6 +546,49 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::ContentFilterOptions get_content_filter() const; + // DYNAMIC TYPE ================================================================================== + // TODO(methylDragon): Reorder later + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr + get_shared_dynamic_message_type() = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + get_shared_dynamic_message() = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() = 0; + + /// Borrow a new serialized message (this clones!) + /** \return Shared pointer to a rclcpp::dynamic_typesupport::DynamicMessage. */ + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + create_dynamic_message() = 0; + + RCLCPP_PUBLIC + virtual + void + return_dynamic_message(rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) = 0; + + RCLCPP_PUBLIC + virtual + void + handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) = 0; + + RCLCPP_PUBLIC + bool + take_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage & message_out, + rclcpp::MessageInfo & message_info_out); + // =============================================================================================== + protected: template void @@ -587,7 +641,7 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_DISABLE_COPY(SubscriptionBase) rosidl_message_type_support_t type_support_; - bool is_serialized_; + SubscriptionType subscription_type_; std::atomic subscription_in_use_by_wait_set_{false}; std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 38773e2ec2..f7c89260bc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -39,6 +39,7 @@ rcpputils rcutils rmw + rosidl_dynamic_typesupport statistics_msgs tracetools diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 9bafbe3106..08b023e8e9 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -24,6 +24,7 @@ #include "rcl/error_handling.h" #include "rcpputils/scope_exit.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/guard_condition.hpp" @@ -598,70 +599,136 @@ take_and_do_error_handling( void Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { + using rclcpp::dynamic_typesupport::DynamicMessage; + rclcpp::MessageInfo message_info; message_info.get_rmw_message_info().from_intra_process = false; - if (subscription->is_serialized()) { - // This is the case where a copy of the serialized message is taken from - // the middleware via inter-process communication. - std::shared_ptr serialized_msg = subscription->create_serialized_message(); - take_and_do_error_handling( - "taking a serialized message from topic", - subscription->get_topic_name(), - [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, - [&]() - { - subscription->handle_serialized_message(serialized_msg, message_info); - }); - subscription->return_serialized_message(serialized_msg); - } else if (subscription->can_loan_messages()) { - // This is the case where a loaned message is taken from the middleware via - // inter-process communication, given to the user for their callback, - // and then returned. - void * loaned_msg = nullptr; - // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage - // is extened to support subscriptions as well. - take_and_do_error_handling( - "taking a loaned message from topic", - subscription->get_topic_name(), - [&]() + switch (subscription->get_subscription_type()) { + // Take ROS message + case rclcpp::SubscriptionType::ROS_MESSAGE: { - rcl_ret_t ret = rcl_take_loaned_message( - subscription->get_subscription_handle().get(), - &loaned_msg, - &message_info.get_rmw_message_info(), - nullptr); - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { - return false; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + if (subscription->can_loan_messages()) { + // This is the case where a loaned message is taken from the middleware via + // inter-process communication, given to the user for their callback, + // and then returned. + void * loaned_msg = nullptr; + // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage + // is extened to support subscriptions as well. + take_and_do_error_handling( + "taking a loaned message from topic", + subscription->get_topic_name(), + [&]() + { + rcl_ret_t ret = rcl_take_loaned_message( + subscription->get_subscription_handle().get(), + &loaned_msg, + &message_info.get_rmw_message_info(), + nullptr); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; + }, + [&]() {subscription->handle_loaned_message(loaned_msg, message_info);}); + if (nullptr != loaned_msg) { + rcl_ret_t ret = rcl_return_loaned_message_from_subscription( + subscription->get_subscription_handle().get(), loaned_msg); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "rcl_return_loaned_message_from_subscription() failed for subscription on topic " + "'%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + } + loaned_msg = nullptr; + } + } else { + // This case is taking a copy of the message data from the middleware via + // inter-process communication. + std::shared_ptr message = subscription->create_message(); + take_and_do_error_handling( + "taking a message from topic", + subscription->get_topic_name(), + [&]() {return subscription->take_type_erased(message.get(), message_info);}, + [&]() {subscription->handle_message(message, message_info);}); + subscription->return_message(message); } - return true; - }, - [&]() {subscription->handle_loaned_message(loaned_msg, message_info);}); - if (nullptr != loaned_msg) { - rcl_ret_t ret = rcl_return_loaned_message_from_subscription( - subscription->get_subscription_handle().get(), - loaned_msg); - if (RCL_RET_OK != ret) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); + break; + } + + // Take serialized message + case rclcpp::SubscriptionType::SERIALIZED_MESSAGE: + { + // This is the case where a copy of the serialized message is taken from + // the middleware via inter-process communication. + std::shared_ptr serialized_msg = + subscription->create_serialized_message(); + take_and_do_error_handling( + "taking a serialized message from topic", + subscription->get_topic_name(), + [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, + [&]() + { + subscription->handle_serialized_message(serialized_msg, message_info); + }); + subscription->return_serialized_message(serialized_msg); + break; + } + + // DYNAMIC SUBSCRIPTION ======================================================================== + // If a subscription is dynamic, then it will use its serialization-specific dynamic data. + // + // Two cases: + // - Dynamic type subscription using dynamic type stored in its own internal type support struct + // - Non-dynamic type subscription with no stored dynamic type + // - Subscriptions of this type must be able to lookup the local message description to + // generate a dynamic type at runtime! + // - TODO(methylDragon): I won't be handling this case yet + + // Take dynamic message directly from the middleware + case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT: + { + DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message(); + take_and_do_error_handling( + "taking a dynamic message from topic", + subscription->get_topic_name(), + // This modifies the stored dynamic data in the DynamicMessage in-place + [&]() {return subscription->take_dynamic_message(*dynamic_message, message_info);}, + [&]() {subscription->handle_dynamic_message(dynamic_message, message_info);}); + subscription->return_dynamic_message(dynamic_message); + break; + } + + // Take serialized and then convert to dynamic message + case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED: + { + std::shared_ptr serialized_msg = + subscription->create_serialized_message(); + + // NOTE(methylDragon): Is this clone necessary? If I'm following the pattern, it seems so. + DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message(); + take_and_do_error_handling( + "taking a serialized message from topic", + subscription->get_topic_name(), + [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, + [&]() + { + bool ret = dynamic_message->deserialize(serialized_msg->get_rcl_serialized_message()); + if (!ret) { + throw_from_rcl_error(ret, "Couldn't convert serialized message to dynamic data!"); + } + subscription->handle_dynamic_message(dynamic_message, message_info); + } + ); + subscription->return_serialized_message(serialized_msg); + subscription->return_dynamic_message(dynamic_message); + break; } - loaned_msg = nullptr; - } - } else { - // This case is taking a copy of the message data from the middleware via - // inter-process communication. - std::shared_ptr message = subscription->create_message(); - take_and_do_error_handling( - "taking a message from topic", - subscription->get_topic_name(), - [&]() {return subscription->take_type_erased(message.get(), message_info);}, - [&]() {subscription->handle_message(message, message_info);}); - subscription->return_message(message); } + return; } void diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index cc50955773..6e27de81da 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -43,8 +43,7 @@ void GenericSubscription::handle_message( "handle_message is not implemented for GenericSubscription"); } -void -GenericSubscription::handle_serialized_message( +void GenericSubscription::handle_serialized_message( const std::shared_ptr & message, const rclcpp::MessageInfo &) { @@ -72,4 +71,55 @@ void GenericSubscription::return_serialized_message( message.reset(); } + +// DYNAMIC TYPE ==================================================================================== +// TODO(methylDragon): Reorder later +rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr +GenericSubscription::get_shared_dynamic_message_type() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message_type is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr +GenericSubscription::get_shared_dynamic_message() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr +GenericSubscription::get_shared_dynamic_serialization_support() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_serialization_support is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr +GenericSubscription::create_dynamic_message() +{ + throw rclcpp::exceptions::UnimplementedError( + "create_dynamic_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) +{ + (void) message; + throw rclcpp::exceptions::UnimplementedError( + "return_dynamic_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) +{ + (void) message; + (void) message_info; + throw rclcpp::exceptions::UnimplementedError( + "handle_dynamic_message is not implemented for GenericSubscription"); +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index e95cb4ac49..bd48efcb8c 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -22,6 +22,7 @@ #include "rcpputils/scope_exit.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" @@ -32,6 +33,8 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rosidl_dynamic_typesupport/types.h" + using rclcpp::SubscriptionBase; SubscriptionBase::SubscriptionBase( @@ -41,7 +44,7 @@ SubscriptionBase::SubscriptionBase( const rcl_subscription_options_t & subscription_options, const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks, - bool is_serialized) + SubscriptionType subscription_type) : node_base_(node_base), node_handle_(node_base_->get_shared_rcl_node_handle()), node_logger_(rclcpp::get_node_logger(node_handle_.get())), @@ -49,8 +52,16 @@ SubscriptionBase::SubscriptionBase( intra_process_subscription_id_(0), event_callbacks_(event_callbacks), type_support_(type_support_handle), - is_serialized_(is_serialized) + subscription_type_(subscription_type) { + if (!rmw_feature_supported(RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE) && + subscription_type == rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT) + { + throw std::runtime_error( + "Cannot set subscription to take dynamic message directly, feature not supported in rmw" + ); + } + auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { @@ -255,10 +266,10 @@ SubscriptionBase::get_message_type_support_handle() const return type_support_; } -bool -SubscriptionBase::is_serialized() const +rclcpp::SubscriptionType +SubscriptionBase::get_subscription_type() const { - return is_serialized_; + return subscription_type_; } size_t @@ -442,8 +453,7 @@ SubscriptionBase::set_content_filter( rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); - std::vector cstrings = - get_c_vector_string(expression_parameters); + std::vector cstrings = get_c_vector_string(expression_parameters); rcl_ret_t ret = rcl_subscription_content_filter_options_init( subscription_handle_.get(), get_c_string(filter_expression), @@ -515,3 +525,24 @@ SubscriptionBase::get_content_filter() const return ret_options; } + + +// DYNAMIC TYPE ================================================================================== +// TODO(methylDragon): Reorder later +bool +SubscriptionBase::take_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage & message_out, + rclcpp::MessageInfo & message_info_out) +{ + rcl_ret_t ret = rcl_take_dynamic_message( + this->get_subscription_handle().get(), + &message_out.get_rosidl_dynamic_data(), + &message_info_out.get_rmw_message_info(), + nullptr); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; +} From 1bc71726298ebb125de48736152d052b2d52bc6d Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 11 Apr 2023 15:21:36 -0700 Subject: [PATCH 2/6] Add stubbed out classes Signed-off-by: methylDragon --- rclcpp/CMakeLists.txt | 5 + .../detail/dynamic_message_impl.hpp | 327 ++++++++ .../dynamic_message_type_builder_impl.hpp | 189 +++++ .../dynamic_typesupport/dynamic_message.hpp | 432 ++++++++++ .../dynamic_message_type.hpp | 197 +++++ .../dynamic_message_type_builder.hpp | 409 ++++++++++ .../dynamic_message_type_support.hpp | 166 ++++ .../dynamic_serialization_support.hpp | 98 +++ rclcpp/include/rclcpp/rclcpp.hpp | 17 + .../dynamic_typesupport/dynamic_message.cpp | 769 ++++++++++++++++++ .../dynamic_message_type.cpp | 281 +++++++ .../dynamic_message_type_builder.cpp | 614 ++++++++++++++ .../dynamic_message_type_support.cpp | 317 ++++++++ .../dynamic_serialization_support.cpp | 98 +++ .../node_interfaces/test_node_topics.cpp | 17 + rclcpp/test/rclcpp/test_generic_pubsub.cpp | 4 +- 16 files changed, 3938 insertions(+), 2 deletions(-) create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 6979d980f8..96351f7528 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -49,6 +49,11 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp src/rclcpp/detail/utilities.cpp src/rclcpp/duration.cpp + src/rclcpp/dynamic_typesupport/dynamic_message.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp + src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp src/rclcpp/event.cpp src/rclcpp/exceptions/exceptions.cpp src/rclcpp/executable_list.cpp diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp new file mode 100644 index 0000000000..5ae6827692 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp @@ -0,0 +1,327 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#endif + + +#define __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + template<> \ + ValueT \ + DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) \ + { \ + ValueT out; \ + rosidl_dynamic_typesupport_dynamic_data_get_ ## FunctionT ## _value( \ + &rosidl_dynamic_data_, id, &out); \ + return out; \ + } + +#define __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + template<> \ + ValueT \ + DynamicMessage::get_value(const std::string & name) \ + { \ + return get_value(get_member_id(name)); \ + } + +#define __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + template<> \ + void \ + DynamicMessage::set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value) \ + { \ + rosidl_dynamic_typesupport_dynamic_data_set_ ## FunctionT ## _value( \ + &rosidl_dynamic_data_, id, value); \ + } + +#define __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + template<> \ + void \ + DynamicMessage::set_value(const std::string & name, ValueT value) \ + { \ + set_value(get_member_id(name), value); \ + } + +#define __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) \ + template<> \ + rosidl_dynamic_typesupport_member_id_t \ + DynamicMessage::insert_value(ValueT value) \ + { \ + rosidl_dynamic_typesupport_member_id_t out; \ + rosidl_dynamic_typesupport_dynamic_data_insert_ ## FunctionT ## _value( \ + &rosidl_dynamic_data_, value, &out); \ + return out; \ + } + +#define DYNAMIC_MESSAGE_DEFINITIONS(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + +DYNAMIC_MESSAGE_DEFINITIONS(bool, bool); +// DYNAMIC_MESSAGE_DEFINITIONS(std::byte, byte); +DYNAMIC_MESSAGE_DEFINITIONS(char, char); +DYNAMIC_MESSAGE_DEFINITIONS(float, float32); +DYNAMIC_MESSAGE_DEFINITIONS(double, float64); +DYNAMIC_MESSAGE_DEFINITIONS(int8_t, int8); +DYNAMIC_MESSAGE_DEFINITIONS(int16_t, int16); +DYNAMIC_MESSAGE_DEFINITIONS(int32_t, int32); +DYNAMIC_MESSAGE_DEFINITIONS(int64_t, int64); +DYNAMIC_MESSAGE_DEFINITIONS(uint8_t, uint8); +DYNAMIC_MESSAGE_DEFINITIONS(uint16_t, uint16); +DYNAMIC_MESSAGE_DEFINITIONS(uint32_t, uint32); +DYNAMIC_MESSAGE_DEFINITIONS(uint64_t, uint64); +// DYNAMIC_MESSAGE_DEFINITIONS(std::string, std::string); +// DYNAMIC_MESSAGE_DEFINITIONS(std::u16string, std::u16string); + +// Byte and String getters have a different implementation and are defined below + + +// BYTE ============================================================================================ +template<> +std::byte +DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + unsigned char out; + rosidl_dynamic_typesupport_dynamic_data_get_byte_value(&get_rosidl_dynamic_data(), id, &out); + return static_cast(out); +} + + +template<> +std::byte +DynamicMessage::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +void +DynamicMessage::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::byte value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_byte_value( + &rosidl_dynamic_data_, id, static_cast(value)); +} + + +template<> +void +DynamicMessage::set_value(const std::string & name, const std::byte value) +{ + set_value(get_member_id(name), value); +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_value(const std::byte value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_byte_value( + &rosidl_dynamic_data_, static_cast(value), &out); + return out; +} + + +// STRINGS ========================================================================================= +template<> +std::string +DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + size_t buf_length; + char * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_string_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length); + auto out = std::string(buf, buf_length); + delete buf; + return out; +} + + +template<> +std::u16string +DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + size_t buf_length; + char16_t * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_wstring_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length); + auto out = std::u16string(buf, buf_length); + delete buf; + return out; +} + + +template<> +std::string +DynamicMessage::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +std::u16string +DynamicMessage::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +void +DynamicMessage::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_string_value( + &rosidl_dynamic_data_, id, value.c_str(), value.size()); +} + + +template<> +void +DynamicMessage::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_wstring_value( + &rosidl_dynamic_data_, id, value.c_str(), value.size()); +} + + +template<> +void +DynamicMessage::set_value(const std::string & name, const std::string value) +{ + set_value(get_member_id(name), value); +} + + +template<> +void +DynamicMessage::set_value(const std::string & name, const std::u16string value) +{ + set_value(get_member_id(name), value); +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_value(const std::string value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_string_value( + &rosidl_dynamic_data_, value.c_str(), value.size(), &out); + return out; +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_value(const std::u16string value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_wstring_value( + &rosidl_dynamic_data_, value.c_str(), value.size(), &out); + return out; +} + + +// THROW FOR UNSUPPORTED TYPES ===================================================================== +template +ValueT +DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type"); +} + + +template +ValueT +DynamicMessage::get_value(const std::string & name) +{ + throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type"); +} + + +template +void +DynamicMessage::set_value( + rosidl_dynamic_typesupport_member_id_t id, ValueT value) +{ + throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type"); +} + + +template +void +DynamicMessage::set_value(const std::string & name, ValueT value) +{ + throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type"); +} + + +template +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_value(ValueT value) +{ + throw rclcpp::exceptions::UnimplementedError("insert_value is not implemented for input type"); +} + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN +#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN +#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN +#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN +#undef __DYNAMIC_MESSAGE_INSERT_VALUE +#undef DYNAMIC_MESSAGE_DEFINITIONS + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp new file mode 100644 index 0000000000..60188146e1 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp @@ -0,0 +1,189 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#endif + + +#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicMessageTypeBuilder::add_member( \ + rosidl_dynamic_typesupport_member_id_t id, \ + const std::string & name, \ + const std::string & default_value) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _member( \ + &rosidl_dynamic_type_builder_, \ + id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \ + } + +#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicMessageTypeBuilder::add_array_member( \ + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, \ + size_t array_length, \ + const std::string & default_value) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _array_member( \ + &rosidl_dynamic_type_builder_, \ + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \ + array_length); \ + } + +#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicMessageTypeBuilder::add_unbounded_sequence_member( \ + rosidl_dynamic_typesupport_member_id_t id, \ + const std::string & name, \ + const std::string & default_value) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## \ + _unbounded_sequence_member( \ + &rosidl_dynamic_type_builder_, \ + id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \ + } + +#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicMessageTypeBuilder::add_bounded_sequence_member( \ + rosidl_dynamic_typesupport_member_id_t id, \ + const std::string & name, \ + size_t sequence_bound, \ + const std::string & default_value) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _bounded_sequence_member( \ + &rosidl_dynamic_type_builder_, \ + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \ + sequence_bound); \ + } + +#define DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(MemberT, FunctionT) \ + __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(bool, bool); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::byte, byte); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(char, char); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(float, float32); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(double, float64); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int8_t, int8); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int16_t, int16); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int32_t, int32); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int64_t, int64); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint8_t, uint8); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint16_t, uint16); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint32_t, uint32); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint64_t, uint64); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::string, string); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::u16string, wstring); + + +// THROW FOR UNSUPPORTED TYPES ===================================================================== +template +void +DynamicMessageTypeBuilder::add_member( + rosidl_dynamic_typesupport_member_id_t id, + const std::string & name, + const std::string & default_value) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_member is not implemented for input type"); +} + + +template +void +DynamicMessageTypeBuilder::add_array_member( + rosidl_dynamic_typesupport_member_id_t id, + const std::string & name, + size_t array_length, const std::string & default_value) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_array_member is not implemented for input type"); +} + + +template +void +DynamicMessageTypeBuilder::add_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, + const std::string & name, + const std::string & default_value) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_unbounded_sequence_member is not implemented for input type"); +} + + +template +void +DynamicMessageTypeBuilder::add_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, + const std::string & name, + size_t sequence_bound, + const std::string & default_value) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_bounded_sequence_member is not implemented for input type"); +} + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN +#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN +#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN +#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN +#undef DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp new file mode 100644 index 0000000000..fef984a1e0 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp @@ -0,0 +1,432 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ + +#include +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +class DynamicMessageType; +class DynamicMessageTypeBuilder; + +/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t +/** + * This class: + * - Exposes getter methods for the struct + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed + * DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport. + * - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer + * must point to the same location in memory as the stored raw pointer! + * + * Note: This class is meant to map to the lower level rosidl_dynamic_typesupport_dynamic_data_t, + * even though rosidl_dynamic_typesupport_dynamic_data_t is equivalent to + * rosidl_dynamic_typesupport_dynamic_data_t, exposing the fundamental methods available to + * rosidl_dynamic_typesupport_dynamic_data_t, allowing the user to access the data's fields. + */ +class DynamicMessage : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage) + + // CONSTRUCTION ================================================================================== + // Most constructors require a passed in DynamicSerializationSupport::SharedPtr, to extend the + // lifetime of the serialization support (if the constructor cannot otherwise get it from args). + // + // In cases where a dynamic data pointer is passed, the serialization support composed by + // the data should be the exact same object managed by the DynamicSerializationSupport, + // otherwise the lifetime management will not work properly. + + /// Construct a new DynamicMessage with the provided dynamic type builder, using its allocator + RCLCPP_PUBLIC + explicit DynamicMessage(std::shared_ptr dynamic_type_builder); + + /// Construct a new DynamicMessage with the provided dynamic type builder and allocator + RCLCPP_PUBLIC + DynamicMessage( + std::shared_ptr dynamic_type_builder, + rcl_allocator_t allocator); + + /// Construct a new DynamicMessage with the provided dynamic type, using its allocator + RCLCPP_PUBLIC + explicit DynamicMessage(std::shared_ptr dynamic_type); + + /// Construct a new DynamicMessage with the provided dynamic type and allocator + RCLCPP_PUBLIC + DynamicMessage( + std::shared_ptr dynamic_type, + rcl_allocator_t allocator); + + /// Assume ownership of struct + RCLCPP_PUBLIC + DynamicMessage( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_data_t && rosidl_dynamic_data); + + /// Loaning constructor + /// Must only be called with a rosidl dynaimc data object obtained from loaning! + // NOTE(methylDragon): I'd put this in protected, but I need this exposed to + // enable_shared_from_this... + RCLCPP_PUBLIC + DynamicMessage( + DynamicMessage::SharedPtr parent_data, + rosidl_dynamic_typesupport_dynamic_data_t && rosidl_loaned_data); + + // NOTE(methylDragon): Deliberately no constructor from description to nudge users towards using + // construction from dynamic type/builder, which is more efficient + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessage(DynamicMessage && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessage & operator=(DynamicMessage && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicMessage(); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_data_t & + get_rosidl_dynamic_data(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_data_t & + get_rosidl_dynamic_data() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + RCLCPP_PUBLIC + size_t + get_item_count() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_member_id(size_t index) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_member_id(const std::string & name) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_array_index(size_t index) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_array_index(const std::string & name) const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + DynamicMessage + clone(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage + init_from_type( + DynamicMessageType & type, rcl_allocator_t allocator = rcl_get_default_allocator()) const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + init_from_type_shared( + DynamicMessageType & type, rcl_allocator_t allocator = rcl_get_default_allocator()) const; + + RCLCPP_PUBLIC + bool + equals(const DynamicMessage & other) const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + loan_value( + rosidl_dynamic_typesupport_member_id_t id, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + loan_value( + const std::string & name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + void + clear_all_values(); + + RCLCPP_PUBLIC + void + clear_nonkey_values(); + + RCLCPP_PUBLIC + void + clear_value(rosidl_dynamic_typesupport_member_id_t id); + + RCLCPP_PUBLIC + void + clear_value(const std::string & name); + + RCLCPP_PUBLIC + void + clear_sequence(); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_sequence_data(); + + RCLCPP_PUBLIC + void + remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index); + + RCLCPP_PUBLIC + bool + serialize(rcl_serialized_message_t & buffer); + + RCLCPP_PUBLIC + bool + deserialize(rcl_serialized_message_t & buffer); + + + // MEMBER ACCESS TEMPLATES ======================================================================= + /** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + + template + ValueT + get_value(rosidl_dynamic_typesupport_member_id_t id); + + template + ValueT + get_value(const std::string & name); + + template + void + set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value); + + template + void + set_value(const std::string & name, ValueT value); + + template + rosidl_dynamic_typesupport_member_id_t + insert_value(ValueT value); + + + // FIXED STRING MEMBER ACCESS ==================================================================== + RCLCPP_PUBLIC + const std::string + get_fixed_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_length); + + RCLCPP_PUBLIC + const std::string + get_fixed_string_value(const std::string & name, size_t string_length); + + RCLCPP_PUBLIC + const std::u16string + get_fixed_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length); + + RCLCPP_PUBLIC + const std::u16string + get_fixed_wstring_value(const std::string & name, size_t wstring_length); + + RCLCPP_PUBLIC + void + set_fixed_string_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length); + + RCLCPP_PUBLIC + void + set_fixed_string_value(const std::string & name, const std::string value, size_t string_length); + + RCLCPP_PUBLIC + void + set_fixed_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length); + + RCLCPP_PUBLIC + void + set_fixed_wstring_value( + const std::string & name, const std::u16string value, size_t wstring_length); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_fixed_string_value(const std::string value, size_t string_length); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_fixed_wstring_value(const std::u16string value, size_t wstring_length); + + + // BOUNDED STRING MEMBER ACCESS ================================================================== + RCLCPP_PUBLIC + const std::string + get_bounded_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_bound); + + RCLCPP_PUBLIC + const std::string + get_bounded_string_value(const std::string & name, size_t string_bound); + + RCLCPP_PUBLIC + const std::u16string + get_bounded_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound); + + RCLCPP_PUBLIC + const std::u16string + get_bounded_wstring_value(const std::string & name, size_t wstring_bound); + + RCLCPP_PUBLIC + void + set_bounded_string_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound); + + RCLCPP_PUBLIC + void + set_bounded_string_value(const std::string & name, const std::string value, size_t string_bound); + + RCLCPP_PUBLIC + void + set_bounded_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound); + + RCLCPP_PUBLIC + void + set_bounded_wstring_value( + const std::string & name, const std::u16string value, size_t wstring_bound); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_bounded_string_value(const std::string value, size_t string_bound); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound); + + + // NESTED MEMBER ACCESS ========================================================================== + RCLCPP_PUBLIC + DynamicMessage + get_complex_value( + rosidl_dynamic_typesupport_member_id_t id, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage + get_complex_value( + const std::string & name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_complex_value_shared( + rosidl_dynamic_typesupport_member_id_t id, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_complex_value_shared( + const std::string & name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + void + set_complex_value(rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value); + + RCLCPP_PUBLIC + void + set_complex_value(const std::string & name, DynamicMessage & value); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_complex_value_copy(const DynamicMessage & value); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_complex_value(DynamicMessage & value); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // DynamicSerializationSupport + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data_; + bool is_loaned_; + + // Used for returning the loaned value, and lifetime management + DynamicMessage::SharedPtr parent_data_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessage) + + RCLCPP_PUBLIC + DynamicMessage(); + + RCLCPP_PUBLIC + bool + match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_data_t & dynamic_data); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp new file mode 100644 index 0000000000..beaaee08a5 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp @@ -0,0 +1,197 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ + +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +class DynamicMessage; +class DynamicMessageTypeBuilder; + +/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t` +/** + * This class: + * - Exposes getter methods for the struct + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class borrows the `rosidl_dynamic_typesupport_serialization_support_t` stored in the + * passed `DynamicSerializationSupport`. + * So it cannot outlive the `DynamicSerializationSupport`. + * - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t` + * pointer must point to the same location in memory as the stored raw pointer! + * + * This class is meant to map to the lower level `rosidl_dynamic_typesupport_dynamic_type_t`, + * which can be constructed via `DynamicMessageTypeBuilder`, which maps to + * `rosidl_dynamic_typesupport_dynamic_type_builder_t`. + * + * The usual method of obtaining a `DynamicMessageType` is through construction of + * `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then + * taking ownership of its contents. But `DynamicMessageTypeBuilder` can also be used to obtain + * `DynamicMessageType` by constructing it bottom-up instead, since it exposes the lower_level + * rosidl methods. + */ +class DynamicMessageType : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType) + + // CONSTRUCTION ================================================================================== + // Most constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the + // lifetime of the serialization support (if the constructor cannot otherwise get it from args). + // + // In cases where a dynamic type pointer is passed, the serialization support composed by + // the type should be the exact same object managed by the `DynamicSerializationSupport`, + // otherwise the lifetime management will not work properly. + + /// Construct a new `DynamicMessageType` with the provided dynamic type builder, + /// using its allocator + RCLCPP_PUBLIC + explicit DynamicMessageType(std::shared_ptr dynamic_type_builder); + + /// Construct a new `DynamicMessageType` with the provided dynamic type builder and allocator + RCLCPP_PUBLIC + DynamicMessageType( + std::shared_ptr dynamic_type_builder, + rcl_allocator_t allocator); + + /// Assume ownership of struct + RCLCPP_PUBLIC + DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_t && rosidl_dynamic_type); + + /// From description + RCLCPP_PUBLIC + DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessageType(DynamicMessageType && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessageType & operator=(DynamicMessageType && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicMessageType(); + + /// Swaps the serialization support if `serialization_support` is populated + /** + * The user can call this with another description to reconfigure the type without changing the + * serialization support + */ + RCLCPP_PUBLIC + void + init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator(), + DynamicSerializationSupport::SharedPtr serialization_support = nullptr); + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + size_t + get_member_count() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_type_t & + get_rosidl_dynamic_type(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_type_t & + get_rosidl_dynamic_type() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + DynamicMessageType + clone(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + bool + equals(const DynamicMessageType & other) const; + + RCLCPP_PUBLIC + DynamicMessage + build_dynamic_message(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + std::shared_ptr + build_dynamic_message_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // `DynamicSerializationSupport` + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageType) + + RCLCPP_PUBLIC + DynamicMessageType(); + + RCLCPP_PUBLIC + bool + match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp new file mode 100644 index 0000000000..fdb52d8c49 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp @@ -0,0 +1,409 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ + +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +class DynamicMessage; +class DynamicMessageType; + +/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *` +/** + * This class: + * - Manages the lifetime of the raw pointer. + * - Exposes getter methods to get the raw pointer and shared pointers + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed + * `DynamicSerializationSupport`. + * So it cannot outlive the `DynamicSerializationSupport`. + * - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t` + * pointer must point to the same location in memory as the stored raw pointer! + * + * This class is meant to map to rosidl_dynamic_typesupport_dynamic_type_builder_t, facilitating the + * construction of dynamic types bottom-up in the C++ layer. + * + * The usual method of obtaining a `DynamicMessageType` is through construction of + * `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then + * taking ownership of its contents. + * But `DynamicMessageTypeBuilder` can also be used to obtain `DynamicMessageType` by constructing + * it bottom-up instead, since it exposes the lower_level rosidl methods. + */ +class DynamicMessageTypeBuilder : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder) + + // CONSTRUCTION ================================================================================== + // All constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the + // lifetime of the serialization support. + // + // In cases where a dynamic type builder pointer is passed, the serialization support composed by + // the builder should be the exact same object managed by the `DynamicSerializationSupport`, + // otherwise the lifetime management will not work properly. + + /// Construct a new `DynamicMessageTypeBuilder` with the provided serialization support using its + /// allocator + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name); + + /// Construct a new `DynamicMessageTypeBuilder` with the provided serialization support and + /// allocator + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name, + rcl_allocator_t allocator); + + /// Assume ownership of struct + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_builder_t && dynamic_type_builder); + + /// From description + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessageTypeBuilder & operator=(DynamicMessageTypeBuilder && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeBuilder(); + + /// Swaps the serialization support if serialization_support is populated + /** + * The user can call this with another description to reconfigure the type without changing the + * serialization support + */ + RCLCPP_PUBLIC + void + init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator(), + DynamicSerializationSupport::SharedPtr serialization_support = nullptr); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_type_builder_t & + get_rosidl_dynamic_type_builder(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_type_builder_t & + get_rosidl_dynamic_type_builder() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + void + set_name(const std::string & name); + + RCLCPP_PUBLIC + DynamicMessageTypeBuilder + clone(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessageTypeBuilder::SharedPtr + clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + void + clear(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage + build_dynamic_message(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + build_dynamic_message_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessageType + build_dynamic_message_type(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + build_dynamic_message_type_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + + // ADD MEMBERS TEMPLATES ========================================================================= + /** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + + template + void + add_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + const std::string & default_value = ""); + + template + void + add_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length, + const std::string & default_value = ""); + + template + void + add_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + const std::string & default_value = ""); + + template + void + add_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound, + const std::string & default_value = ""); + + + // ADD FIXED STRING MEMBERS ====================================================================== + RCLCPP_PUBLIC + void + add_fixed_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_length, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_length, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_string_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_length, size_t sequence_bound, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_wstring_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_length, size_t sequence_bound, const std::string & default_value = ""); + + + // ADD BOUNDED STRING MEMBERS ==================================================================== + RCLCPP_PUBLIC + void + add_bounded_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_string_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t sequence_bound, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_wstring_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t sequence_bound, const std::string & default_value = ""); + + + // ADD NESTED MEMBERS ============================================================================ + RCLCPP_PUBLIC + void + add_complex_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, size_t sequence_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_array_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, size_t array_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_unbounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_bounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound, + const std::string & default_value = ""); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // `DynamicSerializationSupport` + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageTypeBuilder) + + RCLCPP_PUBLIC + DynamicMessageTypeBuilder(); + + RCLCPP_PUBLIC + void + init_from_serialization_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name, + rcl_allocator_t allocator); + + RCLCPP_PUBLIC + bool + match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_builder_t & dynamic_type_builder); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp new file mode 100644 index 0000000000..e3483dc9e6 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp @@ -0,0 +1,166 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for `rosidl_message_type_support_t` containing managed +/// instances of the typesupport handle impl. +/** + * + * NOTE: This class is the recommended way to obtain the dynamic message type + * support struct, instead of `rcl_dynamic_message_type_support_handle_init()`, + * because this class will manage the lifetimes for you. + * + * Do NOT call rcl_dynamic_message_type_support_handle_fini + * + * This class: + * - Exposes getter methods for the struct + * - Stores shared pointers to wrapper classes that expose the underlying + * serialization support API + * + * Ownership: + * - This class, similarly to the `rosidl_dynamic_typesupport_serialization_support_t`, must outlive + * all downstream usages of the serialization support. + */ +class DynamicMessageTypeSupport : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport) + + // CONSTRUCTION ================================================================================== + /// From description + /// Does NOT take ownership of the description (copies instead.) + /// Constructs type support top-down (calling `rcl_dynamic_message_type_support_handle_create()`) + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + const rosidl_runtime_c__type_description__TypeDescription & description, + const std::string & serialization_library_name = "", + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// From description, for provided serialization support + /// Does NOT take ownership of the description (copies instead.) + /// Constructs type support top-down (calling `rosidl_dynamic_message_type_support_handle_init()`) + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Assume ownership of managed types + /// Does NOT take ownership of the description (copies instead.) + /// + /// The serialization support used to construct all managed SharedPtrs must match. + /// The structure of the provided `description` must match the `dynamic_message_type` + /// The structure of the provided `dynamic_message_type` must match the `dynamic_message + /// + /// In this case, the user would have constructed the type support bototm-up (by creating the + /// respective dynamic members.) + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeSupport(); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + const rosidl_message_type_support_t & + get_const_rosidl_message_type_support(); + + RCLCPP_PUBLIC + const rosidl_message_type_support_t & + get_const_rosidl_message_type_support() const; + + RCLCPP_PUBLIC + const rosidl_runtime_c__type_description__TypeDescription & + get_rosidl_runtime_c_type_description() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + get_shared_dynamic_message_type(); + + RCLCPP_PUBLIC + DynamicMessageType::ConstSharedPtr + get_shared_dynamic_message_type() const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_shared_dynamic_message(); + + RCLCPP_PUBLIC + DynamicMessage::ConstSharedPtr + get_shared_dynamic_message() const; + +protected: + RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport) + + RCLCPP_PUBLIC + rosidl_message_type_support_t & + get_rosidl_message_type_support(); + +private: + RCLCPP_PUBLIC + DynamicMessageTypeSupport(); + + DynamicSerializationSupport::SharedPtr serialization_support_; + DynamicMessageType::SharedPtr dynamic_message_type_; + DynamicMessage::SharedPtr dynamic_message_; + + rosidl_message_type_support_t rosidl_message_type_support_; +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp new file mode 100644 index 0000000000..1aae3cff5a --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp @@ -0,0 +1,98 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ + +#include +#include +#include + +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t +/** + * This class: + * - Exposes getter methods for the struct + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class, similarly to the rosidl_dynamic_typesupport_serialization_support_t, must outlive + * all downstream usages of the serialization support. + */ +class DynamicSerializationSupport : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport) + + // CONSTRUCTION ================================================================================== + RCLCPP_PUBLIC + explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Get the rmw middleware implementation specific serialization support (configured by name) + RCLCPP_PUBLIC + DynamicSerializationSupport( + const std::string & serialization_library_name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Assume ownership of struct + RCLCPP_PUBLIC + explicit DynamicSerializationSupport( + rosidl_dynamic_typesupport_serialization_support_t && rosidl_serialization_support); + + /// Move constructor + RCLCPP_PUBLIC + DynamicSerializationSupport(DynamicSerializationSupport && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicSerializationSupport & operator=(DynamicSerializationSupport && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicSerializationSupport(); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_serialization_support_t & + get_rosidl_serialization_support(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_serialization_support_t & + get_rosidl_serialization_support() const; + +protected: + RCLCPP_DISABLE_COPY(DynamicSerializationSupport) + +private: + rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_; +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index f1d751ff3f..7cd7e21a8a 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -117,6 +117,21 @@ * - Allocator related items: * - rclcpp/allocator/allocator_common.hpp * - rclcpp/allocator/allocator_deleter.hpp + * - Dynamic typesupport wrappers + * - rclcpp::dynamic_typesupport::DynamicMessage + * - rclcpp::dynamic_typesupport::DynamicMessageType + * - rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder + * - rclcpp::dynamic_typesupport::DynamicSerializationSupport + * - rclcpp/dynamic_typesupport/dynamic_message.hpp + * - rclcpp/dynamic_typesupport/dynamic_message_type.hpp + * - rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp + * - rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp + * - Dynamic typesupport + * - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport + * - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp + * - Dynamic subscription + * - rclcpp::DynamicSubscription + * - rclcpp/dynamic_subscription.hpp * - Generic publisher * - rclcpp::Node::create_generic_publisher() * - rclcpp::GenericPublisher @@ -167,4 +182,6 @@ #include "rclcpp/waitable.hpp" #include "rclcpp/wait_set.hpp" +#include "rclcpp/dynamic_subscription.hpp" + #endif // RCLCPP__RCLCPP_HPP_ diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp new file mode 100644 index 0000000000..97e701061f --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp @@ -0,0 +1,769 @@ +// Copyright 2023 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. + +#include +#include +#include +#include + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ +// Template specialization implementations +#include "rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp" +#endif + + +// CONSTRUCTION ================================================================================== +DynamicMessage::DynamicMessage(const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder) +: DynamicMessage::DynamicMessage( + dynamic_type_builder, dynamic_type_builder->get_rosidl_dynamic_type_builder().allocator) {} + + +DynamicMessage::DynamicMessage( + const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder, + rcl_allocator_t allocator) +: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data()), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic message could not bind serialization support!"); + } + if (!dynamic_type_builder) { + throw std::runtime_error("dynamic message type builder cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + dynamic_type_builder->get_rosidl_dynamic_type_builder(); + + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type_builder( + &rosidl_dynamic_type_builder, &allocator, &get_rosidl_dynamic_data()); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic data object from dynamic type builder"); + } +} + + +DynamicMessage::DynamicMessage(const DynamicMessageType::SharedPtr dynamic_type) +: DynamicMessage::DynamicMessage( + dynamic_type, dynamic_type->get_rosidl_dynamic_type().allocator) {} + + +DynamicMessage::DynamicMessage( + const DynamicMessageType::SharedPtr dynamic_type, + rcl_allocator_t allocator) +: serialization_support_(dynamic_type->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data()), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic type could not bind serialization support!"); + } + if (!dynamic_type) { + throw std::runtime_error("dynamic message type cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = + dynamic_type->get_rosidl_dynamic_type(); + + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( + &rosidl_dynamic_type, &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not init new dynamic data object from dynamic type") + + rcl_get_error_string().str); + } +} + + +DynamicMessage::DynamicMessage( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_data_t && rosidl_dynamic_data) +: serialization_support_(serialization_support), + rosidl_dynamic_data_(std::move(rosidl_dynamic_data)), + is_loaned_(false), + parent_data_(nullptr) +{ + if (serialization_support) { + if (!match_serialization_support_(*serialization_support, rosidl_dynamic_data)) { + throw std::runtime_error( + "serialization support library identifier does not match dynamic data's!"); + } + } +} + + +DynamicMessage::DynamicMessage( + DynamicMessage::SharedPtr parent_data, + rosidl_dynamic_typesupport_dynamic_data_t && rosidl_loaned_data) +: serialization_support_(parent_data->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(std::move(rosidl_loaned_data)), + is_loaned_(true), + parent_data_(nullptr) +{ + if (!parent_data) { + throw std::runtime_error("parent dynamic data cannot be nullptr!"); + } + if (serialization_support_) { + if (!match_serialization_support_(*serialization_support_, rosidl_loaned_data)) { + throw std::runtime_error( + "serialization support library identifier does not match loaned dynamic data's!"); + } + } + parent_data_ = parent_data; +} + + +DynamicMessage::DynamicMessage(DynamicMessage && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_data_(std::exchange( + other.rosidl_dynamic_data_, + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data())), + is_loaned_(other.is_loaned_), + parent_data_(std::exchange(other.parent_data_, nullptr)) +{} + + +DynamicMessage & +DynamicMessage::operator=(DynamicMessage && other) noexcept +{ + std::swap(serialization_support_, other.serialization_support_); + std::swap(rosidl_dynamic_data_, other.rosidl_dynamic_data_); + is_loaned_ = other.is_loaned_; + std::swap(parent_data_, other.parent_data_); + return *this; +} + + +DynamicMessage::~DynamicMessage() +{ + if (!is_loaned_) { + if (rosidl_dynamic_typesupport_dynamic_data_fini(&get_rosidl_dynamic_data()) != + RCUTILS_RET_OK) + { + RCUTILS_LOG_ERROR("could not fini rosidl dynamic data"); + } + return; + } + + // Loaned case + if (!parent_data_) { + RCUTILS_LOG_ERROR("dynamic data is loaned, but parent is missing!!"); + } else { + rosidl_dynamic_typesupport_dynamic_data_return_loaned_value( + &parent_data_->get_rosidl_dynamic_data(), &get_rosidl_dynamic_data()); + } +} + + +bool +DynamicMessage::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_data_t & rosidl_dynamic_type_data) +{ + if (serialization_support.get_serialization_library_identifier() != std::string( + rosidl_dynamic_type_data.serialization_support->serialization_library_identifier)) + { + RCUTILS_LOG_ERROR("serialization support library identifier does not match dynamic data's"); + return false; + } + return true; +} + + +// GETTERS ======================================================================================= +const std::string +DynamicMessage::get_serialization_library_identifier() const +{ + return std::string( + get_rosidl_dynamic_data().serialization_support->serialization_library_identifier); +} + + +const std::string +DynamicMessage::get_name() const +{ + size_t buf_length; + const char * buf; + if ( + rosidl_dynamic_typesupport_dynamic_data_get_name( + &get_rosidl_dynamic_data(), &buf, + &buf_length) != + RCUTILS_RET_OK) + { + throw std::runtime_error( + std::string("could not get name for dynamic data") + rcl_get_error_string().str); + } + return std::string(buf, buf_length); +} + + +rosidl_dynamic_typesupport_dynamic_data_t & +DynamicMessage::get_rosidl_dynamic_data() +{ + return rosidl_dynamic_data_; +} + + +const rosidl_dynamic_typesupport_dynamic_data_t & +DynamicMessage::get_rosidl_dynamic_data() const +{ + return rosidl_dynamic_data_; +} + + +DynamicSerializationSupport::SharedPtr +DynamicMessage::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessage::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +size_t +DynamicMessage::get_item_count() const +{ + size_t item_count; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_item_count( + &get_rosidl_dynamic_data(), &item_count); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not get item count of dynamic data"); + } + return item_count; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::get_member_id(size_t index) const +{ + rosidl_dynamic_typesupport_member_id_t member_id; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_at_index( + &get_rosidl_dynamic_data(), index, &member_id); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not member id of dynamic data element by index"); + } + return member_id; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::get_member_id(const std::string & name) const +{ + rosidl_dynamic_typesupport_member_id_t member_id; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_by_name( + &get_rosidl_dynamic_data(), name.c_str(), name.size(), &member_id); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not member id of dynamic data element by name"); + } + return member_id; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::get_array_index(size_t index) const +{ + rosidl_dynamic_typesupport_member_id_t array_index; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_array_index( + &get_rosidl_dynamic_data(), index, &array_index); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not array index of dynamic data element by index"); + } + return array_index; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::get_array_index(const std::string & name) const +{ + return get_array_index(get_member_id(name)); +} + + +// METHODS ======================================================================================= +DynamicMessage +DynamicMessage::clone(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone( + &get_rosidl_dynamic_data(), &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage(get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_data)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::clone_shared(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone( + &get_rosidl_dynamic_data(), &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage::make_shared( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_data)); +} + + +DynamicMessage +DynamicMessage::init_from_type(DynamicMessageType & type, rcl_allocator_t allocator) const +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( + &type.get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic data object from dynamic type"); + } + return DynamicMessage(serialization_support_, std::move(rosidl_dynamic_data)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::init_from_type_shared(DynamicMessageType & type, rcl_allocator_t allocator) const +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( + &type.get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic data object from dynamic type"); + } + return DynamicMessage::make_shared(serialization_support_, std::move(rosidl_dynamic_data)); +} + + +bool +DynamicMessage::equals(const DynamicMessage & other) const +{ + if (get_serialization_library_identifier() != other.get_serialization_library_identifier()) { + throw std::runtime_error("library identifiers don't match"); + } + bool equals; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_equals( + &get_rosidl_dynamic_data(), &other.get_rosidl_dynamic_data(), &equals); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not equate dynamic messages"); + } + return equals; +} + + +DynamicMessage::SharedPtr +DynamicMessage::loan_value( + rosidl_dynamic_typesupport_member_id_t id, + rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_loan_value( + &get_rosidl_dynamic_data(), id, &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not loan dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage::make_shared(shared_from_this(), std::move(rosidl_dynamic_data)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::loan_value(const std::string & name, rcl_allocator_t allocator) +{ + return loan_value(get_member_id(name), allocator); +} + + +void +DynamicMessage::clear_all_values() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_all_values(&get_rosidl_dynamic_data()); +} + + +void +DynamicMessage::clear_nonkey_values() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_nonkey_values(&get_rosidl_dynamic_data()); +} + + +void +DynamicMessage::clear_value(rosidl_dynamic_typesupport_member_id_t id) +{ + rosidl_dynamic_typesupport_dynamic_data_clear_value(&get_rosidl_dynamic_data(), id); +} + + +void +DynamicMessage::clear_value(const std::string & name) +{ + clear_value(get_member_id(name)); +} + + +void +DynamicMessage::clear_sequence() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_sequence_data(&get_rosidl_dynamic_data()); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_sequence_data() +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_sequence_data(&get_rosidl_dynamic_data(), &out); + return out; +} + + +void +DynamicMessage::remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index) +{ + rosidl_dynamic_typesupport_dynamic_data_remove_sequence_data( + &get_rosidl_dynamic_data(), index); +} + + +bool +DynamicMessage::serialize(rcl_serialized_message_t & buffer) +{ + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_serialize( + &get_rosidl_dynamic_data(), &buffer); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could serialize loan dynamic data: ") + rcl_get_error_string().str); + } + return true; +} + + +bool +DynamicMessage::deserialize(rcl_serialized_message_t & buffer) +{ + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_deserialize( + &get_rosidl_dynamic_data(), &buffer); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could deserialize loan dynamic data: ") + rcl_get_error_string().str); + } + return true; +} + + +// MEMBER ACCESS =================================================================================== +// Defined in "detail/dynamic_message_impl.hpp" + + +// FIXED STRING MEMBER ACCESS ====================================================================== +const std::string +DynamicMessage::get_fixed_string_value( + rosidl_dynamic_typesupport_member_id_t id, size_t string_length) +{ + size_t buf_length; + char * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_fixed_string_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length, string_length); + auto out = std::string(buf, buf_length); + delete buf; + return out; +} + + +const std::string +DynamicMessage::get_fixed_string_value(const std::string & name, size_t string_length) +{ + return get_fixed_string_value(get_member_id(name), string_length); +} + + +const std::u16string +DynamicMessage::get_fixed_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length) +{ + size_t buf_length; + char16_t * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_fixed_wstring_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_length); + auto out = std::u16string(buf, buf_length); + delete buf; + return out; +} + + +const std::u16string +DynamicMessage::get_fixed_wstring_value(const std::string & name, size_t wstring_length) +{ + return get_fixed_wstring_value(get_member_id(name), wstring_length); +} + + +void +DynamicMessage::set_fixed_string_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length) +{ + rosidl_dynamic_typesupport_dynamic_data_set_fixed_string_value( + &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_length); +} + + +void +DynamicMessage::set_fixed_string_value( + const std::string & name, const std::string value, size_t string_length) +{ + set_fixed_string_value(get_member_id(name), value, string_length); +} + + +void +DynamicMessage::set_fixed_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length) +{ + rosidl_dynamic_typesupport_dynamic_data_set_fixed_wstring_value( + &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_length); +} + + +void +DynamicMessage::set_fixed_wstring_value( + const std::string & name, const std::u16string value, size_t wstring_length) +{ + set_fixed_wstring_value(get_member_id(name), value, wstring_length); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_fixed_string_value(const std::string value, size_t string_length) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_fixed_string_value( + &get_rosidl_dynamic_data(), value.c_str(), value.size(), string_length, &out); + return out; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_fixed_wstring_value(const std::u16string value, size_t wstring_length) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_fixed_wstring_value( + &get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_length, &out); + return out; +} + + +// BOUNDED STRING MEMBER ACCESS ==================================================================== +const std::string +DynamicMessage::get_bounded_string_value( + rosidl_dynamic_typesupport_member_id_t id, size_t string_bound) +{ + size_t buf_length; + char * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_bounded_string_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length, string_bound); + auto out = std::string(buf, buf_length); + delete buf; + return out; +} + + +const std::string +DynamicMessage::get_bounded_string_value(const std::string & name, size_t string_bound) +{ + return get_bounded_string_value(get_member_id(name), string_bound); +} + + +const std::u16string +DynamicMessage::get_bounded_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound) +{ + size_t buf_length; + char16_t * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_bounded_wstring_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_bound); + auto out = std::u16string(buf, buf_length); + delete buf; + return out; +} + + +const std::u16string +DynamicMessage::get_bounded_wstring_value(const std::string & name, size_t wstring_bound) +{ + return get_bounded_wstring_value(get_member_id(name), wstring_bound); +} + + +void +DynamicMessage::set_bounded_string_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound) +{ + rosidl_dynamic_typesupport_dynamic_data_set_bounded_string_value( + &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_bound); +} + + +void +DynamicMessage::set_bounded_string_value( + const std::string & name, const std::string value, size_t string_bound) +{ + set_bounded_string_value(get_member_id(name), value, string_bound); +} + + +void +DynamicMessage::set_bounded_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound) +{ + rosidl_dynamic_typesupport_dynamic_data_set_bounded_wstring_value( + &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_bound); +} + + +void +DynamicMessage::set_bounded_wstring_value( + const std::string & name, const std::u16string value, size_t wstring_bound) +{ + set_bounded_wstring_value(get_member_id(name), value, wstring_bound); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_bounded_string_value(const std::string value, size_t string_bound) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_bounded_string_value( + &get_rosidl_dynamic_data(), value.c_str(), value.size(), string_bound, &out); + return out; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_bounded_wstring_value( + &get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_bound, &out); + return out; +} + + +// NESTED MEMBER ACCESS ============================================================================ +DynamicMessage +DynamicMessage::get_complex_value( + rosidl_dynamic_typesupport_member_id_t id, rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t out = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rosidl_dynamic_typesupport_dynamic_data_get_complex_value( + &get_rosidl_dynamic_data(), id, &allocator, &out); + return DynamicMessage(get_shared_dynamic_serialization_support(), std::move(out)); +} + + +DynamicMessage +DynamicMessage::get_complex_value(const std::string & name, rcl_allocator_t allocator) +{ + return get_complex_value(get_member_id(name), allocator); +} + + +DynamicMessage::SharedPtr +DynamicMessage::get_complex_value_shared( + rosidl_dynamic_typesupport_member_id_t id, rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t out = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rosidl_dynamic_typesupport_dynamic_data_get_complex_value( + &get_rosidl_dynamic_data(), id, &allocator, &out); + return DynamicMessage::make_shared(get_shared_dynamic_serialization_support(), std::move(out)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::get_complex_value_shared(const std::string & name, rcl_allocator_t allocator) +{ + return get_complex_value_shared(get_member_id(name), allocator); +} + + +void +DynamicMessage::set_complex_value( + rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_complex_value( + &get_rosidl_dynamic_data(), id, &value.get_rosidl_dynamic_data()); +} + + +void +DynamicMessage::set_complex_value(const std::string & name, DynamicMessage & value) +{ + set_complex_value(get_member_id(name), value); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_complex_value_copy(const DynamicMessage & value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_complex_value_copy( + &get_rosidl_dynamic_data(), &value.get_rosidl_dynamic_data(), &out); + return out; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_complex_value(DynamicMessage & value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_complex_value( + &get_rosidl_dynamic_data(), &value.get_rosidl_dynamic_data(), &out); + return out; +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp new file mode 100644 index 0000000000..21a67ebbe0 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp @@ -0,0 +1,281 @@ +// Copyright 2023 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. + +#include +#include +#include +#include + +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + + +// CONSTRUCTION ==================================================================================== +DynamicMessageType::DynamicMessageType(DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder) +: DynamicMessageType::DynamicMessageType( + dynamic_type_builder, dynamic_type_builder->get_rosidl_dynamic_type_builder().allocator) {} + + +DynamicMessageType::DynamicMessageType( + DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder, + rcl_allocator_t allocator) +: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), + rosidl_dynamic_type_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type()) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic type could not bind serialization support!"); + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + dynamic_type_builder->get_rosidl_dynamic_type_builder(); + + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_init_from_dynamic_type_builder( + &rosidl_dynamic_type_builder, &allocator, &get_rosidl_dynamic_type()); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic type object"); + } +} + + +DynamicMessageType::DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_t && rosidl_dynamic_type) +: serialization_support_(serialization_support), + rosidl_dynamic_type_(std::move(rosidl_dynamic_type)) +{ + if (serialization_support) { + if (!match_serialization_support_(*serialization_support, rosidl_dynamic_type)) { + throw std::runtime_error( + "serialization support library identifier does not match dynamic type's!"); + } + } +} + + +DynamicMessageType::DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + rosidl_dynamic_type_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type()) +{ + init_from_description(description, allocator, serialization_support); +} + + +DynamicMessageType::DynamicMessageType(DynamicMessageType && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_type_(std::exchange( + other.rosidl_dynamic_type_, rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type())) +{} + + +DynamicMessageType & +DynamicMessageType::operator=(DynamicMessageType && other) noexcept +{ + std::swap(serialization_support_, other.serialization_support_); + std::swap(rosidl_dynamic_type_, other.rosidl_dynamic_type_); + return *this; +} + + +DynamicMessageType::~DynamicMessageType() +{ + if (rosidl_dynamic_typesupport_dynamic_type_fini(&get_rosidl_dynamic_type()) != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR("could not fini rosidl dynamic type"); + } +} + + +void +DynamicMessageType::init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator, + DynamicSerializationSupport::SharedPtr serialization_support) +{ + if (serialization_support) { + // Swap serialization support if serialization support is given + serialization_support_ = serialization_support; + } + + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_init_from_description( + &serialization_support_->get_rosidl_serialization_support(), + &description, + &allocator, + &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic type object"); + } + + rosidl_dynamic_type_ = std::move(rosidl_dynamic_type); +} + + +bool +DynamicMessageType::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type) +{ + if (serialization_support.get_serialization_library_identifier() != std::string( + rosidl_dynamic_type.serialization_support->serialization_library_identifier)) + { + RCUTILS_LOG_ERROR( + "serialization support library identifier does not match dynamic type's (%s vs %s)", + serialization_support.get_serialization_library_identifier().c_str(), + rosidl_dynamic_type.serialization_support->serialization_library_identifier); + return false; + } + return true; +} + + +// GETTERS ========================================================================================= +const std::string +DynamicMessageType::get_serialization_library_identifier() const +{ + return std::string( + get_rosidl_dynamic_type().serialization_support->serialization_library_identifier); +} + + +const std::string +DynamicMessageType::get_name() const +{ + size_t buf_length; + const char * buf; + rosidl_dynamic_typesupport_dynamic_type_get_name(&get_rosidl_dynamic_type(), &buf, &buf_length); + return std::string(buf, buf_length); +} + + +size_t +DynamicMessageType::get_member_count() const +{ + size_t out; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_get_member_count( + &get_rosidl_dynamic_type(), &out); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not get member count: ") + rcl_get_error_string().str); + } + return out; +} + + +rosidl_dynamic_typesupport_dynamic_type_t & +DynamicMessageType::get_rosidl_dynamic_type() +{ + return rosidl_dynamic_type_; +} + + +const rosidl_dynamic_typesupport_dynamic_type_t & +DynamicMessageType::get_rosidl_dynamic_type() const +{ + return rosidl_dynamic_type_; +} + + +DynamicSerializationSupport::SharedPtr +DynamicMessageType::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessageType::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +// METHODS ========================================================================================= +DynamicMessageType +DynamicMessageType::clone(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone( + &get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic type: ") + rcl_get_error_string().str); + } + return DynamicMessageType( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type)); +} + + +DynamicMessageType::SharedPtr +DynamicMessageType::clone_shared(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone( + &get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic type: ") + rcl_get_error_string().str); + } + return DynamicMessageType::make_shared( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type)); +} + + +bool +DynamicMessageType::equals(const DynamicMessageType & other) const +{ + if (get_serialization_library_identifier() != other.get_serialization_library_identifier()) { + throw std::runtime_error("library identifiers don't match"); + } + bool out; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_equals( + &get_rosidl_dynamic_type(), &other.get_rosidl_dynamic_type(), &out); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not equate dynamic message types: ") + rcl_get_error_string().str); + } + return out; +} + + +DynamicMessage +DynamicMessageType::build_dynamic_message(rcl_allocator_t allocator) +{ + return DynamicMessage(shared_from_this(), allocator); +} + + +DynamicMessage::SharedPtr +DynamicMessageType::build_dynamic_message_shared(rcl_allocator_t allocator) +{ + return DynamicMessage::make_shared(shared_from_this(), allocator); +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp new file mode 100644 index 0000000000..ca225cf107 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp @@ -0,0 +1,614 @@ +// Copyright 2023 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. + +#include +#include +#include +#include + +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ +// Template specialization implementations +#include "rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp" +#endif + +// CONSTRUCTION ==================================================================================== +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, const std::string & name) +: DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + serialization_support, + name, + serialization_support->get_rosidl_serialization_support().allocator) {} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_( + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) +{ + init_from_serialization_support_(serialization_support, name, allocator); +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_builder_t && rosidl_dynamic_type_builder) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_( + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!match_serialization_support_(*serialization_support, rosidl_dynamic_type_builder)) { + throw std::runtime_error( + "serialization support library does not match dynamic type builder's!"); + } +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_( + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + init_from_description(description, allocator, serialization_support); +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_type_builder_(std::exchange( + other.rosidl_dynamic_type_builder_, + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder())) {} + + +DynamicMessageTypeBuilder & +DynamicMessageTypeBuilder::operator=(DynamicMessageTypeBuilder && other) noexcept +{ + std::swap(serialization_support_, other.serialization_support_); + std::swap(rosidl_dynamic_type_builder_, other.rosidl_dynamic_type_builder_); + return *this; +} + + +DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder() +{ + if (rosidl_dynamic_typesupport_dynamic_type_builder_fini(&get_rosidl_dynamic_type_builder()) != + RCUTILS_RET_OK) + { + RCUTILS_LOG_ERROR("could not fini rosidl dynamic type builder"); + } +} + + +void +DynamicMessageTypeBuilder::init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator, + DynamicSerializationSupport::SharedPtr serialization_support) +{ + if (serialization_support) { + // Swap serialization support if serialization support is given + serialization_support_ = serialization_support; + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_init_from_description( + &serialization_support_->get_rosidl_serialization_support(), + &description, + &allocator, + &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic type builder object"); + } + + rosidl_dynamic_type_builder_ = std::move(rosidl_dynamic_type_builder); +} + + +void +DynamicMessageTypeBuilder::init_from_serialization_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name, + rcl_allocator_t allocator) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!&serialization_support->get_rosidl_serialization_support()) { + throw std::runtime_error("serialization support raw pointer cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_init( + &serialization_support->get_rosidl_serialization_support(), + name.c_str(), name.size(), + &allocator, + &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not init dynamic type builder: ") + rcl_get_error_string().str); + } +} + + +bool +DynamicMessageTypeBuilder::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_builder_t & rosidl_dynamic_type_builder) +{ + if (serialization_support.get_serialization_library_identifier() != std::string( + rosidl_dynamic_type_builder.serialization_support->serialization_library_identifier)) + { + RCUTILS_LOG_ERROR( + "serialization support library identifier does not match dynamic type builder's"); + return false; + } + return true; +} + + +// GETTERS ======================================================================================= +const std::string +DynamicMessageTypeBuilder::get_serialization_library_identifier() const +{ + return std::string( + get_rosidl_dynamic_type_builder().serialization_support->serialization_library_identifier); +} + + +const std::string +DynamicMessageTypeBuilder::get_name() const +{ + size_t buf_length; + const char * buf; + rosidl_dynamic_typesupport_dynamic_type_builder_get_name( + &get_rosidl_dynamic_type_builder(), &buf, &buf_length); + return std::string(buf, buf_length); +} + + +rosidl_dynamic_typesupport_dynamic_type_builder_t & +DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() +{ + return rosidl_dynamic_type_builder_; +} + + +const rosidl_dynamic_typesupport_dynamic_type_builder_t & +DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() const +{ + return rosidl_dynamic_type_builder_; +} + + +DynamicSerializationSupport::SharedPtr +DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +// METHODS ======================================================================================= +void +DynamicMessageTypeBuilder::set_name(const std::string & name) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_set_name( + &get_rosidl_dynamic_type_builder(), name.c_str(), name.size()); +} + + +DynamicMessageTypeBuilder +DynamicMessageTypeBuilder::clone(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone( + &get_rosidl_dynamic_type_builder(), &allocator, &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str); + } + return DynamicMessageTypeBuilder( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type_builder)); +} + + +DynamicMessageTypeBuilder::SharedPtr +DynamicMessageTypeBuilder::clone_shared(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone( + &get_rosidl_dynamic_type_builder(), &allocator, &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str); + } + return DynamicMessageTypeBuilder::make_shared( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type_builder)); +} + + +void +DynamicMessageTypeBuilder::clear(rcl_allocator_t allocator) +{ + if (!serialization_support_) { + throw std::runtime_error( + "cannot call clear() on a dynamic type builder with uninitialized serialization support" + ); + } + + const std::string & name = get_name(); + init_from_serialization_support_(serialization_support_, name, allocator); +} + + +DynamicMessage +DynamicMessageTypeBuilder::build_dynamic_message(rcl_allocator_t allocator) +{ + return DynamicMessage(shared_from_this(), allocator); +} + + +DynamicMessage::SharedPtr +DynamicMessageTypeBuilder::build_dynamic_message_shared(rcl_allocator_t allocator) +{ + return DynamicMessage::make_shared(shared_from_this(), allocator); +} + + +DynamicMessageType +DynamicMessageTypeBuilder::build_dynamic_message_type(rcl_allocator_t allocator) +{ + return DynamicMessageType(shared_from_this(), allocator); +} + + +DynamicMessageType::SharedPtr +DynamicMessageTypeBuilder::build_dynamic_message_type_shared(rcl_allocator_t allocator) +{ + return DynamicMessageType::make_shared(shared_from_this(), allocator); +} + + +// ADD MEMBERS ===================================================================================== +// Defined in "detail/dynamic_message_type_builder_impl.hpp" + + +// ADD FIXED STRING MEMBERS ======================================================================== +void +DynamicMessageTypeBuilder::add_fixed_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_length, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_length, array_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_length, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_length, array_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_string_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_length, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_length, sequence_bound); +} + + +void +DynamicMessageTypeBuilder::add_fixed_wstring_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_length, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_length, sequence_bound); +} + + +// ADD BOUNDED STRING MEMBERS ====================================================================== +void +DynamicMessageTypeBuilder::add_bounded_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_bound, array_length); +} + + +void +DynamicMessageTypeBuilder::add_bounded_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_bound, array_length); +} + + +void +DynamicMessageTypeBuilder::add_bounded_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_string_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_bound, sequence_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_wstring_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_bound, sequence_bound); +} + + +// ADD NESTED MEMBERS ============================================================================== +void +DynamicMessageTypeBuilder::add_complex_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type.get_rosidl_dynamic_type()); +} + + +void +DynamicMessageTypeBuilder::add_complex_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type.get_rosidl_dynamic_type(), array_length); +} + + +void +DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type.get_rosidl_dynamic_type()); +} + + +void +DynamicMessageTypeBuilder::add_complex_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type.get_rosidl_dynamic_type(), sequence_bound); +} + + +void +DynamicMessageTypeBuilder::add_complex_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member_builder( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type_builder.get_rosidl_dynamic_type_builder()); +} + + +void +DynamicMessageTypeBuilder::add_complex_array_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, size_t array_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member_builder( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type_builder.get_rosidl_dynamic_type_builder(), array_length); +} + + +void +DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member_builder( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type_builder.get_rosidl_dynamic_type_builder()); +} + + +void +DynamicMessageTypeBuilder::add_complex_bounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member_builder( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type_builder.get_rosidl_dynamic_type_builder(), sequence_bound); +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp new file mode 100644 index 0000000000..b840ab52c3 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp @@ -0,0 +1,317 @@ +// Copyright 2023 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. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/dynamic_message_type_support.h" +#include "rcl/type_hash.h" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" +#include "rcutils/types/rcutils_ret.h" +#include "rmw/dynamic_message_type_support.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +// CONSTRUCTION ==================================================================================== +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + const rosidl_runtime_c__type_description__TypeDescription & description, + const std::string & serialization_library_name, + rcl_allocator_t allocator) +: serialization_support_(nullptr), + dynamic_message_type_(nullptr), + dynamic_message_(nullptr), + rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) +{ + rcl_ret_t ret; + if (serialization_library_name.empty()) { + ret = rcl_dynamic_message_type_support_handle_init( + nullptr, &description, &allocator, &rosidl_message_type_support_); + } else { + ret = rcl_dynamic_message_type_support_handle_init( + serialization_library_name.c_str(), &description, &allocator, &rosidl_message_type_support_); + } + if (ret != RCL_RET_OK) { + std::string error_msg = + std::string("error initializing rosidl message type support: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(error_msg); + } + if (rosidl_message_type_support_.typesupport_identifier != + rosidl_get_dynamic_typesupport_identifier()) + { + throw std::runtime_error("rosidl message type support is of the wrong type"); + } + + auto ts_impl = static_cast(const_cast( + rosidl_message_type_support_.data)); + + serialization_support_ = DynamicSerializationSupport::make_shared( + std::move(ts_impl->serialization_support)); + + dynamic_message_type_ = DynamicMessageType::make_shared( + get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message_type)); + + dynamic_message_ = DynamicMessage::make_shared( + get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message)); +} + +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + dynamic_message_type_(nullptr), + dynamic_message_(nullptr), + rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) +{ + // Check null + if (!serialization_support) { + throw std::runtime_error("serialization_support cannot be nullptr."); + } + + rosidl_type_hash_t type_hash; + rcutils_ret_t hash_ret = rcl_calculate_type_hash( + // TODO(methylDragon): Swap this out with the conversion function when it is ready + reinterpret_cast(&description), + &type_hash); + if (hash_ret != RCL_RET_OK) { + throw std::runtime_error("failed to get type hash"); + } + + rcutils_ret_t ret = rosidl_dynamic_message_type_support_handle_init( + &serialization_support->get_rosidl_serialization_support(), + &type_hash, // type_hash + &description, // type_description + nullptr, // type_description_sources (not implemented for dynamic types) + &allocator, + &rosidl_message_type_support_); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init rosidl message type support"); + } + if (rosidl_message_type_support_.typesupport_identifier != + rosidl_get_dynamic_typesupport_identifier()) + { + throw std::runtime_error("rosidl message type support is of the wrong type"); + } + + auto ts_impl = static_cast( + rosidl_message_type_support_.data); + + dynamic_message_type_ = DynamicMessageType::make_shared( + get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message_type)); + + dynamic_message_ = DynamicMessage::make_shared( + get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message)); +} + +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + dynamic_message_type_(dynamic_message_type), + dynamic_message_(dynamic_message), + rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) +{ + // Check null + if (!serialization_support) { + throw std::runtime_error("serialization_support cannot be nullptr."); + } + if (!dynamic_message_type) { + throw std::runtime_error("dynamic_message_type cannot be nullptr."); + } + if (!dynamic_message) { + throw std::runtime_error("dynamic_message cannot be nullptr."); + } + + // Check identifiers + if (serialization_support->get_serialization_library_identifier() != + dynamic_message_type->get_serialization_library_identifier()) + { + throw std::runtime_error( + "serialization support library identifier does not match " + "dynamic message type library identifier."); + } + if (dynamic_message_type->get_serialization_library_identifier() != + dynamic_message->get_serialization_library_identifier()) + { + throw std::runtime_error( + "dynamic message type library identifier does not match " + "dynamic message library identifier."); + } + + rosidl_type_hash_t type_hash; + rcutils_ret_t hash_ret = rcl_calculate_type_hash( + // TODO(methylDragon): Swap this out with the conversion function when it is ready + // from https://github.com/ros2/rcl/pull/1052 + reinterpret_cast(&description), + &type_hash); + if (hash_ret != RCL_RET_OK) { + std::string error_msg = std::string("failed to get type hash: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(error_msg); + } + + auto ts_impl = static_cast( + allocator.zero_allocate(1, sizeof(rosidl_dynamic_message_type_support_impl_t), allocator.state) + ); + if (!ts_impl) { + throw std::runtime_error("could not allocate rosidl_message_type_support_t"); + } + + ts_impl->allocator = allocator; + ts_impl->type_hash = type_hash; + if (!rosidl_runtime_c__type_description__TypeDescription__copy( + &description, &ts_impl->type_description)) + { + throw std::runtime_error("could not copy type description"); + } + // ts_impl->type_description_sources = // Not used + + ts_impl->serialization_support = serialization_support->get_rosidl_serialization_support(); + ts_impl->dynamic_message_type = &dynamic_message_type->get_rosidl_dynamic_type(); + ts_impl->dynamic_message = &dynamic_message->get_rosidl_dynamic_data(); + + rosidl_message_type_support_ = { + rosidl_get_dynamic_typesupport_identifier(), // typesupport_identifier + ts_impl, // data + get_message_typesupport_handle_function, // func + // get_type_hash_func + rosidl_get_dynamic_message_type_support_type_hash_function, + // get_type_description_func + rosidl_get_dynamic_message_type_support_type_description_function, + // get_type_description_sources_func + rosidl_get_dynamic_message_type_support_type_description_sources_function + }; +} + +DynamicMessageTypeSupport::~DynamicMessageTypeSupport() +{ + // These must go first + serialization_support_.reset(); + dynamic_message_type_.reset(); + dynamic_message_.reset(); + + // Early return if type support isn't populated to avoid segfaults + if (!rosidl_message_type_support_.data) { + return; + } + + // We only partially finalize the rosidl_message_type_support->data since its pointer members are + // managed by their respective SharedPtr wrapper classes. + auto ts_impl = static_cast( + const_cast(rosidl_message_type_support_.data) + ); + rcutils_allocator_t allocator = ts_impl->allocator; + + rosidl_runtime_c__type_description__TypeDescription__fini(&ts_impl->type_description); + rosidl_runtime_c__type_description__TypeSource__Sequence__fini( + &ts_impl->type_description_sources); + allocator.deallocate(static_cast(ts_impl), allocator.state); // Always C allocated +} + + +// GETTERS ========================================================================================= +const std::string +DynamicMessageTypeSupport::get_serialization_library_identifier() const +{ + return serialization_support_->get_serialization_library_identifier(); +} + +rosidl_message_type_support_t & +DynamicMessageTypeSupport::get_rosidl_message_type_support() +{ + return rosidl_message_type_support_; +} + +const rosidl_message_type_support_t & +DynamicMessageTypeSupport::get_const_rosidl_message_type_support() +{ + return rosidl_message_type_support_; +} + +const rosidl_message_type_support_t & +DynamicMessageTypeSupport::get_const_rosidl_message_type_support() const +{ + return rosidl_message_type_support_; +} + +const rosidl_runtime_c__type_description__TypeDescription & +DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() const +{ + auto ts_impl = static_cast( + get_const_rosidl_message_type_support().data + ); + return ts_impl->type_description; +} + +DynamicSerializationSupport::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + +DynamicMessageType::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message_type() +{ + return dynamic_message_type_; +} + +DynamicMessageType::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message_type() const +{ + return dynamic_message_type_; +} + +DynamicMessage::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message() +{ + return dynamic_message_; +} + +DynamicMessage::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message() const +{ + return dynamic_message_; +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp new file mode 100644 index 0000000000..7ffde1591d --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -0,0 +1,98 @@ +// Copyright 2023 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. + +#include +#include +#include +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +// CONSTRUCTION ==================================================================================== +DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator) +: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) {} + +DynamicSerializationSupport::DynamicSerializationSupport( + const std::string & serialization_library_name, + rcl_allocator_t allocator) +: rosidl_serialization_support_( + rosidl_dynamic_typesupport_get_zero_initialized_serialization_support()) +{ + rmw_ret_t ret = RMW_RET_ERROR; + + if (serialization_library_name.empty()) { + ret = rmw_serialization_support_init(NULL, &allocator, &rosidl_serialization_support_); + } else { + ret = rmw_serialization_support_init( + serialization_library_name.c_str(), &allocator, &rosidl_serialization_support_); + } + if (ret != RCL_RET_OK) { + std::string error_msg = + std::string("could not initialize new serialization support object: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(error_msg); + } +} + +DynamicSerializationSupport::DynamicSerializationSupport( + rosidl_dynamic_typesupport_serialization_support_t && rosidl_serialization_support) +: rosidl_serialization_support_(std::move(rosidl_serialization_support)) {} + +DynamicSerializationSupport::DynamicSerializationSupport( + DynamicSerializationSupport && other) noexcept +: rosidl_serialization_support_(std::exchange( + other.rosidl_serialization_support_, + rosidl_dynamic_typesupport_get_zero_initialized_serialization_support())) {} + +DynamicSerializationSupport & +DynamicSerializationSupport::operator=(DynamicSerializationSupport && other) noexcept +{ + std::swap(rosidl_serialization_support_, other.rosidl_serialization_support_); + return *this; +} + +DynamicSerializationSupport::~DynamicSerializationSupport() +{ + rosidl_dynamic_typesupport_serialization_support_fini(&rosidl_serialization_support_); +} + + +// GETTERS ========================================================================================= +const std::string +DynamicSerializationSupport::get_serialization_library_identifier() const +{ + return std::string( + rosidl_dynamic_typesupport_serialization_support_get_library_identifier( + &rosidl_serialization_support_)); +} + +rosidl_dynamic_typesupport_serialization_support_t & +DynamicSerializationSupport::get_rosidl_serialization_support() +{ + return rosidl_serialization_support_; +} + +const rosidl_dynamic_typesupport_serialization_support_t & +DynamicSerializationSupport::get_rosidl_serialization_support() const +{ + return rosidl_serialization_support_; +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index ecfc89a6aa..e8f873f693 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -28,6 +28,11 @@ #include "../../mocking_utils/patch.hpp" #include "../../utils/rclcpp_gtest_macros.hpp" +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + + namespace { @@ -77,6 +82,18 @@ class TestSubscription : public rclcpp::SubscriptionBase const std::shared_ptr &, const rclcpp::MessageInfo &) override {} void return_message(std::shared_ptr &) override {} void return_serialized_message(std::shared_ptr &) override {} + + DynamicMessageType::SharedPtr get_shared_dynamic_message_type() override {return nullptr;} + DynamicMessage::SharedPtr get_shared_dynamic_message() override {return nullptr;} + DynamicSerializationSupport::SharedPtr get_shared_dynamic_serialization_support() override + { + return nullptr; + } + DynamicMessage::SharedPtr create_dynamic_message() override {return nullptr;} + void return_dynamic_message(DynamicMessage::SharedPtr &) override {} + void handle_dynamic_message( + const DynamicMessage::SharedPtr &, + const rclcpp::MessageInfo &) override {} }; class TestNodeTopics : public ::testing::Test diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index f4cef0b757..de50be4fe2 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -89,9 +89,9 @@ class RclcppGenericNodeFixture : public Test T2 message; write_message(data, message); - rclcpp::Serialization ser; + rclcpp::Serialization serialization_support; SerializedMessage result; - ser.serialize_message(&message, &result); + serialization_support.serialize_message(&message, &result); return result; } From ee1964fa022525496b40d50734261e658c04f337 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 11 Apr 2023 15:45:37 -0700 Subject: [PATCH 3/6] Execute the stubbing Signed-off-by: methylDragon --- .../detail/dynamic_message_impl.hpp | 327 -------- .../dynamic_message_type_builder_impl.hpp | 189 ----- .../dynamic_typesupport/dynamic_message.hpp | 361 +-------- .../dynamic_message_type.hpp | 132 +--- .../dynamic_message_type_builder.hpp | 343 +------- .../dynamic_message_type_support.hpp | 113 +-- .../dynamic_serialization_support.hpp | 33 +- rclcpp/include/rclcpp/rclcpp.hpp | 5 - rclcpp/include/rclcpp/subscription_base.hpp | 5 + .../dynamic_typesupport/dynamic_message.cpp | 731 +----------------- .../dynamic_message_type.cpp | 245 +----- .../dynamic_message_type_builder.cpp | 579 +------------- .../dynamic_message_type_support.cpp | 270 +------ .../dynamic_serialization_support.cpp | 63 +- rclcpp/src/rclcpp/executor.cpp | 41 +- rclcpp/src/rclcpp/subscription_base.cpp | 23 +- 16 files changed, 37 insertions(+), 3423 deletions(-) delete mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp delete mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp deleted file mode 100644 index 5ae6827692..0000000000 --- a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp +++ /dev/null @@ -1,327 +0,0 @@ -// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ -#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ - -#include -#include - -#include -#include -#include -#include - -#include "rclcpp/exceptions.hpp" - -#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ -#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" -#endif - - -#define __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \ - template<> \ - ValueT \ - DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) \ - { \ - ValueT out; \ - rosidl_dynamic_typesupport_dynamic_data_get_ ## FunctionT ## _value( \ - &rosidl_dynamic_data_, id, &out); \ - return out; \ - } - -#define __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ - template<> \ - ValueT \ - DynamicMessage::get_value(const std::string & name) \ - { \ - return get_value(get_member_id(name)); \ - } - -#define __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \ - template<> \ - void \ - DynamicMessage::set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value) \ - { \ - rosidl_dynamic_typesupport_dynamic_data_set_ ## FunctionT ## _value( \ - &rosidl_dynamic_data_, id, value); \ - } - -#define __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ - template<> \ - void \ - DynamicMessage::set_value(const std::string & name, ValueT value) \ - { \ - set_value(get_member_id(name), value); \ - } - -#define __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) \ - template<> \ - rosidl_dynamic_typesupport_member_id_t \ - DynamicMessage::insert_value(ValueT value) \ - { \ - rosidl_dynamic_typesupport_member_id_t out; \ - rosidl_dynamic_typesupport_dynamic_data_insert_ ## FunctionT ## _value( \ - &rosidl_dynamic_data_, value, &out); \ - return out; \ - } - -#define DYNAMIC_MESSAGE_DEFINITIONS(ValueT, FunctionT) \ - __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \ - __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ - __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \ - __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ - __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) - - -namespace rclcpp -{ -namespace dynamic_typesupport -{ - -/** - * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: - * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html - * - * Explicitly: - * - Basic types: bool, byte, char - * - Float types: float, double - * - Int types: int8_t, int16_t, int32_t, int64_t - * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t - * - String types: std::string, std::u16string - */ - -DYNAMIC_MESSAGE_DEFINITIONS(bool, bool); -// DYNAMIC_MESSAGE_DEFINITIONS(std::byte, byte); -DYNAMIC_MESSAGE_DEFINITIONS(char, char); -DYNAMIC_MESSAGE_DEFINITIONS(float, float32); -DYNAMIC_MESSAGE_DEFINITIONS(double, float64); -DYNAMIC_MESSAGE_DEFINITIONS(int8_t, int8); -DYNAMIC_MESSAGE_DEFINITIONS(int16_t, int16); -DYNAMIC_MESSAGE_DEFINITIONS(int32_t, int32); -DYNAMIC_MESSAGE_DEFINITIONS(int64_t, int64); -DYNAMIC_MESSAGE_DEFINITIONS(uint8_t, uint8); -DYNAMIC_MESSAGE_DEFINITIONS(uint16_t, uint16); -DYNAMIC_MESSAGE_DEFINITIONS(uint32_t, uint32); -DYNAMIC_MESSAGE_DEFINITIONS(uint64_t, uint64); -// DYNAMIC_MESSAGE_DEFINITIONS(std::string, std::string); -// DYNAMIC_MESSAGE_DEFINITIONS(std::u16string, std::u16string); - -// Byte and String getters have a different implementation and are defined below - - -// BYTE ============================================================================================ -template<> -std::byte -DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) -{ - unsigned char out; - rosidl_dynamic_typesupport_dynamic_data_get_byte_value(&get_rosidl_dynamic_data(), id, &out); - return static_cast(out); -} - - -template<> -std::byte -DynamicMessage::get_value(const std::string & name) -{ - return get_value(get_member_id(name)); -} - - -template<> -void -DynamicMessage::set_value( - rosidl_dynamic_typesupport_member_id_t id, const std::byte value) -{ - rosidl_dynamic_typesupport_dynamic_data_set_byte_value( - &rosidl_dynamic_data_, id, static_cast(value)); -} - - -template<> -void -DynamicMessage::set_value(const std::string & name, const std::byte value) -{ - set_value(get_member_id(name), value); -} - - -template<> -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_value(const std::byte value) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_byte_value( - &rosidl_dynamic_data_, static_cast(value), &out); - return out; -} - - -// STRINGS ========================================================================================= -template<> -std::string -DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) -{ - size_t buf_length; - char * buf = nullptr; - rosidl_dynamic_typesupport_dynamic_data_get_string_value( - &get_rosidl_dynamic_data(), id, &buf, &buf_length); - auto out = std::string(buf, buf_length); - delete buf; - return out; -} - - -template<> -std::u16string -DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) -{ - size_t buf_length; - char16_t * buf = nullptr; - rosidl_dynamic_typesupport_dynamic_data_get_wstring_value( - &get_rosidl_dynamic_data(), id, &buf, &buf_length); - auto out = std::u16string(buf, buf_length); - delete buf; - return out; -} - - -template<> -std::string -DynamicMessage::get_value(const std::string & name) -{ - return get_value(get_member_id(name)); -} - - -template<> -std::u16string -DynamicMessage::get_value(const std::string & name) -{ - return get_value(get_member_id(name)); -} - - -template<> -void -DynamicMessage::set_value( - rosidl_dynamic_typesupport_member_id_t id, const std::string value) -{ - rosidl_dynamic_typesupport_dynamic_data_set_string_value( - &rosidl_dynamic_data_, id, value.c_str(), value.size()); -} - - -template<> -void -DynamicMessage::set_value( - rosidl_dynamic_typesupport_member_id_t id, const std::u16string value) -{ - rosidl_dynamic_typesupport_dynamic_data_set_wstring_value( - &rosidl_dynamic_data_, id, value.c_str(), value.size()); -} - - -template<> -void -DynamicMessage::set_value(const std::string & name, const std::string value) -{ - set_value(get_member_id(name), value); -} - - -template<> -void -DynamicMessage::set_value(const std::string & name, const std::u16string value) -{ - set_value(get_member_id(name), value); -} - - -template<> -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_value(const std::string value) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_string_value( - &rosidl_dynamic_data_, value.c_str(), value.size(), &out); - return out; -} - - -template<> -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_value(const std::u16string value) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_wstring_value( - &rosidl_dynamic_data_, value.c_str(), value.size(), &out); - return out; -} - - -// THROW FOR UNSUPPORTED TYPES ===================================================================== -template -ValueT -DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) -{ - throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type"); -} - - -template -ValueT -DynamicMessage::get_value(const std::string & name) -{ - throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type"); -} - - -template -void -DynamicMessage::set_value( - rosidl_dynamic_typesupport_member_id_t id, ValueT value) -{ - throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type"); -} - - -template -void -DynamicMessage::set_value(const std::string & name, ValueT value) -{ - throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type"); -} - - -template -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_value(ValueT value) -{ - throw rclcpp::exceptions::UnimplementedError("insert_value is not implemented for input type"); -} - - -} // namespace dynamic_typesupport -} // namespace rclcpp - -#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN -#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN -#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN -#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN -#undef __DYNAMIC_MESSAGE_INSERT_VALUE -#undef DYNAMIC_MESSAGE_DEFINITIONS - -#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp deleted file mode 100644 index 60188146e1..0000000000 --- a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp +++ /dev/null @@ -1,189 +0,0 @@ -// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ -#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ - -#include -#include - -#include -#include -#include -#include - -#include "rclcpp/exceptions.hpp" - -#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ -#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" -#endif - - -#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \ - template<> \ - void \ - DynamicMessageTypeBuilder::add_member( \ - rosidl_dynamic_typesupport_member_id_t id, \ - const std::string & name, \ - const std::string & default_value) \ - { \ - rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _member( \ - &rosidl_dynamic_type_builder_, \ - id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \ - } - -#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \ - template<> \ - void \ - DynamicMessageTypeBuilder::add_array_member( \ - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, \ - size_t array_length, \ - const std::string & default_value) \ - { \ - rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _array_member( \ - &rosidl_dynamic_type_builder_, \ - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \ - array_length); \ - } - -#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ - template<> \ - void \ - DynamicMessageTypeBuilder::add_unbounded_sequence_member( \ - rosidl_dynamic_typesupport_member_id_t id, \ - const std::string & name, \ - const std::string & default_value) \ - { \ - rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## \ - _unbounded_sequence_member( \ - &rosidl_dynamic_type_builder_, \ - id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \ - } - -#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ - template<> \ - void \ - DynamicMessageTypeBuilder::add_bounded_sequence_member( \ - rosidl_dynamic_typesupport_member_id_t id, \ - const std::string & name, \ - size_t sequence_bound, \ - const std::string & default_value) \ - { \ - rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _bounded_sequence_member( \ - &rosidl_dynamic_type_builder_, \ - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \ - sequence_bound); \ - } - -#define DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(MemberT, FunctionT) \ - __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \ - __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \ - __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ - __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ - - -namespace rclcpp -{ -namespace dynamic_typesupport -{ - -/** - * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: - * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html - * - * Explicitly: - * - Basic types: bool, byte, char - * - Float types: float, double - * - Int types: int8_t, int16_t, int32_t, int64_t - * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t - * - String types: std::string, std::u16string - */ - -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(bool, bool); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::byte, byte); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(char, char); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(float, float32); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(double, float64); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int8_t, int8); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int16_t, int16); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int32_t, int32); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int64_t, int64); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint8_t, uint8); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint16_t, uint16); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint32_t, uint32); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint64_t, uint64); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::string, string); -DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::u16string, wstring); - - -// THROW FOR UNSUPPORTED TYPES ===================================================================== -template -void -DynamicMessageTypeBuilder::add_member( - rosidl_dynamic_typesupport_member_id_t id, - const std::string & name, - const std::string & default_value) -{ - throw rclcpp::exceptions::UnimplementedError( - "add_member is not implemented for input type"); -} - - -template -void -DynamicMessageTypeBuilder::add_array_member( - rosidl_dynamic_typesupport_member_id_t id, - const std::string & name, - size_t array_length, const std::string & default_value) -{ - throw rclcpp::exceptions::UnimplementedError( - "add_array_member is not implemented for input type"); -} - - -template -void -DynamicMessageTypeBuilder::add_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, - const std::string & name, - const std::string & default_value) -{ - throw rclcpp::exceptions::UnimplementedError( - "add_unbounded_sequence_member is not implemented for input type"); -} - - -template -void -DynamicMessageTypeBuilder::add_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, - const std::string & name, - size_t sequence_bound, - const std::string & default_value) -{ - throw rclcpp::exceptions::UnimplementedError( - "add_bounded_sequence_member is not implemented for input type"); -} - - -} // namespace dynamic_typesupport -} // namespace rclcpp - -#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN -#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN -#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN -#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN -#undef DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS - -#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp index fef984a1e0..284d5a2f4b 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp @@ -36,368 +36,15 @@ class DynamicMessageType; class DynamicMessageTypeBuilder; /// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t -/** - * This class: - * - Exposes getter methods for the struct - * - Exposes the underlying serialization support API - * - * Ownership: - * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed - * DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport. - * - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer - * must point to the same location in memory as the stored raw pointer! - * - * Note: This class is meant to map to the lower level rosidl_dynamic_typesupport_dynamic_data_t, - * even though rosidl_dynamic_typesupport_dynamic_data_t is equivalent to - * rosidl_dynamic_typesupport_dynamic_data_t, exposing the fundamental methods available to - * rosidl_dynamic_typesupport_dynamic_data_t, allowing the user to access the data's fields. - */ +/// STUBBED OUT class DynamicMessage : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage) - // CONSTRUCTION ================================================================================== - // Most constructors require a passed in DynamicSerializationSupport::SharedPtr, to extend the - // lifetime of the serialization support (if the constructor cannot otherwise get it from args). - // - // In cases where a dynamic data pointer is passed, the serialization support composed by - // the data should be the exact same object managed by the DynamicSerializationSupport, - // otherwise the lifetime management will not work properly. - - /// Construct a new DynamicMessage with the provided dynamic type builder, using its allocator - RCLCPP_PUBLIC - explicit DynamicMessage(std::shared_ptr dynamic_type_builder); - - /// Construct a new DynamicMessage with the provided dynamic type builder and allocator - RCLCPP_PUBLIC - DynamicMessage( - std::shared_ptr dynamic_type_builder, - rcl_allocator_t allocator); - - /// Construct a new DynamicMessage with the provided dynamic type, using its allocator - RCLCPP_PUBLIC - explicit DynamicMessage(std::shared_ptr dynamic_type); - - /// Construct a new DynamicMessage with the provided dynamic type and allocator - RCLCPP_PUBLIC - DynamicMessage( - std::shared_ptr dynamic_type, - rcl_allocator_t allocator); - - /// Assume ownership of struct - RCLCPP_PUBLIC - DynamicMessage( - DynamicSerializationSupport::SharedPtr serialization_support, - rosidl_dynamic_typesupport_dynamic_data_t && rosidl_dynamic_data); - - /// Loaning constructor - /// Must only be called with a rosidl dynaimc data object obtained from loaning! - // NOTE(methylDragon): I'd put this in protected, but I need this exposed to - // enable_shared_from_this... - RCLCPP_PUBLIC - DynamicMessage( - DynamicMessage::SharedPtr parent_data, - rosidl_dynamic_typesupport_dynamic_data_t && rosidl_loaned_data); - - // NOTE(methylDragon): Deliberately no constructor from description to nudge users towards using - // construction from dynamic type/builder, which is more efficient - - /// Move constructor - RCLCPP_PUBLIC - DynamicMessage(DynamicMessage && other) noexcept; - - /// Move assignment - RCLCPP_PUBLIC - DynamicMessage & operator=(DynamicMessage && other) noexcept; - RCLCPP_PUBLIC virtual ~DynamicMessage(); - - // GETTERS ======================================================================================= - RCLCPP_PUBLIC - const std::string - get_serialization_library_identifier() const; - - RCLCPP_PUBLIC - const std::string - get_name() const; - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_dynamic_data_t & - get_rosidl_dynamic_data(); - - RCLCPP_PUBLIC - const rosidl_dynamic_typesupport_dynamic_data_t & - get_rosidl_dynamic_data() const; - - RCLCPP_PUBLIC - DynamicSerializationSupport::SharedPtr - get_shared_dynamic_serialization_support(); - - RCLCPP_PUBLIC - DynamicSerializationSupport::ConstSharedPtr - get_shared_dynamic_serialization_support() const; - - RCLCPP_PUBLIC - size_t - get_item_count() const; - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - get_member_id(size_t index) const; - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - get_member_id(const std::string & name) const; - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - get_array_index(size_t index) const; - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - get_array_index(const std::string & name) const; - - - // METHODS ======================================================================================= - RCLCPP_PUBLIC - DynamicMessage - clone(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessage::SharedPtr - clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessage - init_from_type( - DynamicMessageType & type, rcl_allocator_t allocator = rcl_get_default_allocator()) const; - - RCLCPP_PUBLIC - DynamicMessage::SharedPtr - init_from_type_shared( - DynamicMessageType & type, rcl_allocator_t allocator = rcl_get_default_allocator()) const; - - RCLCPP_PUBLIC - bool - equals(const DynamicMessage & other) const; - - RCLCPP_PUBLIC - DynamicMessage::SharedPtr - loan_value( - rosidl_dynamic_typesupport_member_id_t id, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessage::SharedPtr - loan_value( - const std::string & name, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - void - clear_all_values(); - - RCLCPP_PUBLIC - void - clear_nonkey_values(); - - RCLCPP_PUBLIC - void - clear_value(rosidl_dynamic_typesupport_member_id_t id); - - RCLCPP_PUBLIC - void - clear_value(const std::string & name); - - RCLCPP_PUBLIC - void - clear_sequence(); - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - insert_sequence_data(); - - RCLCPP_PUBLIC - void - remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index); - - RCLCPP_PUBLIC - bool - serialize(rcl_serialized_message_t & buffer); - - RCLCPP_PUBLIC - bool - deserialize(rcl_serialized_message_t & buffer); - - - // MEMBER ACCESS TEMPLATES ======================================================================= - /** - * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: - * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html - * - * Explicitly: - * - Basic types: bool, byte, char - * - Float types: float, double - * - Int types: int8_t, int16_t, int32_t, int64_t - * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t - * - String types: std::string, std::u16string - */ - - template - ValueT - get_value(rosidl_dynamic_typesupport_member_id_t id); - - template - ValueT - get_value(const std::string & name); - - template - void - set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value); - - template - void - set_value(const std::string & name, ValueT value); - - template - rosidl_dynamic_typesupport_member_id_t - insert_value(ValueT value); - - - // FIXED STRING MEMBER ACCESS ==================================================================== - RCLCPP_PUBLIC - const std::string - get_fixed_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_length); - - RCLCPP_PUBLIC - const std::string - get_fixed_string_value(const std::string & name, size_t string_length); - - RCLCPP_PUBLIC - const std::u16string - get_fixed_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length); - - RCLCPP_PUBLIC - const std::u16string - get_fixed_wstring_value(const std::string & name, size_t wstring_length); - - RCLCPP_PUBLIC - void - set_fixed_string_value( - rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length); - - RCLCPP_PUBLIC - void - set_fixed_string_value(const std::string & name, const std::string value, size_t string_length); - - RCLCPP_PUBLIC - void - set_fixed_wstring_value( - rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length); - - RCLCPP_PUBLIC - void - set_fixed_wstring_value( - const std::string & name, const std::u16string value, size_t wstring_length); - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - insert_fixed_string_value(const std::string value, size_t string_length); - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - insert_fixed_wstring_value(const std::u16string value, size_t wstring_length); - - - // BOUNDED STRING MEMBER ACCESS ================================================================== - RCLCPP_PUBLIC - const std::string - get_bounded_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_bound); - - RCLCPP_PUBLIC - const std::string - get_bounded_string_value(const std::string & name, size_t string_bound); - - RCLCPP_PUBLIC - const std::u16string - get_bounded_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound); - - RCLCPP_PUBLIC - const std::u16string - get_bounded_wstring_value(const std::string & name, size_t wstring_bound); - - RCLCPP_PUBLIC - void - set_bounded_string_value( - rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound); - - RCLCPP_PUBLIC - void - set_bounded_string_value(const std::string & name, const std::string value, size_t string_bound); - - RCLCPP_PUBLIC - void - set_bounded_wstring_value( - rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound); - - RCLCPP_PUBLIC - void - set_bounded_wstring_value( - const std::string & name, const std::u16string value, size_t wstring_bound); - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - insert_bounded_string_value(const std::string value, size_t string_bound); - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound); - - - // NESTED MEMBER ACCESS ========================================================================== - RCLCPP_PUBLIC - DynamicMessage - get_complex_value( - rosidl_dynamic_typesupport_member_id_t id, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessage - get_complex_value( - const std::string & name, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessage::SharedPtr - get_complex_value_shared( - rosidl_dynamic_typesupport_member_id_t id, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessage::SharedPtr - get_complex_value_shared( - const std::string & name, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - void - set_complex_value(rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value); - - RCLCPP_PUBLIC - void - set_complex_value(const std::string & name, DynamicMessage & value); - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - insert_complex_value_copy(const DynamicMessage & value); - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_member_id_t - insert_complex_value(DynamicMessage & value); - protected: // NOTE(methylDragon): // This is just here to extend the lifetime of the serialization support @@ -418,12 +65,6 @@ class DynamicMessage : public std::enable_shared_from_this RCLCPP_PUBLIC DynamicMessage(); - - RCLCPP_PUBLIC - bool - match_serialization_support_( - const DynamicSerializationSupport & serialization_support, - const rosidl_dynamic_typesupport_dynamic_data_t & dynamic_data); }; } // namespace dynamic_typesupport diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp index beaaee08a5..693e7ccf08 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp @@ -34,139 +34,15 @@ class DynamicMessage; class DynamicMessageTypeBuilder; /// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t` -/** - * This class: - * - Exposes getter methods for the struct - * - Exposes the underlying serialization support API - * - * Ownership: - * - This class borrows the `rosidl_dynamic_typesupport_serialization_support_t` stored in the - * passed `DynamicSerializationSupport`. - * So it cannot outlive the `DynamicSerializationSupport`. - * - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t` - * pointer must point to the same location in memory as the stored raw pointer! - * - * This class is meant to map to the lower level `rosidl_dynamic_typesupport_dynamic_type_t`, - * which can be constructed via `DynamicMessageTypeBuilder`, which maps to - * `rosidl_dynamic_typesupport_dynamic_type_builder_t`. - * - * The usual method of obtaining a `DynamicMessageType` is through construction of - * `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then - * taking ownership of its contents. But `DynamicMessageTypeBuilder` can also be used to obtain - * `DynamicMessageType` by constructing it bottom-up instead, since it exposes the lower_level - * rosidl methods. - */ +/// STUBBED OUT class DynamicMessageType : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType) - // CONSTRUCTION ================================================================================== - // Most constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the - // lifetime of the serialization support (if the constructor cannot otherwise get it from args). - // - // In cases where a dynamic type pointer is passed, the serialization support composed by - // the type should be the exact same object managed by the `DynamicSerializationSupport`, - // otherwise the lifetime management will not work properly. - - /// Construct a new `DynamicMessageType` with the provided dynamic type builder, - /// using its allocator - RCLCPP_PUBLIC - explicit DynamicMessageType(std::shared_ptr dynamic_type_builder); - - /// Construct a new `DynamicMessageType` with the provided dynamic type builder and allocator - RCLCPP_PUBLIC - DynamicMessageType( - std::shared_ptr dynamic_type_builder, - rcl_allocator_t allocator); - - /// Assume ownership of struct - RCLCPP_PUBLIC - DynamicMessageType( - DynamicSerializationSupport::SharedPtr serialization_support, - rosidl_dynamic_typesupport_dynamic_type_t && rosidl_dynamic_type); - - /// From description - RCLCPP_PUBLIC - DynamicMessageType( - DynamicSerializationSupport::SharedPtr serialization_support, - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - /// Move constructor - RCLCPP_PUBLIC - DynamicMessageType(DynamicMessageType && other) noexcept; - - /// Move assignment - RCLCPP_PUBLIC - DynamicMessageType & operator=(DynamicMessageType && other) noexcept; - RCLCPP_PUBLIC virtual ~DynamicMessageType(); - /// Swaps the serialization support if `serialization_support` is populated - /** - * The user can call this with another description to reconfigure the type without changing the - * serialization support - */ - RCLCPP_PUBLIC - void - init_from_description( - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator = rcl_get_default_allocator(), - DynamicSerializationSupport::SharedPtr serialization_support = nullptr); - - // GETTERS ======================================================================================= - RCLCPP_PUBLIC - const std::string - get_serialization_library_identifier() const; - - RCLCPP_PUBLIC - const std::string - get_name() const; - - RCLCPP_PUBLIC - size_t - get_member_count() const; - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_dynamic_type_t & - get_rosidl_dynamic_type(); - - RCLCPP_PUBLIC - const rosidl_dynamic_typesupport_dynamic_type_t & - get_rosidl_dynamic_type() const; - - RCLCPP_PUBLIC - DynamicSerializationSupport::SharedPtr - get_shared_dynamic_serialization_support(); - - RCLCPP_PUBLIC - DynamicSerializationSupport::ConstSharedPtr - get_shared_dynamic_serialization_support() const; - - - // METHODS ======================================================================================= - RCLCPP_PUBLIC - DynamicMessageType - clone(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessageType::SharedPtr - clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - bool - equals(const DynamicMessageType & other) const; - - RCLCPP_PUBLIC - DynamicMessage - build_dynamic_message(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - std::shared_ptr - build_dynamic_message_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); - protected: // NOTE(methylDragon): // This is just here to extend the lifetime of the serialization support @@ -183,12 +59,6 @@ class DynamicMessageType : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder) - // CONSTRUCTION ================================================================================== - // All constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the - // lifetime of the serialization support. - // - // In cases where a dynamic type builder pointer is passed, the serialization support composed by - // the builder should be the exact same object managed by the `DynamicSerializationSupport`, - // otherwise the lifetime management will not work properly. - - /// Construct a new `DynamicMessageTypeBuilder` with the provided serialization support using its - /// allocator - RCLCPP_PUBLIC - DynamicMessageTypeBuilder( - DynamicSerializationSupport::SharedPtr serialization_support, - const std::string & name); - - /// Construct a new `DynamicMessageTypeBuilder` with the provided serialization support and - /// allocator - RCLCPP_PUBLIC - DynamicMessageTypeBuilder( - DynamicSerializationSupport::SharedPtr serialization_support, - const std::string & name, - rcl_allocator_t allocator); - - /// Assume ownership of struct - RCLCPP_PUBLIC - DynamicMessageTypeBuilder( - DynamicSerializationSupport::SharedPtr serialization_support, - rosidl_dynamic_typesupport_dynamic_type_builder_t && dynamic_type_builder); - - /// From description - RCLCPP_PUBLIC - DynamicMessageTypeBuilder( - DynamicSerializationSupport::SharedPtr serialization_support, - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - /// Move constructor - RCLCPP_PUBLIC - DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept; - - /// Move assignment - RCLCPP_PUBLIC - DynamicMessageTypeBuilder & operator=(DynamicMessageTypeBuilder && other) noexcept; - RCLCPP_PUBLIC virtual ~DynamicMessageTypeBuilder(); - /// Swaps the serialization support if serialization_support is populated - /** - * The user can call this with another description to reconfigure the type without changing the - * serialization support - */ - RCLCPP_PUBLIC - void - init_from_description( - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator = rcl_get_default_allocator(), - DynamicSerializationSupport::SharedPtr serialization_support = nullptr); - - - // GETTERS ======================================================================================= - RCLCPP_PUBLIC - const std::string - get_serialization_library_identifier() const; - - RCLCPP_PUBLIC - const std::string - get_name() const; - - RCLCPP_PUBLIC - rosidl_dynamic_typesupport_dynamic_type_builder_t & - get_rosidl_dynamic_type_builder(); - - RCLCPP_PUBLIC - const rosidl_dynamic_typesupport_dynamic_type_builder_t & - get_rosidl_dynamic_type_builder() const; - - RCLCPP_PUBLIC - DynamicSerializationSupport::SharedPtr - get_shared_dynamic_serialization_support(); - - RCLCPP_PUBLIC - DynamicSerializationSupport::ConstSharedPtr - get_shared_dynamic_serialization_support() const; - - - // METHODS ======================================================================================= - RCLCPP_PUBLIC - void - set_name(const std::string & name); - - RCLCPP_PUBLIC - DynamicMessageTypeBuilder - clone(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessageTypeBuilder::SharedPtr - clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - void - clear(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessage - build_dynamic_message(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessage::SharedPtr - build_dynamic_message_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessageType - build_dynamic_message_type(rcl_allocator_t allocator = rcl_get_default_allocator()); - - RCLCPP_PUBLIC - DynamicMessageType::SharedPtr - build_dynamic_message_type_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); - - - // ADD MEMBERS TEMPLATES ========================================================================= - /** - * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: - * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html - * - * Explicitly: - * - Basic types: bool, byte, char - * - Float types: float, double - * - Int types: int8_t, int16_t, int32_t, int64_t - * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t - * - String types: std::string, std::u16string - */ - - template - void - add_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - const std::string & default_value = ""); - - template - void - add_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length, - const std::string & default_value = ""); - - template - void - add_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - const std::string & default_value = ""); - - template - void - add_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound, - const std::string & default_value = ""); - - - // ADD FIXED STRING MEMBERS ====================================================================== - RCLCPP_PUBLIC - void - add_fixed_string_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_fixed_wstring_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_fixed_string_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t string_length, size_t array_length, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_fixed_wstring_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t wstring_length, size_t array_length, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_fixed_string_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_fixed_wstring_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_fixed_string_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t string_length, size_t sequence_bound, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_fixed_wstring_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t wstring_length, size_t sequence_bound, const std::string & default_value = ""); - - - // ADD BOUNDED STRING MEMBERS ==================================================================== - RCLCPP_PUBLIC - void - add_bounded_string_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_bounded_wstring_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_bounded_string_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t string_bound, size_t array_length, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_bounded_wstring_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t wstring_bound, size_t array_length, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_bounded_string_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_bounded_wstring_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_bounded_string_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t string_bound, size_t sequence_bound, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_bounded_wstring_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t wstring_bound, size_t sequence_bound, const std::string & default_value = ""); - - - // ADD NESTED MEMBERS ============================================================================ - RCLCPP_PUBLIC - void - add_complex_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageType & nested_type, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_complex_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageType & nested_type, size_t array_length, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_complex_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageType & nested_type, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_complex_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageType & nested_type, size_t sequence_bound, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_complex_member_builder( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_complex_array_member_builder( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageTypeBuilder & nested_type_builder, size_t array_length, - const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_complex_unbounded_sequence_member_builder( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = ""); - - RCLCPP_PUBLIC - void - add_complex_bounded_sequence_member_builder( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound, - const std::string & default_value = ""); - protected: // NOTE(methylDragon): // This is just here to extend the lifetime of the serialization support @@ -387,19 +59,6 @@ class DynamicMessageTypeBuilder : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport) - // CONSTRUCTION ================================================================================== - /// From description - /// Does NOT take ownership of the description (copies instead.) - /// Constructs type support top-down (calling `rcl_dynamic_message_type_support_handle_create()`) - RCLCPP_PUBLIC - DynamicMessageTypeSupport( - const rosidl_runtime_c__type_description__TypeDescription & description, - const std::string & serialization_library_name = "", - rcl_allocator_t allocator = rcl_get_default_allocator()); - - /// From description, for provided serialization support - /// Does NOT take ownership of the description (copies instead.) - /// Constructs type support top-down (calling `rosidl_dynamic_message_type_support_handle_init()`) - RCLCPP_PUBLIC - DynamicMessageTypeSupport( - DynamicSerializationSupport::SharedPtr serialization_support, - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator = rcl_get_default_allocator()); - - /// Assume ownership of managed types - /// Does NOT take ownership of the description (copies instead.) - /// - /// The serialization support used to construct all managed SharedPtrs must match. - /// The structure of the provided `description` must match the `dynamic_message_type` - /// The structure of the provided `dynamic_message_type` must match the `dynamic_message - /// - /// In this case, the user would have constructed the type support bototm-up (by creating the - /// respective dynamic members.) - RCLCPP_PUBLIC - DynamicMessageTypeSupport( - DynamicSerializationSupport::SharedPtr serialization_support, - DynamicMessageType::SharedPtr dynamic_message_type, - DynamicMessage::SharedPtr dynamic_message, - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator = rcl_get_default_allocator()); - RCLCPP_PUBLIC virtual ~DynamicMessageTypeSupport(); - - // GETTERS ======================================================================================= - RCLCPP_PUBLIC - const std::string - get_serialization_library_identifier() const; - - RCLCPP_PUBLIC - const rosidl_message_type_support_t & - get_const_rosidl_message_type_support(); - - RCLCPP_PUBLIC - const rosidl_message_type_support_t & - get_const_rosidl_message_type_support() const; - - RCLCPP_PUBLIC - const rosidl_runtime_c__type_description__TypeDescription & - get_rosidl_runtime_c_type_description() const; - - RCLCPP_PUBLIC - DynamicSerializationSupport::SharedPtr - get_shared_dynamic_serialization_support(); - - RCLCPP_PUBLIC - DynamicSerializationSupport::ConstSharedPtr - get_shared_dynamic_serialization_support() const; - - RCLCPP_PUBLIC - DynamicMessageType::SharedPtr - get_shared_dynamic_message_type(); - - RCLCPP_PUBLIC - DynamicMessageType::ConstSharedPtr - get_shared_dynamic_message_type() const; - - RCLCPP_PUBLIC - DynamicMessage::SharedPtr - get_shared_dynamic_message(); - - RCLCPP_PUBLIC - DynamicMessage::ConstSharedPtr - get_shared_dynamic_message() const; - protected: - RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport) - - RCLCPP_PUBLIC - rosidl_message_type_support_t & - get_rosidl_message_type_support(); - -private: - RCLCPP_PUBLIC - DynamicMessageTypeSupport(); - DynamicSerializationSupport::SharedPtr serialization_support_; DynamicMessageType::SharedPtr dynamic_message_type_; DynamicMessage::SharedPtr dynamic_message_; rosidl_message_type_support_t rosidl_message_type_support_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport) + + RCLCPP_PUBLIC + DynamicMessageTypeSupport(); }; } // namespace dynamic_typesupport diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp index 1aae3cff5a..22f5a900c0 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp @@ -45,51 +45,22 @@ class DynamicSerializationSupport : public std::enable_shared_from_this const rosidl_message_type_support_t & get_message_type_support_handle() const; + /// DEPRECATED: Check if subscription takes and handles serialized messages + RCLCPP_PUBLIC + bool + is_serialized() const; + /// Return the type of the subscription. /** * \return `SubscriptionType`, which adjusts how messages are received and delivered. diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp index 97e701061f..c340b107b6 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp @@ -36,734 +36,5 @@ using rclcpp::dynamic_typesupport::DynamicMessageType; using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; using rclcpp::dynamic_typesupport::DynamicSerializationSupport; -#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ -// Template specialization implementations -#include "rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp" -#endif - - -// CONSTRUCTION ================================================================================== -DynamicMessage::DynamicMessage(const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder) -: DynamicMessage::DynamicMessage( - dynamic_type_builder, dynamic_type_builder->get_rosidl_dynamic_type_builder().allocator) {} - - -DynamicMessage::DynamicMessage( - const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder, - rcl_allocator_t allocator) -: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), - rosidl_dynamic_data_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data()), - is_loaned_(false), - parent_data_(nullptr) -{ - if (!serialization_support_) { - throw std::runtime_error("dynamic message could not bind serialization support!"); - } - if (!dynamic_type_builder) { - throw std::runtime_error("dynamic message type builder cannot be nullptr!"); - } - - rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = - dynamic_type_builder->get_rosidl_dynamic_type_builder(); - - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type_builder( - &rosidl_dynamic_type_builder, &allocator, &get_rosidl_dynamic_data()); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not init new dynamic data object from dynamic type builder"); - } -} - - -DynamicMessage::DynamicMessage(const DynamicMessageType::SharedPtr dynamic_type) -: DynamicMessage::DynamicMessage( - dynamic_type, dynamic_type->get_rosidl_dynamic_type().allocator) {} - - -DynamicMessage::DynamicMessage( - const DynamicMessageType::SharedPtr dynamic_type, - rcl_allocator_t allocator) -: serialization_support_(dynamic_type->get_shared_dynamic_serialization_support()), - rosidl_dynamic_data_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data()), - is_loaned_(false), - parent_data_(nullptr) -{ - if (!serialization_support_) { - throw std::runtime_error("dynamic type could not bind serialization support!"); - } - if (!dynamic_type) { - throw std::runtime_error("dynamic message type cannot be nullptr!"); - } - - rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = - dynamic_type->get_rosidl_dynamic_type(); - - rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( - &rosidl_dynamic_type, &allocator, &rosidl_dynamic_data); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not init new dynamic data object from dynamic type") + - rcl_get_error_string().str); - } -} - - -DynamicMessage::DynamicMessage( - DynamicSerializationSupport::SharedPtr serialization_support, - rosidl_dynamic_typesupport_dynamic_data_t && rosidl_dynamic_data) -: serialization_support_(serialization_support), - rosidl_dynamic_data_(std::move(rosidl_dynamic_data)), - is_loaned_(false), - parent_data_(nullptr) -{ - if (serialization_support) { - if (!match_serialization_support_(*serialization_support, rosidl_dynamic_data)) { - throw std::runtime_error( - "serialization support library identifier does not match dynamic data's!"); - } - } -} - - -DynamicMessage::DynamicMessage( - DynamicMessage::SharedPtr parent_data, - rosidl_dynamic_typesupport_dynamic_data_t && rosidl_loaned_data) -: serialization_support_(parent_data->get_shared_dynamic_serialization_support()), - rosidl_dynamic_data_(std::move(rosidl_loaned_data)), - is_loaned_(true), - parent_data_(nullptr) -{ - if (!parent_data) { - throw std::runtime_error("parent dynamic data cannot be nullptr!"); - } - if (serialization_support_) { - if (!match_serialization_support_(*serialization_support_, rosidl_loaned_data)) { - throw std::runtime_error( - "serialization support library identifier does not match loaned dynamic data's!"); - } - } - parent_data_ = parent_data; -} - - -DynamicMessage::DynamicMessage(DynamicMessage && other) noexcept -: serialization_support_(std::exchange(other.serialization_support_, nullptr)), - rosidl_dynamic_data_(std::exchange( - other.rosidl_dynamic_data_, - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data())), - is_loaned_(other.is_loaned_), - parent_data_(std::exchange(other.parent_data_, nullptr)) -{} - - -DynamicMessage & -DynamicMessage::operator=(DynamicMessage && other) noexcept -{ - std::swap(serialization_support_, other.serialization_support_); - std::swap(rosidl_dynamic_data_, other.rosidl_dynamic_data_); - is_loaned_ = other.is_loaned_; - std::swap(parent_data_, other.parent_data_); - return *this; -} - - DynamicMessage::~DynamicMessage() -{ - if (!is_loaned_) { - if (rosidl_dynamic_typesupport_dynamic_data_fini(&get_rosidl_dynamic_data()) != - RCUTILS_RET_OK) - { - RCUTILS_LOG_ERROR("could not fini rosidl dynamic data"); - } - return; - } - - // Loaned case - if (!parent_data_) { - RCUTILS_LOG_ERROR("dynamic data is loaned, but parent is missing!!"); - } else { - rosidl_dynamic_typesupport_dynamic_data_return_loaned_value( - &parent_data_->get_rosidl_dynamic_data(), &get_rosidl_dynamic_data()); - } -} - - -bool -DynamicMessage::match_serialization_support_( - const DynamicSerializationSupport & serialization_support, - const rosidl_dynamic_typesupport_dynamic_data_t & rosidl_dynamic_type_data) -{ - if (serialization_support.get_serialization_library_identifier() != std::string( - rosidl_dynamic_type_data.serialization_support->serialization_library_identifier)) - { - RCUTILS_LOG_ERROR("serialization support library identifier does not match dynamic data's"); - return false; - } - return true; -} - - -// GETTERS ======================================================================================= -const std::string -DynamicMessage::get_serialization_library_identifier() const -{ - return std::string( - get_rosidl_dynamic_data().serialization_support->serialization_library_identifier); -} - - -const std::string -DynamicMessage::get_name() const -{ - size_t buf_length; - const char * buf; - if ( - rosidl_dynamic_typesupport_dynamic_data_get_name( - &get_rosidl_dynamic_data(), &buf, - &buf_length) != - RCUTILS_RET_OK) - { - throw std::runtime_error( - std::string("could not get name for dynamic data") + rcl_get_error_string().str); - } - return std::string(buf, buf_length); -} - - -rosidl_dynamic_typesupport_dynamic_data_t & -DynamicMessage::get_rosidl_dynamic_data() -{ - return rosidl_dynamic_data_; -} - - -const rosidl_dynamic_typesupport_dynamic_data_t & -DynamicMessage::get_rosidl_dynamic_data() const -{ - return rosidl_dynamic_data_; -} - - -DynamicSerializationSupport::SharedPtr -DynamicMessage::get_shared_dynamic_serialization_support() -{ - return serialization_support_; -} - - -DynamicSerializationSupport::ConstSharedPtr -DynamicMessage::get_shared_dynamic_serialization_support() const -{ - return serialization_support_; -} - - -size_t -DynamicMessage::get_item_count() const -{ - size_t item_count; - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_item_count( - &get_rosidl_dynamic_data(), &item_count); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not get item count of dynamic data"); - } - return item_count; -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::get_member_id(size_t index) const -{ - rosidl_dynamic_typesupport_member_id_t member_id; - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_at_index( - &get_rosidl_dynamic_data(), index, &member_id); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not member id of dynamic data element by index"); - } - return member_id; -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::get_member_id(const std::string & name) const -{ - rosidl_dynamic_typesupport_member_id_t member_id; - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_by_name( - &get_rosidl_dynamic_data(), name.c_str(), name.size(), &member_id); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not member id of dynamic data element by name"); - } - return member_id; -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::get_array_index(size_t index) const -{ - rosidl_dynamic_typesupport_member_id_t array_index; - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_array_index( - &get_rosidl_dynamic_data(), index, &array_index); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not array index of dynamic data element by index"); - } - return array_index; -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::get_array_index(const std::string & name) const -{ - return get_array_index(get_member_id(name)); -} - - -// METHODS ======================================================================================= -DynamicMessage -DynamicMessage::clone(rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone( - &get_rosidl_dynamic_data(), &allocator, &rosidl_dynamic_data); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not clone dynamic data: ") + rcl_get_error_string().str); - } - return DynamicMessage(get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_data)); -} - - -DynamicMessage::SharedPtr -DynamicMessage::clone_shared(rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone( - &get_rosidl_dynamic_data(), &allocator, &rosidl_dynamic_data); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not clone dynamic data: ") + rcl_get_error_string().str); - } - return DynamicMessage::make_shared( - get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_data)); -} - - -DynamicMessage -DynamicMessage::init_from_type(DynamicMessageType & type, rcl_allocator_t allocator) const -{ - rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( - &type.get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_data); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not init new dynamic data object from dynamic type"); - } - return DynamicMessage(serialization_support_, std::move(rosidl_dynamic_data)); -} - - -DynamicMessage::SharedPtr -DynamicMessage::init_from_type_shared(DynamicMessageType & type, rcl_allocator_t allocator) const -{ - rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( - &type.get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_data); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not init new dynamic data object from dynamic type"); - } - return DynamicMessage::make_shared(serialization_support_, std::move(rosidl_dynamic_data)); -} - - -bool -DynamicMessage::equals(const DynamicMessage & other) const -{ - if (get_serialization_library_identifier() != other.get_serialization_library_identifier()) { - throw std::runtime_error("library identifiers don't match"); - } - bool equals; - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_equals( - &get_rosidl_dynamic_data(), &other.get_rosidl_dynamic_data(), &equals); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not equate dynamic messages"); - } - return equals; -} - - -DynamicMessage::SharedPtr -DynamicMessage::loan_value( - rosidl_dynamic_typesupport_member_id_t id, - rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_loan_value( - &get_rosidl_dynamic_data(), id, &allocator, &rosidl_dynamic_data); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not loan dynamic data: ") + rcl_get_error_string().str); - } - return DynamicMessage::make_shared(shared_from_this(), std::move(rosidl_dynamic_data)); -} - - -DynamicMessage::SharedPtr -DynamicMessage::loan_value(const std::string & name, rcl_allocator_t allocator) -{ - return loan_value(get_member_id(name), allocator); -} - - -void -DynamicMessage::clear_all_values() -{ - rosidl_dynamic_typesupport_dynamic_data_clear_all_values(&get_rosidl_dynamic_data()); -} - - -void -DynamicMessage::clear_nonkey_values() -{ - rosidl_dynamic_typesupport_dynamic_data_clear_nonkey_values(&get_rosidl_dynamic_data()); -} - - -void -DynamicMessage::clear_value(rosidl_dynamic_typesupport_member_id_t id) -{ - rosidl_dynamic_typesupport_dynamic_data_clear_value(&get_rosidl_dynamic_data(), id); -} - - -void -DynamicMessage::clear_value(const std::string & name) -{ - clear_value(get_member_id(name)); -} - - -void -DynamicMessage::clear_sequence() -{ - rosidl_dynamic_typesupport_dynamic_data_clear_sequence_data(&get_rosidl_dynamic_data()); -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_sequence_data() -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_sequence_data(&get_rosidl_dynamic_data(), &out); - return out; -} - - -void -DynamicMessage::remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index) -{ - rosidl_dynamic_typesupport_dynamic_data_remove_sequence_data( - &get_rosidl_dynamic_data(), index); -} - - -bool -DynamicMessage::serialize(rcl_serialized_message_t & buffer) -{ - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_serialize( - &get_rosidl_dynamic_data(), &buffer); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could serialize loan dynamic data: ") + rcl_get_error_string().str); - } - return true; -} - - -bool -DynamicMessage::deserialize(rcl_serialized_message_t & buffer) -{ - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_deserialize( - &get_rosidl_dynamic_data(), &buffer); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could deserialize loan dynamic data: ") + rcl_get_error_string().str); - } - return true; -} - - -// MEMBER ACCESS =================================================================================== -// Defined in "detail/dynamic_message_impl.hpp" - - -// FIXED STRING MEMBER ACCESS ====================================================================== -const std::string -DynamicMessage::get_fixed_string_value( - rosidl_dynamic_typesupport_member_id_t id, size_t string_length) -{ - size_t buf_length; - char * buf = nullptr; - rosidl_dynamic_typesupport_dynamic_data_get_fixed_string_value( - &get_rosidl_dynamic_data(), id, &buf, &buf_length, string_length); - auto out = std::string(buf, buf_length); - delete buf; - return out; -} - - -const std::string -DynamicMessage::get_fixed_string_value(const std::string & name, size_t string_length) -{ - return get_fixed_string_value(get_member_id(name), string_length); -} - - -const std::u16string -DynamicMessage::get_fixed_wstring_value( - rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length) -{ - size_t buf_length; - char16_t * buf = nullptr; - rosidl_dynamic_typesupport_dynamic_data_get_fixed_wstring_value( - &get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_length); - auto out = std::u16string(buf, buf_length); - delete buf; - return out; -} - - -const std::u16string -DynamicMessage::get_fixed_wstring_value(const std::string & name, size_t wstring_length) -{ - return get_fixed_wstring_value(get_member_id(name), wstring_length); -} - - -void -DynamicMessage::set_fixed_string_value( - rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length) -{ - rosidl_dynamic_typesupport_dynamic_data_set_fixed_string_value( - &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_length); -} - - -void -DynamicMessage::set_fixed_string_value( - const std::string & name, const std::string value, size_t string_length) -{ - set_fixed_string_value(get_member_id(name), value, string_length); -} - - -void -DynamicMessage::set_fixed_wstring_value( - rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length) -{ - rosidl_dynamic_typesupport_dynamic_data_set_fixed_wstring_value( - &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_length); -} - - -void -DynamicMessage::set_fixed_wstring_value( - const std::string & name, const std::u16string value, size_t wstring_length) -{ - set_fixed_wstring_value(get_member_id(name), value, wstring_length); -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_fixed_string_value(const std::string value, size_t string_length) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_fixed_string_value( - &get_rosidl_dynamic_data(), value.c_str(), value.size(), string_length, &out); - return out; -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_fixed_wstring_value(const std::u16string value, size_t wstring_length) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_fixed_wstring_value( - &get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_length, &out); - return out; -} - - -// BOUNDED STRING MEMBER ACCESS ==================================================================== -const std::string -DynamicMessage::get_bounded_string_value( - rosidl_dynamic_typesupport_member_id_t id, size_t string_bound) -{ - size_t buf_length; - char * buf = nullptr; - rosidl_dynamic_typesupport_dynamic_data_get_bounded_string_value( - &get_rosidl_dynamic_data(), id, &buf, &buf_length, string_bound); - auto out = std::string(buf, buf_length); - delete buf; - return out; -} - - -const std::string -DynamicMessage::get_bounded_string_value(const std::string & name, size_t string_bound) -{ - return get_bounded_string_value(get_member_id(name), string_bound); -} - - -const std::u16string -DynamicMessage::get_bounded_wstring_value( - rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound) -{ - size_t buf_length; - char16_t * buf = nullptr; - rosidl_dynamic_typesupport_dynamic_data_get_bounded_wstring_value( - &get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_bound); - auto out = std::u16string(buf, buf_length); - delete buf; - return out; -} - - -const std::u16string -DynamicMessage::get_bounded_wstring_value(const std::string & name, size_t wstring_bound) -{ - return get_bounded_wstring_value(get_member_id(name), wstring_bound); -} - - -void -DynamicMessage::set_bounded_string_value( - rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound) -{ - rosidl_dynamic_typesupport_dynamic_data_set_bounded_string_value( - &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_bound); -} - - -void -DynamicMessage::set_bounded_string_value( - const std::string & name, const std::string value, size_t string_bound) -{ - set_bounded_string_value(get_member_id(name), value, string_bound); -} - - -void -DynamicMessage::set_bounded_wstring_value( - rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound) -{ - rosidl_dynamic_typesupport_dynamic_data_set_bounded_wstring_value( - &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_bound); -} - - -void -DynamicMessage::set_bounded_wstring_value( - const std::string & name, const std::u16string value, size_t wstring_bound) -{ - set_bounded_wstring_value(get_member_id(name), value, wstring_bound); -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_bounded_string_value(const std::string value, size_t string_bound) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_bounded_string_value( - &get_rosidl_dynamic_data(), value.c_str(), value.size(), string_bound, &out); - return out; -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_bounded_wstring_value( - &get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_bound, &out); - return out; -} - - -// NESTED MEMBER ACCESS ============================================================================ -DynamicMessage -DynamicMessage::get_complex_value( - rosidl_dynamic_typesupport_member_id_t id, rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_data_t out = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); - rosidl_dynamic_typesupport_dynamic_data_get_complex_value( - &get_rosidl_dynamic_data(), id, &allocator, &out); - return DynamicMessage(get_shared_dynamic_serialization_support(), std::move(out)); -} - - -DynamicMessage -DynamicMessage::get_complex_value(const std::string & name, rcl_allocator_t allocator) -{ - return get_complex_value(get_member_id(name), allocator); -} - - -DynamicMessage::SharedPtr -DynamicMessage::get_complex_value_shared( - rosidl_dynamic_typesupport_member_id_t id, rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_data_t out = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); - rosidl_dynamic_typesupport_dynamic_data_get_complex_value( - &get_rosidl_dynamic_data(), id, &allocator, &out); - return DynamicMessage::make_shared(get_shared_dynamic_serialization_support(), std::move(out)); -} - - -DynamicMessage::SharedPtr -DynamicMessage::get_complex_value_shared(const std::string & name, rcl_allocator_t allocator) -{ - return get_complex_value_shared(get_member_id(name), allocator); -} - - -void -DynamicMessage::set_complex_value( - rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value) -{ - rosidl_dynamic_typesupport_dynamic_data_set_complex_value( - &get_rosidl_dynamic_data(), id, &value.get_rosidl_dynamic_data()); -} - - -void -DynamicMessage::set_complex_value(const std::string & name, DynamicMessage & value) -{ - set_complex_value(get_member_id(name), value); -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_complex_value_copy(const DynamicMessage & value) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_complex_value_copy( - &get_rosidl_dynamic_data(), &value.get_rosidl_dynamic_data(), &out); - return out; -} - - -rosidl_dynamic_typesupport_member_id_t -DynamicMessage::insert_complex_value(DynamicMessage & value) -{ - rosidl_dynamic_typesupport_member_id_t out; - rosidl_dynamic_typesupport_dynamic_data_insert_complex_value( - &get_rosidl_dynamic_data(), &value.get_rosidl_dynamic_data(), &out); - return out; -} +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp index 21a67ebbe0..069aff03aa 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp @@ -34,248 +34,5 @@ using rclcpp::dynamic_typesupport::DynamicMessageType; using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; using rclcpp::dynamic_typesupport::DynamicSerializationSupport; - -// CONSTRUCTION ==================================================================================== -DynamicMessageType::DynamicMessageType(DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder) -: DynamicMessageType::DynamicMessageType( - dynamic_type_builder, dynamic_type_builder->get_rosidl_dynamic_type_builder().allocator) {} - - -DynamicMessageType::DynamicMessageType( - DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder, - rcl_allocator_t allocator) -: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), - rosidl_dynamic_type_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type()) -{ - if (!serialization_support_) { - throw std::runtime_error("dynamic type could not bind serialization support!"); - } - - rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = - dynamic_type_builder->get_rosidl_dynamic_type_builder(); - - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_init_from_dynamic_type_builder( - &rosidl_dynamic_type_builder, &allocator, &get_rosidl_dynamic_type()); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not init new dynamic type object"); - } -} - - -DynamicMessageType::DynamicMessageType( - DynamicSerializationSupport::SharedPtr serialization_support, - rosidl_dynamic_typesupport_dynamic_type_t && rosidl_dynamic_type) -: serialization_support_(serialization_support), - rosidl_dynamic_type_(std::move(rosidl_dynamic_type)) -{ - if (serialization_support) { - if (!match_serialization_support_(*serialization_support, rosidl_dynamic_type)) { - throw std::runtime_error( - "serialization support library identifier does not match dynamic type's!"); - } - } -} - - -DynamicMessageType::DynamicMessageType( - DynamicSerializationSupport::SharedPtr serialization_support, - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator) -: serialization_support_(serialization_support), - rosidl_dynamic_type_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type()) -{ - init_from_description(description, allocator, serialization_support); -} - - -DynamicMessageType::DynamicMessageType(DynamicMessageType && other) noexcept -: serialization_support_(std::exchange(other.serialization_support_, nullptr)), - rosidl_dynamic_type_(std::exchange( - other.rosidl_dynamic_type_, rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type())) -{} - - -DynamicMessageType & -DynamicMessageType::operator=(DynamicMessageType && other) noexcept -{ - std::swap(serialization_support_, other.serialization_support_); - std::swap(rosidl_dynamic_type_, other.rosidl_dynamic_type_); - return *this; -} - - DynamicMessageType::~DynamicMessageType() -{ - if (rosidl_dynamic_typesupport_dynamic_type_fini(&get_rosidl_dynamic_type()) != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR("could not fini rosidl dynamic type"); - } -} - - -void -DynamicMessageType::init_from_description( - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator, - DynamicSerializationSupport::SharedPtr serialization_support) -{ - if (serialization_support) { - // Swap serialization support if serialization support is given - serialization_support_ = serialization_support; - } - - rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_init_from_description( - &serialization_support_->get_rosidl_serialization_support(), - &description, - &allocator, - &rosidl_dynamic_type); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not init new dynamic type object"); - } - - rosidl_dynamic_type_ = std::move(rosidl_dynamic_type); -} - - -bool -DynamicMessageType::match_serialization_support_( - const DynamicSerializationSupport & serialization_support, - const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type) -{ - if (serialization_support.get_serialization_library_identifier() != std::string( - rosidl_dynamic_type.serialization_support->serialization_library_identifier)) - { - RCUTILS_LOG_ERROR( - "serialization support library identifier does not match dynamic type's (%s vs %s)", - serialization_support.get_serialization_library_identifier().c_str(), - rosidl_dynamic_type.serialization_support->serialization_library_identifier); - return false; - } - return true; -} - - -// GETTERS ========================================================================================= -const std::string -DynamicMessageType::get_serialization_library_identifier() const -{ - return std::string( - get_rosidl_dynamic_type().serialization_support->serialization_library_identifier); -} - - -const std::string -DynamicMessageType::get_name() const -{ - size_t buf_length; - const char * buf; - rosidl_dynamic_typesupport_dynamic_type_get_name(&get_rosidl_dynamic_type(), &buf, &buf_length); - return std::string(buf, buf_length); -} - - -size_t -DynamicMessageType::get_member_count() const -{ - size_t out; - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_get_member_count( - &get_rosidl_dynamic_type(), &out); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not get member count: ") + rcl_get_error_string().str); - } - return out; -} - - -rosidl_dynamic_typesupport_dynamic_type_t & -DynamicMessageType::get_rosidl_dynamic_type() -{ - return rosidl_dynamic_type_; -} - - -const rosidl_dynamic_typesupport_dynamic_type_t & -DynamicMessageType::get_rosidl_dynamic_type() const -{ - return rosidl_dynamic_type_; -} - - -DynamicSerializationSupport::SharedPtr -DynamicMessageType::get_shared_dynamic_serialization_support() -{ - return serialization_support_; -} - - -DynamicSerializationSupport::ConstSharedPtr -DynamicMessageType::get_shared_dynamic_serialization_support() const -{ - return serialization_support_; -} - - -// METHODS ========================================================================================= -DynamicMessageType -DynamicMessageType::clone(rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone( - &get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_type); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not clone dynamic type: ") + rcl_get_error_string().str); - } - return DynamicMessageType( - get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type)); -} - - -DynamicMessageType::SharedPtr -DynamicMessageType::clone_shared(rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone( - &get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_type); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not clone dynamic type: ") + rcl_get_error_string().str); - } - return DynamicMessageType::make_shared( - get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type)); -} - - -bool -DynamicMessageType::equals(const DynamicMessageType & other) const -{ - if (get_serialization_library_identifier() != other.get_serialization_library_identifier()) { - throw std::runtime_error("library identifiers don't match"); - } - bool out; - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_equals( - &get_rosidl_dynamic_type(), &other.get_rosidl_dynamic_type(), &out); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not equate dynamic message types: ") + rcl_get_error_string().str); - } - return out; -} - - -DynamicMessage -DynamicMessageType::build_dynamic_message(rcl_allocator_t allocator) -{ - return DynamicMessage(shared_from_this(), allocator); -} - - -DynamicMessage::SharedPtr -DynamicMessageType::build_dynamic_message_shared(rcl_allocator_t allocator) -{ - return DynamicMessage::make_shared(shared_from_this(), allocator); -} +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp index ca225cf107..1c5d3708a1 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp @@ -33,582 +33,5 @@ using rclcpp::dynamic_typesupport::DynamicMessageType; using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; using rclcpp::dynamic_typesupport::DynamicSerializationSupport; -#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ -// Template specialization implementations -#include "rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp" -#endif - -// CONSTRUCTION ==================================================================================== -DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( - DynamicSerializationSupport::SharedPtr serialization_support, const std::string & name) -: DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( - serialization_support, - name, - serialization_support->get_rosidl_serialization_support().allocator) {} - - -DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( - DynamicSerializationSupport::SharedPtr serialization_support, - const std::string & name, - rcl_allocator_t allocator) -: serialization_support_(serialization_support), - rosidl_dynamic_type_builder_( - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) -{ - init_from_serialization_support_(serialization_support, name, allocator); -} - - -DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( - DynamicSerializationSupport::SharedPtr serialization_support, - rosidl_dynamic_typesupport_dynamic_type_builder_t && rosidl_dynamic_type_builder) -: serialization_support_(serialization_support), - rosidl_dynamic_type_builder_( - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) -{ - if (!serialization_support) { - throw std::runtime_error("serialization support cannot be nullptr!"); - } - if (!match_serialization_support_(*serialization_support, rosidl_dynamic_type_builder)) { - throw std::runtime_error( - "serialization support library does not match dynamic type builder's!"); - } -} - - -DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( - DynamicSerializationSupport::SharedPtr serialization_support, - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator) -: serialization_support_(serialization_support), - rosidl_dynamic_type_builder_( - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) -{ - if (!serialization_support) { - throw std::runtime_error("serialization support cannot be nullptr!"); - } - init_from_description(description, allocator, serialization_support); -} - - -DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept -: serialization_support_(std::exchange(other.serialization_support_, nullptr)), - rosidl_dynamic_type_builder_(std::exchange( - other.rosidl_dynamic_type_builder_, - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder())) {} - - -DynamicMessageTypeBuilder & -DynamicMessageTypeBuilder::operator=(DynamicMessageTypeBuilder && other) noexcept -{ - std::swap(serialization_support_, other.serialization_support_); - std::swap(rosidl_dynamic_type_builder_, other.rosidl_dynamic_type_builder_); - return *this; -} - - DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder() -{ - if (rosidl_dynamic_typesupport_dynamic_type_builder_fini(&get_rosidl_dynamic_type_builder()) != - RCUTILS_RET_OK) - { - RCUTILS_LOG_ERROR("could not fini rosidl dynamic type builder"); - } -} - - -void -DynamicMessageTypeBuilder::init_from_description( - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator, - DynamicSerializationSupport::SharedPtr serialization_support) -{ - if (serialization_support) { - // Swap serialization support if serialization support is given - serialization_support_ = serialization_support; - } - - rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_init_from_description( - &serialization_support_->get_rosidl_serialization_support(), - &description, - &allocator, - &rosidl_dynamic_type_builder); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not init new dynamic type builder object"); - } - - rosidl_dynamic_type_builder_ = std::move(rosidl_dynamic_type_builder); -} - - -void -DynamicMessageTypeBuilder::init_from_serialization_support_( - DynamicSerializationSupport::SharedPtr serialization_support, - const std::string & name, - rcl_allocator_t allocator) -{ - if (!serialization_support) { - throw std::runtime_error("serialization support cannot be nullptr!"); - } - if (!&serialization_support->get_rosidl_serialization_support()) { - throw std::runtime_error("serialization support raw pointer cannot be nullptr!"); - } - - rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_init( - &serialization_support->get_rosidl_serialization_support(), - name.c_str(), name.size(), - &allocator, - &rosidl_dynamic_type_builder); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not init dynamic type builder: ") + rcl_get_error_string().str); - } -} - - -bool -DynamicMessageTypeBuilder::match_serialization_support_( - const DynamicSerializationSupport & serialization_support, - const rosidl_dynamic_typesupport_dynamic_type_builder_t & rosidl_dynamic_type_builder) -{ - if (serialization_support.get_serialization_library_identifier() != std::string( - rosidl_dynamic_type_builder.serialization_support->serialization_library_identifier)) - { - RCUTILS_LOG_ERROR( - "serialization support library identifier does not match dynamic type builder's"); - return false; - } - return true; -} - - -// GETTERS ======================================================================================= -const std::string -DynamicMessageTypeBuilder::get_serialization_library_identifier() const -{ - return std::string( - get_rosidl_dynamic_type_builder().serialization_support->serialization_library_identifier); -} - - -const std::string -DynamicMessageTypeBuilder::get_name() const -{ - size_t buf_length; - const char * buf; - rosidl_dynamic_typesupport_dynamic_type_builder_get_name( - &get_rosidl_dynamic_type_builder(), &buf, &buf_length); - return std::string(buf, buf_length); -} - - -rosidl_dynamic_typesupport_dynamic_type_builder_t & -DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() -{ - return rosidl_dynamic_type_builder_; -} - - -const rosidl_dynamic_typesupport_dynamic_type_builder_t & -DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() const -{ - return rosidl_dynamic_type_builder_; -} - - -DynamicSerializationSupport::SharedPtr -DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() -{ - return serialization_support_; -} - - -DynamicSerializationSupport::ConstSharedPtr -DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() const -{ - return serialization_support_; -} - - -// METHODS ======================================================================================= -void -DynamicMessageTypeBuilder::set_name(const std::string & name) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_set_name( - &get_rosidl_dynamic_type_builder(), name.c_str(), name.size()); -} - - -DynamicMessageTypeBuilder -DynamicMessageTypeBuilder::clone(rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone( - &get_rosidl_dynamic_type_builder(), &allocator, &rosidl_dynamic_type_builder); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str); - } - return DynamicMessageTypeBuilder( - get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type_builder)); -} - - -DynamicMessageTypeBuilder::SharedPtr -DynamicMessageTypeBuilder::clone_shared(rcl_allocator_t allocator) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = - rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); - rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone( - &get_rosidl_dynamic_type_builder(), &allocator, &rosidl_dynamic_type_builder); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error( - std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str); - } - return DynamicMessageTypeBuilder::make_shared( - get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type_builder)); -} - - -void -DynamicMessageTypeBuilder::clear(rcl_allocator_t allocator) -{ - if (!serialization_support_) { - throw std::runtime_error( - "cannot call clear() on a dynamic type builder with uninitialized serialization support" - ); - } - - const std::string & name = get_name(); - init_from_serialization_support_(serialization_support_, name, allocator); -} - - -DynamicMessage -DynamicMessageTypeBuilder::build_dynamic_message(rcl_allocator_t allocator) -{ - return DynamicMessage(shared_from_this(), allocator); -} - - -DynamicMessage::SharedPtr -DynamicMessageTypeBuilder::build_dynamic_message_shared(rcl_allocator_t allocator) -{ - return DynamicMessage::make_shared(shared_from_this(), allocator); -} - - -DynamicMessageType -DynamicMessageTypeBuilder::build_dynamic_message_type(rcl_allocator_t allocator) -{ - return DynamicMessageType(shared_from_this(), allocator); -} - - -DynamicMessageType::SharedPtr -DynamicMessageTypeBuilder::build_dynamic_message_type_shared(rcl_allocator_t allocator) -{ - return DynamicMessageType::make_shared(shared_from_this(), allocator); -} - - -// ADD MEMBERS ===================================================================================== -// Defined in "detail/dynamic_message_type_builder_impl.hpp" - - -// ADD FIXED STRING MEMBERS ======================================================================== -void -DynamicMessageTypeBuilder::add_fixed_string_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - string_length); -} - - -void -DynamicMessageTypeBuilder::add_fixed_wstring_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - wstring_length); -} - - -void -DynamicMessageTypeBuilder::add_fixed_string_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t string_length, size_t array_length, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_array_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - string_length, array_length); -} - - -void -DynamicMessageTypeBuilder::add_fixed_wstring_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t wstring_length, size_t array_length, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_array_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - wstring_length, array_length); -} - - -void -DynamicMessageTypeBuilder::add_fixed_string_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_unbounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - string_length); -} - - -void -DynamicMessageTypeBuilder::add_fixed_wstring_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_unbounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - wstring_length); -} - - -void -DynamicMessageTypeBuilder::add_fixed_string_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t string_length, size_t sequence_bound, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_bounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - string_length, sequence_bound); -} - - -void -DynamicMessageTypeBuilder::add_fixed_wstring_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t wstring_length, size_t sequence_bound, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_bounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - wstring_length, sequence_bound); -} - - -// ADD BOUNDED STRING MEMBERS ====================================================================== -void -DynamicMessageTypeBuilder::add_bounded_string_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - string_bound); -} - - -void -DynamicMessageTypeBuilder::add_bounded_wstring_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - wstring_bound); -} - - -void -DynamicMessageTypeBuilder::add_bounded_string_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t string_bound, size_t array_length, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_array_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - string_bound, array_length); -} - - -void -DynamicMessageTypeBuilder::add_bounded_wstring_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t wstring_bound, size_t array_length, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_array_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - wstring_bound, array_length); -} - - -void -DynamicMessageTypeBuilder::add_bounded_string_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_unbounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - string_bound); -} - - -void -DynamicMessageTypeBuilder::add_bounded_wstring_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_unbounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - wstring_bound); -} - - -void -DynamicMessageTypeBuilder::add_bounded_string_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t string_bound, size_t sequence_bound, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_bounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - string_bound, sequence_bound); -} - - -void -DynamicMessageTypeBuilder::add_bounded_wstring_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - size_t wstring_bound, size_t sequence_bound, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_bounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - wstring_bound, sequence_bound); -} - - -// ADD NESTED MEMBERS ============================================================================== -void -DynamicMessageTypeBuilder::add_complex_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageType & nested_type, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - &nested_type.get_rosidl_dynamic_type()); -} - - -void -DynamicMessageTypeBuilder::add_complex_array_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageType & nested_type, size_t array_length, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - &nested_type.get_rosidl_dynamic_type(), array_length); -} - - -void -DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageType & nested_type, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - &nested_type.get_rosidl_dynamic_type()); -} - - -void -DynamicMessageTypeBuilder::add_complex_bounded_sequence_member( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageType & nested_type, size_t sequence_bound, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - &nested_type.get_rosidl_dynamic_type(), sequence_bound); -} - - -void -DynamicMessageTypeBuilder::add_complex_member_builder( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member_builder( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - &nested_type_builder.get_rosidl_dynamic_type_builder()); -} - - -void -DynamicMessageTypeBuilder::add_complex_array_member_builder( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageTypeBuilder & nested_type_builder, size_t array_length, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member_builder( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - &nested_type_builder.get_rosidl_dynamic_type_builder(), array_length); -} - - -void -DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member_builder( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member_builder( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - &nested_type_builder.get_rosidl_dynamic_type_builder()); -} - - -void -DynamicMessageTypeBuilder::add_complex_bounded_sequence_member_builder( - rosidl_dynamic_typesupport_member_id_t id, const std::string & name, - DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound, - const std::string & default_value) -{ - rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member_builder( - &get_rosidl_dynamic_type_builder(), - id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), - &nested_type_builder.get_rosidl_dynamic_type_builder(), sequence_bound); -} +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp index b840ab52c3..3a920c2805 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp @@ -45,273 +45,5 @@ using rclcpp::dynamic_typesupport::DynamicMessageType; using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport; using rclcpp::dynamic_typesupport::DynamicSerializationSupport; -// CONSTRUCTION ==================================================================================== -DynamicMessageTypeSupport::DynamicMessageTypeSupport( - const rosidl_runtime_c__type_description__TypeDescription & description, - const std::string & serialization_library_name, - rcl_allocator_t allocator) -: serialization_support_(nullptr), - dynamic_message_type_(nullptr), - dynamic_message_(nullptr), - rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) -{ - rcl_ret_t ret; - if (serialization_library_name.empty()) { - ret = rcl_dynamic_message_type_support_handle_init( - nullptr, &description, &allocator, &rosidl_message_type_support_); - } else { - ret = rcl_dynamic_message_type_support_handle_init( - serialization_library_name.c_str(), &description, &allocator, &rosidl_message_type_support_); - } - if (ret != RCL_RET_OK) { - std::string error_msg = - std::string("error initializing rosidl message type support: ") + rcl_get_error_string().str; - rcl_reset_error(); - throw std::runtime_error(error_msg); - } - if (rosidl_message_type_support_.typesupport_identifier != - rosidl_get_dynamic_typesupport_identifier()) - { - throw std::runtime_error("rosidl message type support is of the wrong type"); - } - - auto ts_impl = static_cast(const_cast( - rosidl_message_type_support_.data)); - - serialization_support_ = DynamicSerializationSupport::make_shared( - std::move(ts_impl->serialization_support)); - - dynamic_message_type_ = DynamicMessageType::make_shared( - get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message_type)); - - dynamic_message_ = DynamicMessage::make_shared( - get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message)); -} - -DynamicMessageTypeSupport::DynamicMessageTypeSupport( - DynamicSerializationSupport::SharedPtr serialization_support, - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator) -: serialization_support_(serialization_support), - dynamic_message_type_(nullptr), - dynamic_message_(nullptr), - rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) -{ - // Check null - if (!serialization_support) { - throw std::runtime_error("serialization_support cannot be nullptr."); - } - - rosidl_type_hash_t type_hash; - rcutils_ret_t hash_ret = rcl_calculate_type_hash( - // TODO(methylDragon): Swap this out with the conversion function when it is ready - reinterpret_cast(&description), - &type_hash); - if (hash_ret != RCL_RET_OK) { - throw std::runtime_error("failed to get type hash"); - } - - rcutils_ret_t ret = rosidl_dynamic_message_type_support_handle_init( - &serialization_support->get_rosidl_serialization_support(), - &type_hash, // type_hash - &description, // type_description - nullptr, // type_description_sources (not implemented for dynamic types) - &allocator, - &rosidl_message_type_support_); - if (ret != RCUTILS_RET_OK) { - throw std::runtime_error("could not init rosidl message type support"); - } - if (rosidl_message_type_support_.typesupport_identifier != - rosidl_get_dynamic_typesupport_identifier()) - { - throw std::runtime_error("rosidl message type support is of the wrong type"); - } - - auto ts_impl = static_cast( - rosidl_message_type_support_.data); - - dynamic_message_type_ = DynamicMessageType::make_shared( - get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message_type)); - - dynamic_message_ = DynamicMessage::make_shared( - get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message)); -} - -DynamicMessageTypeSupport::DynamicMessageTypeSupport( - DynamicSerializationSupport::SharedPtr serialization_support, - DynamicMessageType::SharedPtr dynamic_message_type, - DynamicMessage::SharedPtr dynamic_message, - const rosidl_runtime_c__type_description__TypeDescription & description, - rcl_allocator_t allocator) -: serialization_support_(serialization_support), - dynamic_message_type_(dynamic_message_type), - dynamic_message_(dynamic_message), - rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) -{ - // Check null - if (!serialization_support) { - throw std::runtime_error("serialization_support cannot be nullptr."); - } - if (!dynamic_message_type) { - throw std::runtime_error("dynamic_message_type cannot be nullptr."); - } - if (!dynamic_message) { - throw std::runtime_error("dynamic_message cannot be nullptr."); - } - - // Check identifiers - if (serialization_support->get_serialization_library_identifier() != - dynamic_message_type->get_serialization_library_identifier()) - { - throw std::runtime_error( - "serialization support library identifier does not match " - "dynamic message type library identifier."); - } - if (dynamic_message_type->get_serialization_library_identifier() != - dynamic_message->get_serialization_library_identifier()) - { - throw std::runtime_error( - "dynamic message type library identifier does not match " - "dynamic message library identifier."); - } - - rosidl_type_hash_t type_hash; - rcutils_ret_t hash_ret = rcl_calculate_type_hash( - // TODO(methylDragon): Swap this out with the conversion function when it is ready - // from https://github.com/ros2/rcl/pull/1052 - reinterpret_cast(&description), - &type_hash); - if (hash_ret != RCL_RET_OK) { - std::string error_msg = std::string("failed to get type hash: ") + rcl_get_error_string().str; - rcl_reset_error(); - throw std::runtime_error(error_msg); - } - - auto ts_impl = static_cast( - allocator.zero_allocate(1, sizeof(rosidl_dynamic_message_type_support_impl_t), allocator.state) - ); - if (!ts_impl) { - throw std::runtime_error("could not allocate rosidl_message_type_support_t"); - } - - ts_impl->allocator = allocator; - ts_impl->type_hash = type_hash; - if (!rosidl_runtime_c__type_description__TypeDescription__copy( - &description, &ts_impl->type_description)) - { - throw std::runtime_error("could not copy type description"); - } - // ts_impl->type_description_sources = // Not used - - ts_impl->serialization_support = serialization_support->get_rosidl_serialization_support(); - ts_impl->dynamic_message_type = &dynamic_message_type->get_rosidl_dynamic_type(); - ts_impl->dynamic_message = &dynamic_message->get_rosidl_dynamic_data(); - - rosidl_message_type_support_ = { - rosidl_get_dynamic_typesupport_identifier(), // typesupport_identifier - ts_impl, // data - get_message_typesupport_handle_function, // func - // get_type_hash_func - rosidl_get_dynamic_message_type_support_type_hash_function, - // get_type_description_func - rosidl_get_dynamic_message_type_support_type_description_function, - // get_type_description_sources_func - rosidl_get_dynamic_message_type_support_type_description_sources_function - }; -} - DynamicMessageTypeSupport::~DynamicMessageTypeSupport() -{ - // These must go first - serialization_support_.reset(); - dynamic_message_type_.reset(); - dynamic_message_.reset(); - - // Early return if type support isn't populated to avoid segfaults - if (!rosidl_message_type_support_.data) { - return; - } - - // We only partially finalize the rosidl_message_type_support->data since its pointer members are - // managed by their respective SharedPtr wrapper classes. - auto ts_impl = static_cast( - const_cast(rosidl_message_type_support_.data) - ); - rcutils_allocator_t allocator = ts_impl->allocator; - - rosidl_runtime_c__type_description__TypeDescription__fini(&ts_impl->type_description); - rosidl_runtime_c__type_description__TypeSource__Sequence__fini( - &ts_impl->type_description_sources); - allocator.deallocate(static_cast(ts_impl), allocator.state); // Always C allocated -} - - -// GETTERS ========================================================================================= -const std::string -DynamicMessageTypeSupport::get_serialization_library_identifier() const -{ - return serialization_support_->get_serialization_library_identifier(); -} - -rosidl_message_type_support_t & -DynamicMessageTypeSupport::get_rosidl_message_type_support() -{ - return rosidl_message_type_support_; -} - -const rosidl_message_type_support_t & -DynamicMessageTypeSupport::get_const_rosidl_message_type_support() -{ - return rosidl_message_type_support_; -} - -const rosidl_message_type_support_t & -DynamicMessageTypeSupport::get_const_rosidl_message_type_support() const -{ - return rosidl_message_type_support_; -} - -const rosidl_runtime_c__type_description__TypeDescription & -DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() const -{ - auto ts_impl = static_cast( - get_const_rosidl_message_type_support().data - ); - return ts_impl->type_description; -} - -DynamicSerializationSupport::SharedPtr -DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() -{ - return serialization_support_; -} - -DynamicSerializationSupport::ConstSharedPtr -DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() const -{ - return serialization_support_; -} - -DynamicMessageType::SharedPtr -DynamicMessageTypeSupport::get_shared_dynamic_message_type() -{ - return dynamic_message_type_; -} - -DynamicMessageType::ConstSharedPtr -DynamicMessageTypeSupport::get_shared_dynamic_message_type() const -{ - return dynamic_message_type_; -} - -DynamicMessage::SharedPtr -DynamicMessageTypeSupport::get_shared_dynamic_message() -{ - return dynamic_message_; -} - -DynamicMessage::ConstSharedPtr -DynamicMessageTypeSupport::get_shared_dynamic_message() const -{ - return dynamic_message_; -} +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp index 7ffde1591d..0986e09622 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -28,71 +28,14 @@ using rclcpp::dynamic_typesupport::DynamicSerializationSupport; // CONSTRUCTION ==================================================================================== DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator) -: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) {} +: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) {} // STUBBED DynamicSerializationSupport::DynamicSerializationSupport( const std::string & serialization_library_name, rcl_allocator_t allocator) : rosidl_serialization_support_( rosidl_dynamic_typesupport_get_zero_initialized_serialization_support()) -{ - rmw_ret_t ret = RMW_RET_ERROR; - - if (serialization_library_name.empty()) { - ret = rmw_serialization_support_init(NULL, &allocator, &rosidl_serialization_support_); - } else { - ret = rmw_serialization_support_init( - serialization_library_name.c_str(), &allocator, &rosidl_serialization_support_); - } - if (ret != RCL_RET_OK) { - std::string error_msg = - std::string("could not initialize new serialization support object: ") + - rcl_get_error_string().str; - rcl_reset_error(); - throw std::runtime_error(error_msg); - } -} - -DynamicSerializationSupport::DynamicSerializationSupport( - rosidl_dynamic_typesupport_serialization_support_t && rosidl_serialization_support) -: rosidl_serialization_support_(std::move(rosidl_serialization_support)) {} - -DynamicSerializationSupport::DynamicSerializationSupport( - DynamicSerializationSupport && other) noexcept -: rosidl_serialization_support_(std::exchange( - other.rosidl_serialization_support_, - rosidl_dynamic_typesupport_get_zero_initialized_serialization_support())) {} - -DynamicSerializationSupport & -DynamicSerializationSupport::operator=(DynamicSerializationSupport && other) noexcept -{ - std::swap(rosidl_serialization_support_, other.rosidl_serialization_support_); - return *this; -} +{} // STUBBED DynamicSerializationSupport::~DynamicSerializationSupport() -{ - rosidl_dynamic_typesupport_serialization_support_fini(&rosidl_serialization_support_); -} - - -// GETTERS ========================================================================================= -const std::string -DynamicSerializationSupport::get_serialization_library_identifier() const -{ - return std::string( - rosidl_dynamic_typesupport_serialization_support_get_library_identifier( - &rosidl_serialization_support_)); -} - -rosidl_dynamic_typesupport_serialization_support_t & -DynamicSerializationSupport::get_rosidl_serialization_support() -{ - return rosidl_serialization_support_; -} - -const rosidl_dynamic_typesupport_serialization_support_t & -DynamicSerializationSupport::get_rosidl_serialization_support() const -{ - return rosidl_serialization_support_; -} +{} // STUBBED diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 08b023e8e9..145c589db9 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -679,53 +679,16 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) } // DYNAMIC SUBSCRIPTION ======================================================================== - // If a subscription is dynamic, then it will use its serialization-specific dynamic data. - // - // Two cases: - // - Dynamic type subscription using dynamic type stored in its own internal type support struct - // - Non-dynamic type subscription with no stored dynamic type - // - Subscriptions of this type must be able to lookup the local message description to - // generate a dynamic type at runtime! - // - TODO(methylDragon): I won't be handling this case yet - // Take dynamic message directly from the middleware case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT: { - DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message(); - take_and_do_error_handling( - "taking a dynamic message from topic", - subscription->get_topic_name(), - // This modifies the stored dynamic data in the DynamicMessage in-place - [&]() {return subscription->take_dynamic_message(*dynamic_message, message_info);}, - [&]() {subscription->handle_dynamic_message(dynamic_message, message_info);}); - subscription->return_dynamic_message(dynamic_message); - break; + throw std::runtime_error("Unimplemented"); } // Take serialized and then convert to dynamic message case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED: { - std::shared_ptr serialized_msg = - subscription->create_serialized_message(); - - // NOTE(methylDragon): Is this clone necessary? If I'm following the pattern, it seems so. - DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message(); - take_and_do_error_handling( - "taking a serialized message from topic", - subscription->get_topic_name(), - [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, - [&]() - { - bool ret = dynamic_message->deserialize(serialized_msg->get_rcl_serialized_message()); - if (!ret) { - throw_from_rcl_error(ret, "Couldn't convert serialized message to dynamic data!"); - } - subscription->handle_dynamic_message(dynamic_message, message_info); - } - ); - subscription->return_serialized_message(serialized_msg); - subscription->return_dynamic_message(dynamic_message); - break; + throw std::runtime_error("Unimplemented"); } } return; diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index bd48efcb8c..b287321f40 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -266,6 +266,15 @@ SubscriptionBase::get_message_type_support_handle() const return type_support_; } +bool +SubscriptionBase::is_serialized() const +{ + RCLCPP_WARN( + rclcpp::get_logger( + "rclcpp"), "is_serialized() is deprecated, use get_subscription_type() instead."); + return subscription_type_ == rclcpp::SubscriptionType::SERIALIZED_MESSAGE; +} + rclcpp::SubscriptionType SubscriptionBase::get_subscription_type() const { @@ -528,21 +537,11 @@ SubscriptionBase::get_content_filter() const // DYNAMIC TYPE ================================================================================== -// TODO(methylDragon): Reorder later bool SubscriptionBase::take_dynamic_message( rclcpp::dynamic_typesupport::DynamicMessage & message_out, rclcpp::MessageInfo & message_info_out) { - rcl_ret_t ret = rcl_take_dynamic_message( - this->get_subscription_handle().get(), - &message_out.get_rosidl_dynamic_data(), - &message_info_out.get_rmw_message_info(), - nullptr); - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { - return false; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - return true; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "take_dynamic_message stubbed out for now."); + return false; } From 0fb986ba3dc4c56cec10133756f6646e224758fe Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 11 Apr 2023 17:05:23 -0700 Subject: [PATCH 4/6] Undo variable rename in test Signed-off-by: methylDragon --- rclcpp/test/rclcpp/test_generic_pubsub.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index de50be4fe2..f4cef0b757 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -89,9 +89,9 @@ class RclcppGenericNodeFixture : public Test T2 message; write_message(data, message); - rclcpp::Serialization serialization_support; + rclcpp::Serialization ser; SerializedMessage result; - serialization_support.serialize_message(&message, &result); + ser.serialize_message(&message, &result); return result; } From dfb51cccb35527de6c24ddaf8e2f70be908178f1 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 11 Apr 2023 19:51:23 -0700 Subject: [PATCH 5/6] Add new subscription type and refine code Signed-off-by: methylDragon --- rclcpp/include/rclcpp/subscription_base.hpp | 1 + .../dynamic_serialization_support.cpp | 4 ++-- rclcpp/src/rclcpp/executor.cpp | 5 +++++ rclcpp/src/rclcpp/generic_subscription.cpp | 21 ++++++++++++------- rclcpp/src/rclcpp/subscription_base.cpp | 4 ++-- 5 files changed, 24 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 1cdbda7cd4..caee46e9f3 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -65,6 +65,7 @@ class IntraProcessManager; enum class SubscriptionType : uint8_t { + INVALID = 0, // The subscription type is most likely uninitialized ROS_MESSAGE = 1, // take message as ROS message and handle as ROS message SERIALIZED_MESSAGE = 2, // take message as serialized and handle as serialized DYNAMIC_MESSAGE_DIRECT = 3, // take message as DynamicMessage and handle as DynamicMessage diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp index 0986e09622..4134dd67f8 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -31,8 +31,8 @@ DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocat : DynamicSerializationSupport::DynamicSerializationSupport("", allocator) {} // STUBBED DynamicSerializationSupport::DynamicSerializationSupport( - const std::string & serialization_library_name, - rcl_allocator_t allocator) + const std::string & /*serialization_library_name*/, + rcl_allocator_t /*allocator*/) : rosidl_serialization_support_( rosidl_dynamic_typesupport_get_zero_initialized_serialization_support()) {} // STUBBED diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 145c589db9..45059bb953 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -690,6 +690,11 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { throw std::runtime_error("Unimplemented"); } + + default: + { + throw std::runtime_error("Subscription type is not supported"); + } } return; } diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index 6e27de81da..e6e61add24 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -25,17 +25,20 @@ namespace rclcpp { -std::shared_ptr GenericSubscription::create_message() +std::shared_ptr +GenericSubscription::create_message() { return create_serialized_message(); } -std::shared_ptr GenericSubscription::create_serialized_message() +std::shared_ptr +GenericSubscription::create_serialized_message() { return std::make_shared(0); } -void GenericSubscription::handle_message( +void +GenericSubscription::handle_message( std::shared_ptr &, const rclcpp::MessageInfo &) { @@ -43,14 +46,16 @@ void GenericSubscription::handle_message( "handle_message is not implemented for GenericSubscription"); } -void GenericSubscription::handle_serialized_message( +void +GenericSubscription::handle_serialized_message( const std::shared_ptr & message, const rclcpp::MessageInfo &) { callback_(message); } -void GenericSubscription::handle_loaned_message( +void +GenericSubscription::handle_loaned_message( void * message, const rclcpp::MessageInfo & message_info) { (void) message; @@ -59,13 +64,15 @@ void GenericSubscription::handle_loaned_message( "handle_loaned_message is not implemented for GenericSubscription"); } -void GenericSubscription::return_message(std::shared_ptr & message) +void +GenericSubscription::return_message(std::shared_ptr & message) { auto typed_message = std::static_pointer_cast(message); return_serialized_message(typed_message); } -void GenericSubscription::return_serialized_message( +void +GenericSubscription::return_serialized_message( std::shared_ptr & message) { message.reset(); diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index b287321f40..2ca69abd97 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -539,8 +539,8 @@ SubscriptionBase::get_content_filter() const // DYNAMIC TYPE ================================================================================== bool SubscriptionBase::take_dynamic_message( - rclcpp::dynamic_typesupport::DynamicMessage & message_out, - rclcpp::MessageInfo & message_info_out) + rclcpp::dynamic_typesupport::DynamicMessage & /*message_out*/, + rclcpp::MessageInfo & /*message_info_out*/) { RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "take_dynamic_message stubbed out for now."); return false; From 1243d80dc55e38f049f03a156ff8db79f816cfc0 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 12 Apr 2023 11:22:01 -0700 Subject: [PATCH 6/6] Refine stubs Signed-off-by: methylDragon --- .../rclcpp/dynamic_typesupport/dynamic_message.hpp | 3 --- .../rclcpp/dynamic_typesupport/dynamic_message_type.hpp | 3 --- .../dynamic_typesupport/dynamic_message_type_builder.hpp | 3 --- .../dynamic_serialization_support.hpp | 9 --------- rclcpp/include/rclcpp/generic_subscription.hpp | 1 - rclcpp/include/rclcpp/subscription_base.hpp | 5 ++++- .../dynamic_serialization_support.cpp | 9 +++++++-- rclcpp/src/rclcpp/subscription_base.cpp | 5 +---- 8 files changed, 12 insertions(+), 26 deletions(-) diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp index 284d5a2f4b..f9586aabb7 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp @@ -32,9 +32,6 @@ namespace rclcpp namespace dynamic_typesupport { -class DynamicMessageType; -class DynamicMessageTypeBuilder; - /// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t /// STUBBED OUT class DynamicMessage : public std::enable_shared_from_this diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp index 693e7ccf08..93cbabdade 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp @@ -30,9 +30,6 @@ namespace rclcpp namespace dynamic_typesupport { -class DynamicMessage; -class DynamicMessageTypeBuilder; - /// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t` /// STUBBED OUT class DynamicMessageType : public std::enable_shared_from_this diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp index 05f6f53047..90a768ead9 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp @@ -30,9 +30,6 @@ namespace rclcpp namespace dynamic_typesupport { -class DynamicMessage; -class DynamicMessageType; - /// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *` /// STUBBED OUT class DynamicMessageTypeBuilder : public std::enable_shared_from_this diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp index 22f5a900c0..dde0710d25 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp @@ -31,15 +31,6 @@ namespace dynamic_typesupport { /// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t -/** - * This class: - * - Exposes getter methods for the struct - * - Exposes the underlying serialization support API - * - * Ownership: - * - This class, similarly to the rosidl_dynamic_typesupport_serialization_support_t, must outlive - * all downstream usages of the serialization support. - */ class DynamicSerializationSupport : public std::enable_shared_from_this { public: diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 231f4d5f9e..dd25e9b919 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -125,7 +125,6 @@ class GenericSubscription : public rclcpp::SubscriptionBase // DYNAMIC TYPE ================================================================================== - // TODO(methylDragon): Reorder later RCLCPP_PUBLIC rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type() override; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index caee46e9f3..a304e69187 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -239,7 +239,10 @@ class SubscriptionBase : public std::enable_shared_from_this const rosidl_message_type_support_t & get_message_type_support_handle() const; - /// DEPRECATED: Check if subscription takes and handles serialized messages + /// Return if the subscription is serialized + /** + * \return `true` if the subscription is serialized, `false` otherwise + */ RCLCPP_PUBLIC bool is_serialized() const; diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp index 4134dd67f8..769f5fb8cf 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -28,14 +28,19 @@ using rclcpp::dynamic_typesupport::DynamicSerializationSupport; // CONSTRUCTION ==================================================================================== DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator) -: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) {} // STUBBED +: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) +{ + throw std::runtime_error("Unimplemented"); +} DynamicSerializationSupport::DynamicSerializationSupport( const std::string & /*serialization_library_name*/, rcl_allocator_t /*allocator*/) : rosidl_serialization_support_( rosidl_dynamic_typesupport_get_zero_initialized_serialization_support()) -{} // STUBBED +{ + throw std::runtime_error("Unimplemented"); +} DynamicSerializationSupport::~DynamicSerializationSupport() {} // STUBBED diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 2ca69abd97..38271cde09 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -269,9 +269,6 @@ SubscriptionBase::get_message_type_support_handle() const bool SubscriptionBase::is_serialized() const { - RCLCPP_WARN( - rclcpp::get_logger( - "rclcpp"), "is_serialized() is deprecated, use get_subscription_type() instead."); return subscription_type_ == rclcpp::SubscriptionType::SERIALIZED_MESSAGE; } @@ -542,6 +539,6 @@ SubscriptionBase::take_dynamic_message( rclcpp::dynamic_typesupport::DynamicMessage & /*message_out*/, rclcpp::MessageInfo & /*message_info_out*/) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "take_dynamic_message stubbed out for now."); + throw std::runtime_error("Unimplemented"); return false; }