diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 6979d980f8..bfe2b74563 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 @@ -95,6 +100,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/qos.cpp src/rclcpp/event_handler.cpp src/rclcpp/qos_overriding_options.cpp + src/rclcpp/dynamic_subscription.cpp src/rclcpp/serialization.cpp src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp diff --git a/rclcpp/include/rclcpp/dynamic_subscription.hpp b/rclcpp/include/rclcpp/dynamic_subscription.hpp new file mode 100644 index 0000000000..07f6cb90ef --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_subscription.hpp @@ -0,0 +1,176 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_ +#define RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_ + +#include + +#include +#include +#include + +#include "rcpputils/shared_library.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/typesupport_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// %Subscription for messages whose type descriptions are obtained at runtime. +/** + * Since the type is not known at compile time, this is not a template, and the dynamic library + * containing type support information has to be identified and loaded based on the type name. + * + * NOTE(methylDragon): No considerations for intra-process handling are made. + */ +class DynamicSubscription : public rclcpp::SubscriptionBase +{ +public: + // cppcheck-suppress unknownMacro + RCLCPP_SMART_PTR_DEFINITIONS(DynamicSubscription) + + template> + DynamicSubscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr type_support, + const std::string & topic_name, + const rclcpp::QoS & qos, + std::function + )> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + bool use_take_dynamic_message = true) + : SubscriptionBase( + node_base, + *(type_support->get_rosidl_message_type_support()), + topic_name, + options.to_rcl_subscription_options( + qos), + options.event_callbacks, + options.use_default_callbacks, + use_take_dynamic_message ? SubscriptionType::DYNAMIC_MESSAGE_DIRECT : SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED), // NOLINT + ts_(type_support), + callback_(callback), + serialization_support_(nullptr), + dynamic_message_(nullptr), + dynamic_message_type_(nullptr) + { + if (!type_support) { + throw std::runtime_error("DynamicMessageTypeSupport cannot be nullptr!"); + } + + if (type_support->get_rosidl_message_type_support()->typesupport_identifier != + rosidl_get_dynamic_typesupport_identifier()) + { + throw std::runtime_error( + "DynamicSubscription must use dynamic type introspection type support!"); + } + + serialization_support_ = type_support->get_shared_dynamic_serialization_support(); + dynamic_message_type_ = type_support->get_shared_dynamic_message_type()->clone_shared(); + dynamic_message_ = type_support->get_shared_dynamic_message()->clone_shared(); + } + + // TODO(methylDragon): + /// Deferred type description constructor, only usable if the middleware implementation supports + /// type discovery + + RCLCPP_PUBLIC + virtual ~DynamicSubscription() = default; + + // Same as create_serialized_message() as the subscription is to serialized_messages only + RCLCPP_PUBLIC + std::shared_ptr create_message() override; + + RCLCPP_PUBLIC + std::shared_ptr create_serialized_message() override; + + /// Cast the message to a rclcpp::SerializedMessage and call the callback. + RCLCPP_PUBLIC + void handle_message( + std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override; + + /// Handle dispatching rclcpp::SerializedMessage to user callback. + RCLCPP_PUBLIC + void + handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) override; + + /// This function is currently not implemented. + RCLCPP_PUBLIC + void handle_loaned_message( + void * loaned_message, const rclcpp::MessageInfo & message_info) override; + + // Same as return_serialized_message() as the subscription is to serialized_messages only + RCLCPP_PUBLIC + void return_message(std::shared_ptr & message) override; + + 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(DynamicSubscription) + + rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr ts_; + std::function + )> callback_; + + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr serialization_support_; + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr dynamic_message_; + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr dynamic_message_type_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_SUBSCRIPTION_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 new file mode 100644 index 0000000000..eccdbccf4d --- /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_.get(), 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_.get(), 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_.get(), 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_.get(), 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_.get(), 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_.get(), 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_.get(), 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_.get(), 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_.get(), 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..f28f8ce293 --- /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_.get(), \ + 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_.get(), \ + 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_.get(), \ + 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_.get(), \ + 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..4b137d71ec --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp @@ -0,0 +1,430 @@ +// 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 "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: + * - 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! + * + * 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 + RCLCPP_PUBLIC + explicit DynamicMessage(std::shared_ptr dynamic_type_builder); + + /// Construct a new DynamicMessage with the provided dynamic type + RCLCPP_PUBLIC + explicit DynamicMessage(std::shared_ptr dynamic_type); + + /// Assume ownership of raw pointer + RCLCPP_PUBLIC + DynamicMessage( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data); + + /// Copy shared pointer + RCLCPP_PUBLIC + DynamicMessage( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_data); + + /// Loaning constructor + /// Must only be called with raw ptr 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 + + /// Copy constructor + RCLCPP_PUBLIC + DynamicMessage(const DynamicMessage & other); + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessage(DynamicMessage && other) noexcept; + + /// Copy assignment + RCLCPP_PUBLIC + DynamicMessage & operator=(const DynamicMessage & other); + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessage & operator=(DynamicMessage && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicMessage(); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_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 + std::shared_ptr + get_shared_rosidl_dynamic_data(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_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() const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + clone_shared() const; + + RCLCPP_PUBLIC + DynamicMessage + init_from_type(DynamicMessageType & type) const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + init_from_type_shared(DynamicMessageType & type) const; + + RCLCPP_PUBLIC + bool + equals(const DynamicMessage & other) const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + loan_value(rosidl_dynamic_typesupport_member_id_t id); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + loan_value(const std::string & name); + + 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); + + RCLCPP_PUBLIC + DynamicMessage + get_complex_value(const std::string & name); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_complex_value_shared(const std::string & name); + + 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_; + + std::shared_ptr rosidl_dynamic_data_; + + bool is_loaned_; + + // Used for returning the loaned value, and lifetime management + DynamicMessage::SharedPtr parent_data_; + +private: + 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..316a17fa71 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp @@ -0,0 +1,207 @@ +// 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 "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: + * - 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 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 + RCLCPP_PUBLIC + explicit DynamicMessageType(std::shared_ptr dynamic_type_builder); + + /// Assume ownership of raw pointer + RCLCPP_PUBLIC + DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type); + + /// Copy shared pointer + RCLCPP_PUBLIC + DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_type); + + /// From description + RCLCPP_PUBLIC + DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description); + + /// Copy constructor + RCLCPP_PUBLIC + DynamicMessageType(const DynamicMessageType & other); + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessageType(DynamicMessageType && other) noexcept; + + /// Copy assignment + RCLCPP_PUBLIC + DynamicMessageType & operator=(const DynamicMessageType & other); + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessageType & operator=(DynamicMessageType && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicMessageType(); + + /// Swaps the serialization support if `serialization_support` is populated + RCLCPP_PUBLIC + void + init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + DynamicSerializationSupport::SharedPtr serialization_support = nullptr); + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_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 + std::shared_ptr + get_shared_rosidl_dynamic_type(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_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() const; + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + clone_shared() const; + + RCLCPP_PUBLIC + bool + equals(const DynamicMessageType & other) const; + + RCLCPP_PUBLIC + DynamicMessage + build_dynamic_message(); + + RCLCPP_PUBLIC + std::shared_ptr + build_dynamic_message_shared(); + +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_; + + std::shared_ptr rosidl_dynamic_type_; + +private: + 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..d25382e98c --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp @@ -0,0 +1,414 @@ +// 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 "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 + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name); + + /// Assume ownership of raw pointer + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_builder_t * dynamic_type_builder); + + /// Copy shared pointer + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr dynamic_type_builder); + + /// Copy constructor + RCLCPP_PUBLIC + DynamicMessageTypeBuilder(const DynamicMessageTypeBuilder & other); + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept; + + /// Copy assignment + RCLCPP_PUBLIC + DynamicMessageTypeBuilder & operator=(const DynamicMessageTypeBuilder & other); + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessageTypeBuilder & operator=(DynamicMessageTypeBuilder && other) noexcept; + + /// From description + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description); + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeBuilder(); + + /// Swaps the serialization support if serialization_support is populated + RCLCPP_PUBLIC + void + init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + DynamicSerializationSupport::SharedPtr serialization_support = nullptr); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_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 + std::shared_ptr + get_shared_rosidl_dynamic_type_builder(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_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() const; + + RCLCPP_PUBLIC + DynamicMessageTypeBuilder::SharedPtr + clone_shared() const; + + RCLCPP_PUBLIC + void + clear(); + + RCLCPP_PUBLIC + DynamicMessage + build_dynamic_message(); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + build_dynamic_message_shared(); + + RCLCPP_PUBLIC + DynamicMessageType + build_dynamic_message_type(); + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + build_dynamic_message_type_shared(); + + + // 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_; + + std::shared_ptr rosidl_dynamic_type_builder_; + +private: + RCLCPP_PUBLIC + DynamicMessageTypeBuilder(); + + RCLCPP_PUBLIC + void + init_from_serialization_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name); + + 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..ea8764fa29 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp @@ -0,0 +1,205 @@ +// 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 "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_create()`, + * because this class will manage the lifetimes for you. + * + * Do NOT call rcl_dynamic_message_type_support_handle_destroy!! + * + * This class: + * - Manages the lifetime of the raw pointer. + * - Exposes getter methods to get the raw pointer and shared pointers + * - 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 = ""); + + /// 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_create()`) + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description); + + /// 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); + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeSupport(); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_library_identifier() const; + + RCLCPP_PUBLIC + rosidl_message_type_support_t * + get_rosidl_message_type_support(); + + RCLCPP_PUBLIC + const rosidl_message_type_support_t * + get_rosidl_message_type_support() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_message_type_support(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_message_type_support() const; + + RCLCPP_PUBLIC + rosidl_runtime_c__type_description__TypeDescription * + get_rosidl_runtime_c_type_description(); + + RCLCPP_PUBLIC + const rosidl_runtime_c__type_description__TypeDescription * + get_rosidl_runtime_c_type_description() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_runtime_c_type_description(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_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) + + DynamicSerializationSupport::SharedPtr serialization_support_; + DynamicMessageType::SharedPtr dynamic_message_type_; + DynamicMessage::SharedPtr dynamic_message_; + std::shared_ptr description_; + + std::shared_ptr rosidl_message_type_support_; + +private: + RCLCPP_PUBLIC + DynamicMessageTypeSupport(); + + RCLCPP_PUBLIC + void + manage_description_(rosidl_runtime_c__type_description__TypeDescription * description); + + RCLCPP_PUBLIC + void + init_dynamic_message_type_( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription * description); + + RCLCPP_PUBLIC + void + init_dynamic_message_(DynamicMessageType::SharedPtr dynamic_type); + + // By aggregation + RCLCPP_PUBLIC + void + init_rosidl_message_type_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + rosidl_runtime_c__type_description__TypeDescription * description); +}; + + +} // 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..6e62a4c931 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp @@ -0,0 +1,109 @@ +// 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 "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: + * - 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, 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 + DynamicSerializationSupport(); + + /// Get the rmw middleware implementation specific serialization support (configured by name) + RCLCPP_PUBLIC + explicit DynamicSerializationSupport(const std::string & serialization_library_name); + + /// Assume ownership of raw pointer + RCLCPP_PUBLIC + explicit DynamicSerializationSupport( + rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support); + + /// Copy shared pointer + RCLCPP_PUBLIC + DynamicSerializationSupport( + std::shared_ptr 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_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; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_serialization_support(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_serialization_support() const; + +protected: + RCLCPP_DISABLE_COPY(DynamicSerializationSupport) + + std::shared_ptr rosidl_serialization_support_; +}; + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ 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/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/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..242bc03533 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 @@ -568,6 +622,10 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::node_interfaces::NodeBaseInterface * const node_base_; + // TODO(methylDragon): Remove if we don't need this + // rclcpp::node_interfaces::NodeGraphInterface * const node_graph_; + // rclcpp::node_interfaces::NodeServicesInterface * const node_services_; + std::shared_ptr node_handle_; std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; @@ -587,7 +645,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/dynamic_subscription.cpp b/rclcpp/src/rclcpp/dynamic_subscription.cpp new file mode 100644 index 0000000000..cdb87cbfea --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_subscription.cpp @@ -0,0 +1,113 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/dynamic_subscription.hpp" + +#include +#include + +#include "rcl/subscription.h" + +#include "rclcpp/exceptions.hpp" + +namespace rclcpp +{ + +std::shared_ptr DynamicSubscription::create_message() +{ + return create_serialized_message(); +} + +std::shared_ptr DynamicSubscription::create_serialized_message() +{ + return std::make_shared(0); +} + +void DynamicSubscription::handle_message(std::shared_ptr &, const rclcpp::MessageInfo &) +{ + throw rclcpp::exceptions::UnimplementedError( + "handle_message is not implemented for DynamicSubscription"); +} + +void DynamicSubscription::handle_serialized_message( + const std::shared_ptr &, const rclcpp::MessageInfo &) +{ + throw rclcpp::exceptions::UnimplementedError( + "handle_serialized_message is not implemented for DynamicSubscription"); +} + +void DynamicSubscription::handle_loaned_message(void *, const rclcpp::MessageInfo &) +{ + throw rclcpp::exceptions::UnimplementedError( + "handle_loaned_message is not implemented for DynamicSubscription"); +} + +void DynamicSubscription::return_message(std::shared_ptr & message) +{ + auto typed_message = std::static_pointer_cast(message); + return_serialized_message(typed_message); +} + +void DynamicSubscription::return_serialized_message( + std::shared_ptr & message) +{ + message.reset(); +} + + +// DYNAMIC TYPE ==================================================================================== +// TODO(methylDragon): Re-order later + +// Does not clone +rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr +DynamicSubscription::get_shared_dynamic_message_type() +{ + return dynamic_message_type_; +} + +// Does not clone +rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr +DynamicSubscription::get_shared_dynamic_message() +{ + return dynamic_message_; +} + +rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr +DynamicSubscription::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + +rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr +DynamicSubscription::create_dynamic_message() +{ + return dynamic_message_->init_from_type_shared(*dynamic_message_type_); +} + +void +DynamicSubscription::return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) +{ + message.reset(); +} + +void DynamicSubscription::handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) +{ + (void) message_info; + callback_(message, ts_->get_shared_rosidl_runtime_c_type_description()); +} + +} // namespace rclcpp 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..68249d622b --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp @@ -0,0 +1,838 @@ +// 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 "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) +: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(nullptr), + is_loaned_(false), + parent_data_(nullptr) +{ + 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(); + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("dynamic type builder cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type_builder( + rosidl_dynamic_type_builder, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) { + throw std::runtime_error("could not create new dynamic data object from dynamic type builder"); + } + + rosidl_dynamic_data_.reset( + rosidl_dynamic_data, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void { + rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data); + }); +} + + +DynamicMessage::DynamicMessage(const DynamicMessageType::SharedPtr dynamic_type) +: serialization_support_(dynamic_type->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(nullptr), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic type could not bind serialization support!"); + } + + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = + dynamic_type->get_rosidl_dynamic_type(); + if (!rosidl_dynamic_type) { + throw std::runtime_error("dynamic type cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type( + rosidl_dynamic_type, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) { + throw std::runtime_error( + std::string("could not create new dynamic data object from dynamic type") + + rcl_get_error_string().str); + } + + rosidl_dynamic_data_.reset( + rosidl_dynamic_data, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void { + rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data); + }); +} + + +DynamicMessage::DynamicMessage( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data) +: serialization_support_(serialization_support), + rosidl_dynamic_data_(nullptr), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!rosidl_dynamic_data) { + throw std::runtime_error("rosidl dynamic data cannot be 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!"); + } + } + + rosidl_dynamic_data_.reset( + rosidl_dynamic_data, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void { + rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data); + }); +} + + +DynamicMessage::DynamicMessage( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_data) +: serialization_support_(serialization_support), + rosidl_dynamic_data_(rosidl_dynamic_data), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!rosidl_dynamic_data) { + throw std::runtime_error("rosidl dynamic data cannot be 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_(nullptr), + is_loaned_(true), + parent_data_(nullptr) +{ + if (!parent_data) { + throw std::runtime_error("parent dynamic data cannot be nullptr!"); + } + if (!rosidl_loaned_data) { + throw std::runtime_error("loaned rosidl 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!"); + } + } + + rosidl_dynamic_data_.reset( + rosidl_loaned_data, + // Custom no-op deleter + [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void { + // Data fini and destruction is deferred to return_loaned_value() + (void) rosidl_dynamic_data; + }); + parent_data_ = parent_data; +} + + +DynamicMessage::DynamicMessage(const DynamicMessage & other) +: enable_shared_from_this(), + serialization_support_(nullptr), + rosidl_dynamic_data_(nullptr), + is_loaned_(false), + parent_data_(nullptr) +{ + DynamicMessage out = other.clone(); + // We don't copy is_loaned_ or parent_data_ because it's a fresh copy now + std::swap(serialization_support_, out.serialization_support_); + std::swap(rosidl_dynamic_data_, out.rosidl_dynamic_data_); +} + + +DynamicMessage::DynamicMessage(DynamicMessage && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_data_(std::exchange(other.rosidl_dynamic_data_, nullptr)), + is_loaned_(other.is_loaned_), + parent_data_(std::exchange(other.parent_data_, nullptr)) +{} + + +DynamicMessage & +DynamicMessage::operator=(const DynamicMessage & other) +{ + return *this = DynamicMessage(other); +} + + +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 (!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) +{ + bool out = true; + + if (serialization_support.get_library_identifier() != std::string( + rosidl_dynamic_type_data.serialization_support->library_identifier)) + { + RCUTILS_LOG_ERROR("serialization support library identifier does not match dynamic data's"); + out = false; + } + + // TODO(methylDragon): Can I do this?? Is it portable? + if (serialization_support.get_rosidl_serialization_support() != + rosidl_dynamic_type_data.serialization_support) + { + RCUTILS_LOG_ERROR("serialization support pointer does not match dynamic data's"); + out = false; + } + + return out; +} + + +// GETTERS ======================================================================================= +const std::string +DynamicMessage::get_library_identifier() const +{ + return std::string(rosidl_dynamic_data_->serialization_support->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_.get(); +} + + +const rosidl_dynamic_typesupport_dynamic_data_t * +DynamicMessage::get_rosidl_dynamic_data() const +{ + return rosidl_dynamic_data_.get(); +} + + +std::shared_ptr +DynamicMessage::get_shared_rosidl_dynamic_data() +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_data_.get()); +} + + +std::shared_ptr +DynamicMessage::get_shared_rosidl_dynamic_data() const +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_data_.get()); +} + + +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() const +{ + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone( + get_rosidl_dynamic_data(), &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) { + throw std::runtime_error( + std::string("could not clone dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage(serialization_support_, rosidl_dynamic_data); +} + + +DynamicMessage::SharedPtr +DynamicMessage::clone_shared() const +{ + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone( + get_rosidl_dynamic_data(), &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) { + throw std::runtime_error( + std::string("could not clone dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage::make_shared(serialization_support_, rosidl_dynamic_data); +} + + +DynamicMessage +DynamicMessage::init_from_type(DynamicMessageType & type) const +{ + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type( + type.get_rosidl_dynamic_type(), &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) { + throw std::runtime_error("could not create new dynamic data object from dynamic type"); + } + return DynamicMessage(serialization_support_, rosidl_dynamic_data); +} + + +DynamicMessage::SharedPtr +DynamicMessage::init_from_type_shared(DynamicMessageType & type) const +{ + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type( + type.get_rosidl_dynamic_type(), &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) { + throw std::runtime_error("could not create new dynamic data object from dynamic type"); + } + return DynamicMessage::make_shared(serialization_support_, rosidl_dynamic_data); +} + + +bool +DynamicMessage::equals(const DynamicMessage & other) const +{ + if (get_library_identifier() != other.get_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) +{ + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_loan_value( + get_rosidl_dynamic_data(), id, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) { + throw std::runtime_error( + std::string("could not loan dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage::make_shared(shared_from_this(), rosidl_dynamic_data); +} + + +DynamicMessage::SharedPtr +DynamicMessage::loan_value(const std::string & name) +{ + return loan_value(get_member_id(name)); +} + + +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) +{ + bool success; + rcutils_ret_t ret = + rosidl_dynamic_typesupport_dynamic_data_serialize(get_rosidl_dynamic_data(), buffer, &success); + if (ret != RCUTILS_RET_OK || !success) { + throw std::runtime_error( + std::string("could serialize loan dynamic data: ") + rcl_get_error_string().str); + } + return success; +} + + +bool +DynamicMessage::deserialize(rcl_serialized_message_t * buffer) +{ + bool success; + rcutils_ret_t ret = + rosidl_dynamic_typesupport_dynamic_data_deserialize( + get_rosidl_dynamic_data(), buffer, + &success); + if (ret != RCUTILS_RET_OK || !success) { + throw std::runtime_error( + std::string("could deserialize loan dynamic data: ") + rcl_get_error_string().str); + } + return success; +} + + +// 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) +{ + rosidl_dynamic_typesupport_dynamic_data_t * out_ptr = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_complex_value( + get_rosidl_dynamic_data(), id, &out_ptr); + return DynamicMessage(get_shared_dynamic_serialization_support(), out_ptr); +} + + +DynamicMessage +DynamicMessage::get_complex_value(const std::string & name) +{ + return get_complex_value(get_member_id(name)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id) +{ + rosidl_dynamic_typesupport_dynamic_data_t * out_ptr = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_complex_value( + get_rosidl_dynamic_data(), id, &out_ptr); + return DynamicMessage::make_shared(get_shared_dynamic_serialization_support(), out_ptr); +} + + +DynamicMessage::SharedPtr +DynamicMessage::get_complex_value_shared(const std::string & name) +{ + return get_complex_value_shared(get_member_id(name)); +} + + +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..f5a5d539d7 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp @@ -0,0 +1,338 @@ +// 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 "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) +: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), + rosidl_dynamic_type_(nullptr) +{ + 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(); + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("dynamic type builder cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_create_from_dynamic_type_builder( + rosidl_dynamic_type_builder, &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) { + throw std::runtime_error("could not create new dynamic type object"); + } + + rosidl_dynamic_type_.reset( + rosidl_dynamic_type, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void { + rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type); + }); +} + + +DynamicMessageType::DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type) +: serialization_support_(serialization_support), rosidl_dynamic_type_(nullptr) +{ + if (!rosidl_dynamic_type) { + throw std::runtime_error("rosidl dynamic type cannot be nullptr!"); + } + 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!"); + } + } + + rosidl_dynamic_type_.reset( + rosidl_dynamic_type, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void { + rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type); + }); +} + + +DynamicMessageType::DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_type) +: serialization_support_(serialization_support), rosidl_dynamic_type_(rosidl_dynamic_type) +{ + if (!rosidl_dynamic_type) { + throw std::runtime_error("rosidl dynamic type cannot be nullptr!"); + } + 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) +: serialization_support_(serialization_support), rosidl_dynamic_type_(nullptr) +{ + init_from_description(description, serialization_support); +} + + +DynamicMessageType::DynamicMessageType(const DynamicMessageType & other) +: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_(nullptr) +{ + DynamicMessageType out = other.clone(); + std::swap(serialization_support_, out.serialization_support_); + std::swap(rosidl_dynamic_type_, out.rosidl_dynamic_type_); +} + + +DynamicMessageType::DynamicMessageType(DynamicMessageType && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_type_(std::exchange(other.rosidl_dynamic_type_, nullptr)) {} + + +DynamicMessageType & +DynamicMessageType::operator=(const DynamicMessageType & other) +{ + return *this = DynamicMessageType(other); +} + + +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() {} + + +void +DynamicMessageType::init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + 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 = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_create_from_description( + serialization_support_->get_rosidl_serialization_support(), &description, &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) { + throw std::runtime_error("could not create new dynamic type object"); + } + + rosidl_dynamic_type_.reset( + rosidl_dynamic_type, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void { + rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type); + }); +} + + +bool +DynamicMessageType::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type) +{ + bool out = true; + + if (serialization_support.get_library_identifier() != std::string( + rosidl_dynamic_type.serialization_support->library_identifier)) + { + RCUTILS_LOG_ERROR( + "serialization support library identifier does not match dynamic type's"); + out = false; + } + + // TODO(methylDragon): Can I do this?? Is it portable? + if (serialization_support.get_rosidl_serialization_support() != + rosidl_dynamic_type.serialization_support) + { + RCUTILS_LOG_ERROR( + "serialization support pointer does not match dynamic type's"); + out = false; + } + + return out; +} + + +// GETTERS ========================================================================================= +const std::string +DynamicMessageType::get_library_identifier() const +{ + return std::string(rosidl_dynamic_type_->serialization_support->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( + rosidl_dynamic_type_.get(), &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_.get(); +} + + +const rosidl_dynamic_typesupport_dynamic_type_t * +DynamicMessageType::get_rosidl_dynamic_type() const +{ + return rosidl_dynamic_type_.get(); +} + + +std::shared_ptr +DynamicMessageType::get_shared_rosidl_dynamic_type() +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_type_.get()); +} + + +std::shared_ptr +DynamicMessageType::get_shared_rosidl_dynamic_type() const +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_type_.get()); +} + + +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() const +{ + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone( + get_rosidl_dynamic_type(), &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) { + throw std::runtime_error( + std::string("could not clone dynamic type: ") + rcl_get_error_string().str); + } + return DynamicMessageType(serialization_support_, rosidl_dynamic_type); +} + + +DynamicMessageType::SharedPtr +DynamicMessageType::clone_shared() const +{ + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone( + get_rosidl_dynamic_type(), &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) { + throw std::runtime_error( + std::string("could not clone dynamic type: ") + rcl_get_error_string().str); + } + return DynamicMessageType::make_shared(serialization_support_, rosidl_dynamic_type); +} + + +bool +DynamicMessageType::equals(const DynamicMessageType & other) const +{ + if (get_library_identifier() != other.get_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() +{ + return DynamicMessage(shared_from_this()); +} + + +DynamicMessage::SharedPtr +DynamicMessageType::build_dynamic_message_shared() +{ + return DynamicMessage::make_shared(shared_from_this()); +} 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..37ed9964d3 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp @@ -0,0 +1,675 @@ +// 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 "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) +: serialization_support_(serialization_support), rosidl_dynamic_type_builder_(nullptr) +{ + init_from_serialization_support_(serialization_support, name); + if (!rosidl_dynamic_type_builder_) { + throw std::runtime_error("could not create new dynamic type builder object"); + } +} + + +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_(nullptr) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("rosidl dynamic type builder 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!"); + } + + rosidl_dynamic_type_builder_.reset( + rosidl_dynamic_type_builder, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void { + rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder); + }); +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_type_builder) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_(rosidl_dynamic_type_builder) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("rosidl dynamic type builder cannot be nullptr!"); + } + if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type_builder.get())) { + 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) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_(nullptr) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + init_from_description(description, serialization_support); +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(const DynamicMessageTypeBuilder & other) +: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_builder_(nullptr) +{ + DynamicMessageTypeBuilder out = other.clone(); + std::swap(serialization_support_, out.serialization_support_); + std::swap(rosidl_dynamic_type_builder_, out.rosidl_dynamic_type_builder_); +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_type_builder_(std::exchange(other.rosidl_dynamic_type_builder_, nullptr)) {} + + +DynamicMessageTypeBuilder & +DynamicMessageTypeBuilder::operator=(const DynamicMessageTypeBuilder & other) +{ + return *this = DynamicMessageTypeBuilder(other); +} + + +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() {} + + +void +DynamicMessageTypeBuilder::init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + 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 = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_create_from_description( + serialization_support_->get_rosidl_serialization_support(), &description, + &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) { + throw std::runtime_error("could not create new dynamic type builder object"); + } + + rosidl_dynamic_type_builder_.reset( + rosidl_dynamic_type_builder, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void { + rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder); + }); +} + + +void +DynamicMessageTypeBuilder::init_from_serialization_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name) +{ + 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 = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_create( + serialization_support->get_rosidl_serialization_support(), + name.c_str(), name.size(), + &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); + } + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("could not init dynamic type builder object"); + } + + rosidl_dynamic_type_builder_.reset( + rosidl_dynamic_type_builder, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void { + rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder); + }); +} + + +bool +DynamicMessageTypeBuilder::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_builder_t & rosidl_dynamic_type_builder) +{ + bool out = true; + + if (serialization_support.get_library_identifier() != std::string( + rosidl_dynamic_type_builder.serialization_support->library_identifier)) + { + RCUTILS_LOG_ERROR( + "serialization support library identifier does not match dynamic type builder's"); + out = false; + } + + // TODO(methylDragon): Can I do this?? Is it portable? + if (serialization_support.get_rosidl_serialization_support() != + rosidl_dynamic_type_builder.serialization_support) + { + RCUTILS_LOG_ERROR( + "serialization support pointer does not match dynamic type builder's"); + out = false; + } + + return out; +} + + +// GETTERS ======================================================================================= +const std::string +DynamicMessageTypeBuilder::get_library_identifier() const +{ + return std::string(rosidl_dynamic_type_builder_->serialization_support->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_.get(); +} + + +const rosidl_dynamic_typesupport_dynamic_type_builder_t * +DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() const +{ + return rosidl_dynamic_type_builder_.get(); +} + + +std::shared_ptr +DynamicMessageTypeBuilder::get_shared_rosidl_dynamic_type_builder() +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_type_builder_.get()); +} + + +std::shared_ptr +DynamicMessageTypeBuilder::get_shared_rosidl_dynamic_type_builder() const +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_type_builder_.get()); +} + + +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() const +{ + rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone( + get_rosidl_dynamic_type_builder(), &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) { + throw std::runtime_error( + std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str); + } + return DynamicMessageTypeBuilder(serialization_support_, rosidl_dynamic_type_builder); +} + + +DynamicMessageTypeBuilder::SharedPtr +DynamicMessageTypeBuilder::clone_shared() const +{ + rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone( + get_rosidl_dynamic_type_builder(), &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) { + throw std::runtime_error( + std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str); + } + return DynamicMessageTypeBuilder::make_shared( + serialization_support_, rosidl_dynamic_type_builder); +} + + +void +DynamicMessageTypeBuilder::clear() +{ + 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); + if (!rosidl_dynamic_type_builder_) { + throw std::runtime_error("could not create new dynamic type builder object"); + } +} + + +DynamicMessage +DynamicMessageTypeBuilder::build_dynamic_message() +{ + return DynamicMessage(shared_from_this()); +} + + +DynamicMessage::SharedPtr +DynamicMessageTypeBuilder::build_dynamic_message_shared() +{ + return DynamicMessage::make_shared(shared_from_this()); +} + + +DynamicMessageType +DynamicMessageTypeBuilder::build_dynamic_message_type() +{ + return DynamicMessageType(shared_from_this()); +} + + +DynamicMessageType::SharedPtr +DynamicMessageTypeBuilder::build_dynamic_message_type_shared() +{ + return DynamicMessageType::make_shared(shared_from_this()); +} + + +// 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..802a46d560 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp @@ -0,0 +1,465 @@ +// 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 "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) +: serialization_support_(nullptr), + dynamic_message_type_(nullptr), + dynamic_message_(nullptr), + description_(nullptr), + rosidl_message_type_support_(nullptr) +{ + rosidl_message_type_support_t * ts = nullptr; + rcl_ret_t ret; + + if (serialization_library_name.empty()) { + ret = rcl_dynamic_message_type_support_handle_create(nullptr, &description, &ts); + } else { + ret = rcl_dynamic_message_type_support_handle_create( + serialization_library_name.c_str(), &description, &ts); + } + if (ret != RCL_RET_OK) { + throw std::runtime_error("error initializing rosidl message type support"); + } + if (!ts) { + throw std::runtime_error("could not init rosidl message type support"); + } + if (!ts->data) { + throw std::runtime_error("could not init rosidl message type support impl"); + } + if (ts->typesupport_identifier != rosidl_get_dynamic_typesupport_identifier()) { + throw std::runtime_error("rosidl message type support is of the wrong type"); + } + + // NOTE(methylDragon): Not technically const correct, but since it's a const void *, + // we do it anyway... + auto ts_impl = static_cast(ts->data); + + // NOTE(methylDragon): We don't destroy the rosidl_message_type_support->data since its members + // are managed by the passed in SharedPtr wrapper classes. We just delete it. + rosidl_message_type_support_.reset( + ts, + [](rosidl_message_type_support_t * ts) -> void { + auto ts_impl = static_cast(ts->data); + auto allocator = rcl_get_default_allocator(); + + // These are all C allocated + allocator.deallocate(ts_impl->type_hash, &allocator.state); + allocator.deallocate( + const_cast(ts_impl), &allocator.state); + allocator.deallocate(ts, &allocator.state); + } + ); + + manage_description_(ts_impl->type_description); + serialization_support_ = DynamicSerializationSupport::make_shared(ts_impl->serialization_support); + + dynamic_message_type_ = DynamicMessageType::make_shared( + get_shared_dynamic_serialization_support(), ts_impl->dynamic_message_type); + + dynamic_message_ = DynamicMessage::make_shared( + get_shared_dynamic_serialization_support(), ts_impl->dynamic_message); + + if (!rosidl_message_type_support_) { + throw std::runtime_error("could not init rosidl message type support."); + } +} + + +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description) +: serialization_support_(serialization_support), + dynamic_message_type_(nullptr), + dynamic_message_(nullptr), + description_(nullptr), + rosidl_message_type_support_(nullptr) +{ + // Check null + if (!serialization_support) { + throw std::runtime_error("serialization_support cannot be nullptr."); + } + + rosidl_message_type_support_t * ts = nullptr; + + auto type_hash = std::make_unique(); + 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.get()); + if (hash_ret != RCL_RET_OK || !type_hash) { + throw std::runtime_error("failed to get type hash"); + } + + rcutils_ret_t ret = rosidl_dynamic_message_type_support_handle_create( + serialization_support->get_rosidl_serialization_support(), + type_hash.get(), // type_hash + &description, // type_description + nullptr, // type_description_sources (not implemented for dynamic types) + &ts); + if (ret != RCUTILS_RET_OK || !ts) { + throw std::runtime_error("could not init rosidl message type support"); + } + if (!ts->data) { + throw std::runtime_error("could not init rosidl message type support impl"); + } + if (ts->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(ts->data); + + // NOTE(methylDragon): We don't finalize the rosidl_message_type_support->data since its members + // are managed by the passed in SharedPtr wrapper classes. We just delete it. + rosidl_message_type_support_.reset( + ts, + [](rosidl_message_type_support_t * ts) -> void { + auto ts_impl = static_cast(ts->data); + + // These are allocated with new + delete ts_impl->type_hash; // Only because we should've allocated it here + delete ts_impl; + delete ts; + } + ); + manage_description_(ts_impl->type_description); + + dynamic_message_type_ = DynamicMessageType::make_shared( + get_shared_dynamic_serialization_support(), ts_impl->dynamic_message_type); + + dynamic_message_ = DynamicMessage::make_shared( + get_shared_dynamic_serialization_support(), ts_impl->dynamic_message); + + if (!rosidl_message_type_support_) { + throw std::runtime_error("could not init rosidl message type support."); + } + + type_hash.release(); +} + + +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + const rosidl_runtime_c__type_description__TypeDescription & description) +: serialization_support_(serialization_support), + dynamic_message_type_(dynamic_message_type), + dynamic_message_(dynamic_message), + description_(nullptr), + rosidl_message_type_support_(nullptr) +{ + // 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."); + } + + description_.reset( + new rosidl_runtime_c__type_description__TypeDescription(), + [](rosidl_runtime_c__type_description__TypeDescription * description) -> void { + rosidl_runtime_c__type_description__TypeDescription__destroy(description); + }); + if (!description_) { + throw std::runtime_error("could not init type description."); + } + if (!rosidl_runtime_c__type_description__TypeDescription__copy( + &description, + description_.get())) + { + throw std::runtime_error("could not copy type description."); + } + + // Check identifiers + if (serialization_support->get_library_identifier() != + dynamic_message_type->get_library_identifier()) + { + throw std::runtime_error( + "serialization support library identifier does not match " + "dynamic message type library identifier."); + } + if (dynamic_message_type->get_library_identifier() != dynamic_message->get_library_identifier()) { + throw std::runtime_error( + "dynamic message type library identifier does not match " + "dynamic message library identifier."); + } + + // Check pointers + /* *INDENT-OFF* */ + if (serialization_support->get_rosidl_serialization_support() != + dynamic_message_type + ->get_shared_dynamic_serialization_support() + ->get_rosidl_serialization_support()) + { + throw std::runtime_error("serialization support pointer does not match dynamic message type's"); + } + if (dynamic_message_type + ->get_shared_dynamic_serialization_support() + ->get_rosidl_serialization_support() != + dynamic_message_type + ->get_shared_dynamic_serialization_support() + ->get_rosidl_serialization_support()) + { + throw std::runtime_error("serialization support does not match pointer dynamic message type's"); + } + /* *INDENT-ON* */ + + init_rosidl_message_type_support_( + serialization_support_, dynamic_message_type_, dynamic_message_, description_.get()); + if (!rosidl_message_type_support_) { + throw std::runtime_error("could not init rosidl message type support."); + } +} + + +DynamicMessageTypeSupport::~DynamicMessageTypeSupport() {} + + +void +DynamicMessageTypeSupport::manage_description_( + rosidl_runtime_c__type_description__TypeDescription * description) +{ + if (!description) { + throw std::runtime_error("description cannot be nullptr"); + } + description_.reset( + description, + [](rosidl_runtime_c__type_description__TypeDescription * description) -> void { + rosidl_runtime_c__type_description__TypeDescription__destroy(description); + }); +} + + +// This looks like rmw_`dynamic_message_type_support_handle_create()`, but instead just aggregates +// already initialized objects +void +DynamicMessageTypeSupport::init_rosidl_message_type_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + rosidl_runtime_c__type_description__TypeDescription * description) +{ + bool middleware_supports_type_discovery = rmw_feature_supported( + RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY); + + if (!middleware_supports_type_discovery && !description) { + throw std::runtime_error( + "Middleware does not support type discovery! Deferred dynamic type" + "message type support will never be populated! You must provide a type " + "description."); + return; + } + + auto type_hash = std::make_unique(); + 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.get()); + if (hash_ret != RCL_RET_OK || !type_hash) { + throw std::runtime_error("failed to get type hash"); + } + + rosidl_dynamic_message_type_support_impl_t * ts_impl = + new rosidl_dynamic_message_type_support_impl_t { + type_hash.get(), // type_hash + description, // type_description + nullptr, // NOTE(methylDragon): Not supported for now // type_description_sources + serialization_support->get_rosidl_serialization_support(), // serialization_support + dynamic_message_type->get_rosidl_dynamic_type(), // dynamic_message_type + dynamic_message->get_rosidl_dynamic_data() // dynamic_message + }; + if (!ts_impl) { + throw std::runtime_error( + "Could not allocate rosidl_dynamic_message_type_support_impl_t struct"); + return; + } + + // NOTE(methylDragon): We don't finalize the rosidl_message_type_support->data since its members + // are managed by the passed in SharedPtr wrapper classes. We just delete it. + rosidl_message_type_support_.reset( + new rosidl_message_type_support_t{ + 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 + }, + [](rosidl_message_type_support_t * ts) -> void { + auto ts_impl = static_cast(ts->data); + auto allocator = rcl_get_default_allocator(); + // Only because we should've allocated it here (also it's C allocated) + allocator.deallocate(ts_impl->type_hash, &allocator.state); + delete ts_impl; + } + ); + + if (!rosidl_message_type_support_) { + throw std::runtime_error("Could not allocate rosidl_message_type_support_t struct"); + delete ts_impl; + return; + } + + type_hash.release(); +} + + +// GETTERS ========================================================================================= +const std::string +DynamicMessageTypeSupport::get_library_identifier() const +{ + return serialization_support_->get_library_identifier(); +} + + +rosidl_message_type_support_t * +DynamicMessageTypeSupport::get_rosidl_message_type_support() +{ + return rosidl_message_type_support_.get(); +} + + +const rosidl_message_type_support_t * +DynamicMessageTypeSupport::get_rosidl_message_type_support() const +{ + return rosidl_message_type_support_.get(); +} + + +std::shared_ptr +DynamicMessageTypeSupport::get_shared_rosidl_message_type_support() +{ + return rosidl_message_type_support_; +} + + +std::shared_ptr +DynamicMessageTypeSupport::get_shared_rosidl_message_type_support() const +{ + return rosidl_message_type_support_; +} + + +rosidl_runtime_c__type_description__TypeDescription * +DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() +{ + return description_.get(); +} + + +const rosidl_runtime_c__type_description__TypeDescription * +DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() const +{ + return description_.get(); +} + + +std::shared_ptr +DynamicMessageTypeSupport::get_shared_rosidl_runtime_c_type_description() +{ + return description_; +} + + +std::shared_ptr +DynamicMessageTypeSupport::get_shared_rosidl_runtime_c_type_description() const +{ + return 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..8ba07a078c --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -0,0 +1,136 @@ +// 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 "rcutils/logging_macros.h" +#include "rmw/dynamic_message_type_support.h" +#include "rmw/ret_types.h" + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + + +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +// CONSTRUCTION ==================================================================================== +DynamicSerializationSupport::DynamicSerializationSupport() +: DynamicSerializationSupport::DynamicSerializationSupport("") {} + +DynamicSerializationSupport::DynamicSerializationSupport( + const std::string & serialization_library_name) +: rosidl_serialization_support_(nullptr) +{ + rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support = nullptr; + rmw_ret_t ret = RMW_RET_ERROR; + + if (serialization_library_name.empty()) { + ret = rmw_get_serialization_support(NULL, &rosidl_serialization_support); + } else { + ret = rmw_get_serialization_support( + serialization_library_name.c_str(), &rosidl_serialization_support); + } + + if (ret != RMW_RET_OK || !rosidl_serialization_support) { + throw std::runtime_error("could not create new serialization support object"); + } + + rosidl_serialization_support_.reset( + rosidl_serialization_support, + // Custom deleter + [](rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) -> void { + rosidl_dynamic_typesupport_serialization_support_destroy(rosidl_serialization_support); + }); +} + + +DynamicSerializationSupport::DynamicSerializationSupport( + rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) +: rosidl_serialization_support_(nullptr) +{ + if (!rosidl_serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + + // Custom deleter + rosidl_serialization_support_.reset( + rosidl_serialization_support, + [](rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) -> void { + rosidl_dynamic_typesupport_serialization_support_destroy(rosidl_serialization_support); + }); +} + + +DynamicSerializationSupport::DynamicSerializationSupport( + std::shared_ptr rosidl_serialization_support) +: rosidl_serialization_support_(rosidl_serialization_support) {} + + +DynamicSerializationSupport::DynamicSerializationSupport( + DynamicSerializationSupport && other) noexcept +: rosidl_serialization_support_(std::exchange(other.rosidl_serialization_support_, nullptr)) {} + + +DynamicSerializationSupport & +DynamicSerializationSupport::operator=(DynamicSerializationSupport && other) noexcept +{ + std::swap(rosidl_serialization_support_, other.rosidl_serialization_support_); + return *this; +} + + +DynamicSerializationSupport::~DynamicSerializationSupport() {} + + +// GETTERS ========================================================================================= +const std::string +DynamicSerializationSupport::get_library_identifier() const +{ + return std::string( + rosidl_dynamic_typesupport_serialization_support_get_library_identifier( + rosidl_serialization_support_.get())); +} + + +rosidl_dynamic_typesupport_serialization_support_t * +DynamicSerializationSupport::get_rosidl_serialization_support() +{ + return rosidl_serialization_support_.get(); +} + + +const rosidl_dynamic_typesupport_serialization_support_t * +DynamicSerializationSupport::get_rosidl_serialization_support() const +{ + return rosidl_serialization_support_.get(); +} + + +std::shared_ptr +DynamicSerializationSupport::get_shared_rosidl_serialization_support() +{ + return std::shared_ptr( + shared_from_this(), rosidl_serialization_support_.get()); +} + + +std::shared_ptr +DynamicSerializationSupport::get_shared_rosidl_serialization_support() const +{ + return std::shared_ptr( + shared_from_this(), rosidl_serialization_support_.get()); +} diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 9bafbe3106..fdb28c26bb 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..f9fcd5628f 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; +} 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; }