diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 8712b48856..5233b0919a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -113,6 +113,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/rate.cpp src/rclcpp/serialization.cpp src/rclcpp/serialized_message.cpp diff --git a/rclcpp/include/rclcpp/dynamic_subscription.hpp b/rclcpp/include/rclcpp/dynamic_subscription.hpp new file mode 100644 index 0000000000..caddf5c58a --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_subscription.hpp @@ -0,0 +1,175 @@ +// 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) + : SubscriptionBase( + node_base, + type_support->get_const_rosidl_message_type_support(), + topic_name, + options.to_rcl_subscription_options( + qos), + options.event_callbacks, + options.use_default_callbacks, + DeliveredMessageKind::DYNAMIC_MESSAGE), + 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_const_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..5ae6827692 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp @@ -0,0 +1,327 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#endif + + +#define __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + template<> \ + ValueT \ + DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) \ + { \ + ValueT out; \ + rosidl_dynamic_typesupport_dynamic_data_get_ ## FunctionT ## _value( \ + &rosidl_dynamic_data_, id, &out); \ + return out; \ + } + +#define __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + template<> \ + ValueT \ + DynamicMessage::get_value(const std::string & name) \ + { \ + return get_value(get_member_id(name)); \ + } + +#define __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + template<> \ + void \ + DynamicMessage::set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value) \ + { \ + rosidl_dynamic_typesupport_dynamic_data_set_ ## FunctionT ## _value( \ + &rosidl_dynamic_data_, id, value); \ + } + +#define __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + template<> \ + void \ + DynamicMessage::set_value(const std::string & name, ValueT value) \ + { \ + set_value(get_member_id(name), value); \ + } + +#define __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) \ + template<> \ + rosidl_dynamic_typesupport_member_id_t \ + DynamicMessage::insert_value(ValueT value) \ + { \ + rosidl_dynamic_typesupport_member_id_t out; \ + rosidl_dynamic_typesupport_dynamic_data_insert_ ## FunctionT ## _value( \ + &rosidl_dynamic_data_, value, &out); \ + return out; \ + } + +#define DYNAMIC_MESSAGE_DEFINITIONS(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + +DYNAMIC_MESSAGE_DEFINITIONS(bool, bool); +// DYNAMIC_MESSAGE_DEFINITIONS(std::byte, byte); +DYNAMIC_MESSAGE_DEFINITIONS(char, char); +DYNAMIC_MESSAGE_DEFINITIONS(float, float32); +DYNAMIC_MESSAGE_DEFINITIONS(double, float64); +DYNAMIC_MESSAGE_DEFINITIONS(int8_t, int8); +DYNAMIC_MESSAGE_DEFINITIONS(int16_t, int16); +DYNAMIC_MESSAGE_DEFINITIONS(int32_t, int32); +DYNAMIC_MESSAGE_DEFINITIONS(int64_t, int64); +DYNAMIC_MESSAGE_DEFINITIONS(uint8_t, uint8); +DYNAMIC_MESSAGE_DEFINITIONS(uint16_t, uint16); +DYNAMIC_MESSAGE_DEFINITIONS(uint32_t, uint32); +DYNAMIC_MESSAGE_DEFINITIONS(uint64_t, uint64); +// DYNAMIC_MESSAGE_DEFINITIONS(std::string, std::string); +// DYNAMIC_MESSAGE_DEFINITIONS(std::u16string, std::u16string); + +// Byte and String getters have a different implementation and are defined below + + +// BYTE ============================================================================================ +template<> +std::byte +DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + unsigned char out; + rosidl_dynamic_typesupport_dynamic_data_get_byte_value(&get_rosidl_dynamic_data(), id, &out); + return static_cast(out); +} + + +template<> +std::byte +DynamicMessage::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +void +DynamicMessage::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::byte value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_byte_value( + &rosidl_dynamic_data_, id, static_cast(value)); +} + + +template<> +void +DynamicMessage::set_value(const std::string & name, const std::byte value) +{ + set_value(get_member_id(name), value); +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_value(const std::byte value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_byte_value( + &rosidl_dynamic_data_, static_cast(value), &out); + return out; +} + + +// STRINGS ========================================================================================= +template<> +std::string +DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + size_t buf_length; + char * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_string_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length); + auto out = std::string(buf, buf_length); + delete buf; + return out; +} + + +template<> +std::u16string +DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + size_t buf_length; + char16_t * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_wstring_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length); + auto out = std::u16string(buf, buf_length); + delete buf; + return out; +} + + +template<> +std::string +DynamicMessage::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +std::u16string +DynamicMessage::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +void +DynamicMessage::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_string_value( + &rosidl_dynamic_data_, id, value.c_str(), value.size()); +} + + +template<> +void +DynamicMessage::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_wstring_value( + &rosidl_dynamic_data_, id, value.c_str(), value.size()); +} + + +template<> +void +DynamicMessage::set_value(const std::string & name, const std::string value) +{ + set_value(get_member_id(name), value); +} + + +template<> +void +DynamicMessage::set_value(const std::string & name, const std::u16string value) +{ + set_value(get_member_id(name), value); +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_value(const std::string value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_string_value( + &rosidl_dynamic_data_, value.c_str(), value.size(), &out); + return out; +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_value(const std::u16string value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_wstring_value( + &rosidl_dynamic_data_, value.c_str(), value.size(), &out); + return out; +} + + +// THROW FOR UNSUPPORTED TYPES ===================================================================== +template +ValueT +DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type"); +} + + +template +ValueT +DynamicMessage::get_value(const std::string & name) +{ + throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type"); +} + + +template +void +DynamicMessage::set_value( + rosidl_dynamic_typesupport_member_id_t id, ValueT value) +{ + throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type"); +} + + +template +void +DynamicMessage::set_value(const std::string & name, ValueT value) +{ + throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type"); +} + + +template +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_value(ValueT value) +{ + throw rclcpp::exceptions::UnimplementedError("insert_value is not implemented for input type"); +} + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN +#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN +#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN +#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN +#undef __DYNAMIC_MESSAGE_INSERT_VALUE +#undef DYNAMIC_MESSAGE_DEFINITIONS + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp new file mode 100644 index 0000000000..60188146e1 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp @@ -0,0 +1,189 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#endif + + +#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicMessageTypeBuilder::add_member( \ + rosidl_dynamic_typesupport_member_id_t id, \ + const std::string & name, \ + const std::string & default_value) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _member( \ + &rosidl_dynamic_type_builder_, \ + id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \ + } + +#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicMessageTypeBuilder::add_array_member( \ + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, \ + size_t array_length, \ + const std::string & default_value) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _array_member( \ + &rosidl_dynamic_type_builder_, \ + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \ + array_length); \ + } + +#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicMessageTypeBuilder::add_unbounded_sequence_member( \ + rosidl_dynamic_typesupport_member_id_t id, \ + const std::string & name, \ + const std::string & default_value) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## \ + _unbounded_sequence_member( \ + &rosidl_dynamic_type_builder_, \ + id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \ + } + +#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicMessageTypeBuilder::add_bounded_sequence_member( \ + rosidl_dynamic_typesupport_member_id_t id, \ + const std::string & name, \ + size_t sequence_bound, \ + const std::string & default_value) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _bounded_sequence_member( \ + &rosidl_dynamic_type_builder_, \ + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \ + sequence_bound); \ + } + +#define DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(MemberT, FunctionT) \ + __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(bool, bool); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::byte, byte); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(char, char); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(float, float32); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(double, float64); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int8_t, int8); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int16_t, int16); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int32_t, int32); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int64_t, int64); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint8_t, uint8); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint16_t, uint16); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint32_t, uint32); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint64_t, uint64); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::string, string); +DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::u16string, wstring); + + +// THROW FOR UNSUPPORTED TYPES ===================================================================== +template +void +DynamicMessageTypeBuilder::add_member( + rosidl_dynamic_typesupport_member_id_t id, + const std::string & name, + const std::string & default_value) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_member is not implemented for input type"); +} + + +template +void +DynamicMessageTypeBuilder::add_array_member( + rosidl_dynamic_typesupport_member_id_t id, + const std::string & name, + size_t array_length, const std::string & default_value) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_array_member is not implemented for input type"); +} + + +template +void +DynamicMessageTypeBuilder::add_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, + const std::string & name, + const std::string & default_value) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_unbounded_sequence_member is not implemented for input type"); +} + + +template +void +DynamicMessageTypeBuilder::add_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, + const std::string & name, + size_t sequence_bound, + const std::string & default_value) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_bounded_sequence_member is not implemented for input type"); +} + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN +#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN +#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN +#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN +#undef DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp index f9586aabb7..96721dadf3 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp @@ -32,16 +32,372 @@ namespace rclcpp namespace dynamic_typesupport { +class DynamicMessageType; +class DynamicMessageTypeBuilder; + /// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t -/// STUBBED OUT +/** + * This class: + * - Exposes getter methods for the struct + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed + * DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport. + * - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer + * must point to the same location in memory as the stored raw pointer! + * + * Note: This class is meant to map to the lower level rosidl_dynamic_typesupport_dynamic_data_t, + * even though rosidl_dynamic_typesupport_dynamic_data_t is equivalent to + * rosidl_dynamic_typesupport_dynamic_data_t, exposing the fundamental methods available to + * rosidl_dynamic_typesupport_dynamic_data_t, allowing the user to access the data's fields. + */ class DynamicMessage : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage) + // CONSTRUCTION ================================================================================== + // Most constructors require a passed in DynamicSerializationSupport::SharedPtr, to extend the + // lifetime of the serialization support (if the constructor cannot otherwise get it from args). + // + // In cases where a dynamic data pointer is passed, the serialization support composed by + // the data should be the exact same object managed by the DynamicSerializationSupport, + // otherwise the lifetime management will not work properly. + + /// Construct a new DynamicMessage with the provided dynamic type builder, using its allocator + RCLCPP_PUBLIC + explicit DynamicMessage(std::shared_ptr dynamic_type_builder); + + /// Construct a new DynamicMessage with the provided dynamic type builder and allocator + RCLCPP_PUBLIC + DynamicMessage( + std::shared_ptr dynamic_type_builder, + rcl_allocator_t allocator); + + /// Construct a new DynamicMessage with the provided dynamic type, using its allocator + RCLCPP_PUBLIC + explicit DynamicMessage(std::shared_ptr dynamic_type); + + /// Construct a new DynamicMessage with the provided dynamic type and allocator + RCLCPP_PUBLIC + DynamicMessage( + std::shared_ptr dynamic_type, + rcl_allocator_t allocator); + + /// Assume ownership of struct + RCLCPP_PUBLIC + DynamicMessage( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_data_t && rosidl_dynamic_data); + + /// Loaning constructor + /// Must only be called with a rosidl dynaimc data object obtained from loaning! + // NOTE(methylDragon): I'd put this in protected, but I need this exposed to + // enable_shared_from_this... + RCLCPP_PUBLIC + DynamicMessage( + DynamicMessage::SharedPtr parent_data, + rosidl_dynamic_typesupport_dynamic_data_t && rosidl_loaned_data); + + // NOTE(methylDragon): Deliberately no constructor from description to nudge users towards using + // construction from dynamic type/builder, which is more efficient + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessage(DynamicMessage && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessage & operator=(DynamicMessage && other) noexcept; + RCLCPP_PUBLIC virtual ~DynamicMessage(); + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_data_t & + get_rosidl_dynamic_data(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_data_t & + get_rosidl_dynamic_data() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + RCLCPP_PUBLIC + size_t + get_item_count() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_member_id(size_t index) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_member_id(const std::string & name) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_array_index(size_t index) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_array_index(const std::string & name) const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + DynamicMessage + clone(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage + init_from_type( + DynamicMessageType & type, rcl_allocator_t allocator = rcl_get_default_allocator()) const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + init_from_type_shared( + DynamicMessageType & type, rcl_allocator_t allocator = rcl_get_default_allocator()) const; + + RCLCPP_PUBLIC + bool + equals(const DynamicMessage & other) const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + loan_value( + rosidl_dynamic_typesupport_member_id_t id, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + loan_value( + const std::string & name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + void + clear_all_values(); + + RCLCPP_PUBLIC + void + clear_nonkey_values(); + + RCLCPP_PUBLIC + void + clear_value(rosidl_dynamic_typesupport_member_id_t id); + + RCLCPP_PUBLIC + void + clear_value(const std::string & name); + + RCLCPP_PUBLIC + void + clear_sequence(); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_sequence_data(); + + RCLCPP_PUBLIC + void + remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index); + + RCLCPP_PUBLIC + bool + serialize(rcl_serialized_message_t & buffer); + + RCLCPP_PUBLIC + bool + deserialize(rcl_serialized_message_t & buffer); + + + // MEMBER ACCESS TEMPLATES ======================================================================= + /** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + + template + ValueT + get_value(rosidl_dynamic_typesupport_member_id_t id); + + template + ValueT + get_value(const std::string & name); + + template + void + set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value); + + template + void + set_value(const std::string & name, ValueT value); + + template + rosidl_dynamic_typesupport_member_id_t + insert_value(ValueT value); + + + // FIXED STRING MEMBER ACCESS ==================================================================== + RCLCPP_PUBLIC + const std::string + get_fixed_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_length); + + RCLCPP_PUBLIC + const std::string + get_fixed_string_value(const std::string & name, size_t string_length); + + RCLCPP_PUBLIC + const std::u16string + get_fixed_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length); + + RCLCPP_PUBLIC + const std::u16string + get_fixed_wstring_value(const std::string & name, size_t wstring_length); + + RCLCPP_PUBLIC + void + set_fixed_string_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length); + + RCLCPP_PUBLIC + void + set_fixed_string_value(const std::string & name, const std::string value, size_t string_length); + + RCLCPP_PUBLIC + void + set_fixed_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length); + + RCLCPP_PUBLIC + void + set_fixed_wstring_value( + const std::string & name, const std::u16string value, size_t wstring_length); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_fixed_string_value(const std::string value, size_t string_length); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_fixed_wstring_value(const std::u16string value, size_t wstring_length); + + + // BOUNDED STRING MEMBER ACCESS ================================================================== + RCLCPP_PUBLIC + const std::string + get_bounded_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_bound); + + RCLCPP_PUBLIC + const std::string + get_bounded_string_value(const std::string & name, size_t string_bound); + + RCLCPP_PUBLIC + const std::u16string + get_bounded_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound); + + RCLCPP_PUBLIC + const std::u16string + get_bounded_wstring_value(const std::string & name, size_t wstring_bound); + + RCLCPP_PUBLIC + void + set_bounded_string_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound); + + RCLCPP_PUBLIC + void + set_bounded_string_value(const std::string & name, const std::string value, size_t string_bound); + + RCLCPP_PUBLIC + void + set_bounded_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound); + + RCLCPP_PUBLIC + void + set_bounded_wstring_value( + const std::string & name, const std::u16string value, size_t wstring_bound); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_bounded_string_value(const std::string value, size_t string_bound); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound); + + + // NESTED MEMBER ACCESS ========================================================================== + RCLCPP_PUBLIC + DynamicMessage + get_nested_data( + rosidl_dynamic_typesupport_member_id_t id, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage + get_nested_data( + const std::string & name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_nested_data_shared( + rosidl_dynamic_typesupport_member_id_t id, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_nested_data_shared( + const std::string & name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + void + set_nested_data(rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value); + + RCLCPP_PUBLIC + void + set_nested_data(const std::string & name, DynamicMessage & value); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_nested_data_copy(const DynamicMessage & value); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_nested_data(DynamicMessage & value); + protected: // NOTE(methylDragon): // This is just here to extend the lifetime of the serialization support @@ -62,6 +418,12 @@ class DynamicMessage : public std::enable_shared_from_this RCLCPP_PUBLIC DynamicMessage(); + + RCLCPP_PUBLIC + bool + match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_data_t & dynamic_data); }; } // namespace dynamic_typesupport diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp index 93cbabdade..beaaee08a5 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp @@ -30,16 +30,143 @@ namespace rclcpp namespace dynamic_typesupport { +class DynamicMessage; +class DynamicMessageTypeBuilder; + /// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t` -/// STUBBED OUT +/** + * This class: + * - Exposes getter methods for the struct + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class borrows the `rosidl_dynamic_typesupport_serialization_support_t` stored in the + * passed `DynamicSerializationSupport`. + * So it cannot outlive the `DynamicSerializationSupport`. + * - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t` + * pointer must point to the same location in memory as the stored raw pointer! + * + * This class is meant to map to the lower level `rosidl_dynamic_typesupport_dynamic_type_t`, + * which can be constructed via `DynamicMessageTypeBuilder`, which maps to + * `rosidl_dynamic_typesupport_dynamic_type_builder_t`. + * + * The usual method of obtaining a `DynamicMessageType` is through construction of + * `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then + * taking ownership of its contents. But `DynamicMessageTypeBuilder` can also be used to obtain + * `DynamicMessageType` by constructing it bottom-up instead, since it exposes the lower_level + * rosidl methods. + */ class DynamicMessageType : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType) + // CONSTRUCTION ================================================================================== + // Most constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the + // lifetime of the serialization support (if the constructor cannot otherwise get it from args). + // + // In cases where a dynamic type pointer is passed, the serialization support composed by + // the type should be the exact same object managed by the `DynamicSerializationSupport`, + // otherwise the lifetime management will not work properly. + + /// Construct a new `DynamicMessageType` with the provided dynamic type builder, + /// using its allocator + RCLCPP_PUBLIC + explicit DynamicMessageType(std::shared_ptr dynamic_type_builder); + + /// Construct a new `DynamicMessageType` with the provided dynamic type builder and allocator + RCLCPP_PUBLIC + DynamicMessageType( + std::shared_ptr dynamic_type_builder, + rcl_allocator_t allocator); + + /// Assume ownership of struct + RCLCPP_PUBLIC + DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_t && rosidl_dynamic_type); + + /// From description + RCLCPP_PUBLIC + DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessageType(DynamicMessageType && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessageType & operator=(DynamicMessageType && other) noexcept; + RCLCPP_PUBLIC virtual ~DynamicMessageType(); + /// Swaps the serialization support if `serialization_support` is populated + /** + * The user can call this with another description to reconfigure the type without changing the + * serialization support + */ + RCLCPP_PUBLIC + void + init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator(), + DynamicSerializationSupport::SharedPtr serialization_support = nullptr); + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + size_t + get_member_count() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_type_t & + get_rosidl_dynamic_type(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_type_t & + get_rosidl_dynamic_type() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + DynamicMessageType + clone(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + bool + equals(const DynamicMessageType & other) const; + + RCLCPP_PUBLIC + DynamicMessage + build_dynamic_message(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + std::shared_ptr + build_dynamic_message_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + protected: // NOTE(methylDragon): // This is just here to extend the lifetime of the serialization support @@ -56,6 +183,12 @@ class DynamicMessageType : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder) + // CONSTRUCTION ================================================================================== + // All constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the + // lifetime of the serialization support. + // + // In cases where a dynamic type builder pointer is passed, the serialization support composed by + // the builder should be the exact same object managed by the `DynamicSerializationSupport`, + // otherwise the lifetime management will not work properly. + + /// Construct a new `DynamicMessageTypeBuilder` with the provided serialization support using its + /// allocator + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name); + + /// Construct a new `DynamicMessageTypeBuilder` with the provided serialization support and + /// allocator + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name, + rcl_allocator_t allocator); + + /// Assume ownership of struct + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_builder_t && dynamic_type_builder); + + /// From description + RCLCPP_PUBLIC + DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Move constructor + RCLCPP_PUBLIC + DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicMessageTypeBuilder & operator=(DynamicMessageTypeBuilder && other) noexcept; + RCLCPP_PUBLIC virtual ~DynamicMessageTypeBuilder(); + /// Swaps the serialization support if serialization_support is populated + /** + * The user can call this with another description to reconfigure the type without changing the + * serialization support + */ + RCLCPP_PUBLIC + void + init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator(), + DynamicSerializationSupport::SharedPtr serialization_support = nullptr); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_type_builder_t & + get_rosidl_dynamic_type_builder(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_type_builder_t & + get_rosidl_dynamic_type_builder() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + void + set_name(const std::string & name); + + RCLCPP_PUBLIC + DynamicMessageTypeBuilder + clone(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessageTypeBuilder::SharedPtr + clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + void + clear(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage + build_dynamic_message(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + build_dynamic_message_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessageType + build_dynamic_message_type(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + build_dynamic_message_type_shared(rcl_allocator_t allocator = rcl_get_default_allocator()); + + + // ADD MEMBERS TEMPLATES ========================================================================= + /** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + + template + void + add_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + const std::string & default_value = ""); + + template + void + add_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length, + const std::string & default_value = ""); + + template + void + add_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + const std::string & default_value = ""); + + template + void + add_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound, + const std::string & default_value = ""); + + + // ADD FIXED STRING MEMBERS ====================================================================== + RCLCPP_PUBLIC + void + add_fixed_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_length, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_length, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_string_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_length, size_t sequence_bound, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_fixed_wstring_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_length, size_t sequence_bound, const std::string & default_value = ""); + + + // ADD BOUNDED STRING MEMBERS ==================================================================== + RCLCPP_PUBLIC + void + add_bounded_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_string_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t sequence_bound, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_bounded_wstring_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t sequence_bound, const std::string & default_value = ""); + + + // ADD NESTED MEMBERS ============================================================================ + RCLCPP_PUBLIC + void + add_complex_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, size_t array_length, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, size_t sequence_bound, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_array_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, size_t array_length, + const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_unbounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = ""); + + RCLCPP_PUBLIC + void + add_complex_bounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound, + const std::string & default_value = ""); + protected: // NOTE(methylDragon): // This is just here to extend the lifetime of the serialization support @@ -56,6 +387,19 @@ class DynamicMessageTypeBuilder : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport) + // CONSTRUCTION ================================================================================== + /// From description + /// Does NOT take ownership of the description (copies instead.) + /// Constructs type support top-down (calling `rcl_dynamic_message_type_support_handle_create()`) + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + const rosidl_runtime_c__type_description__TypeDescription & description, + const std::string & serialization_library_name = "", + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// From description, for provided serialization support + /// Does NOT take ownership of the description (copies instead.) + /// Constructs type support top-down (calling `rosidl_dynamic_message_type_support_handle_init()`) + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Assume ownership of managed types + /// Does NOT take ownership of the description (copies instead.) + /// + /// The serialization support used to construct all managed SharedPtrs must match. + /// The structure of the provided `description` must match the `dynamic_message_type` + /// The structure of the provided `dynamic_message_type` must match the `dynamic_message + /// + /// In this case, the user would have constructed the type support bototm-up (by creating the + /// respective dynamic members.) + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator = rcl_get_default_allocator()); + RCLCPP_PUBLIC virtual ~DynamicMessageTypeSupport(); + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + const rosidl_message_type_support_t & + get_const_rosidl_message_type_support(); + + RCLCPP_PUBLIC + const rosidl_message_type_support_t & + get_const_rosidl_message_type_support() const; + + RCLCPP_PUBLIC + const rosidl_runtime_c__type_description__TypeDescription & + get_rosidl_runtime_c_type_description() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + get_shared_dynamic_message_type(); + + RCLCPP_PUBLIC + DynamicMessageType::ConstSharedPtr + get_shared_dynamic_message_type() const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_shared_dynamic_message(); + + RCLCPP_PUBLIC + DynamicMessage::ConstSharedPtr + get_shared_dynamic_message() const; + protected: + RCLCPP_PUBLIC + rosidl_message_type_support_t & + get_rosidl_message_type_support(); + DynamicSerializationSupport::SharedPtr serialization_support_; DynamicMessageType::SharedPtr dynamic_message_type_; DynamicMessage::SharedPtr dynamic_message_; diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp index dde0710d25..176980e889 100644 --- a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp @@ -31,22 +31,60 @@ namespace dynamic_typesupport { /// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t +/** + * This class: + * - Exposes getter methods for the struct + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class, similarly to the rosidl_dynamic_typesupport_serialization_support_t, must outlive + * all downstream usages of the serialization support. + */ class DynamicSerializationSupport : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport) + // CONSTRUCTION ================================================================================== RCLCPP_PUBLIC explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator()); + /// Get the rmw middleware implementation specific serialization support (configured by name) RCLCPP_PUBLIC DynamicSerializationSupport( const std::string & serialization_library_name, rcl_allocator_t allocator = rcl_get_default_allocator()); + /// Assume ownership of struct + RCLCPP_PUBLIC + explicit DynamicSerializationSupport( + rosidl_dynamic_typesupport_serialization_support_t && rosidl_serialization_support); + + /// Move constructor + RCLCPP_PUBLIC + DynamicSerializationSupport(DynamicSerializationSupport && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicSerializationSupport & operator=(DynamicSerializationSupport && other) noexcept; + RCLCPP_PUBLIC virtual ~DynamicSerializationSupport(); + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_serialization_library_identifier() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_serialization_support_t & + get_rosidl_serialization_support(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_serialization_support_t & + get_rosidl_serialization_support() const; + protected: rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_; diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 50af3f1a89..482a19ea88 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -133,6 +133,9 @@ * - 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 @@ -184,4 +187,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_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 615f3852b6..18c1da82fa 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -644,6 +644,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::recursive_mutex callback_mutex_; diff --git a/rclcpp/src/rclcpp/dynamic_subscription.cpp b/rclcpp/src/rclcpp/dynamic_subscription.cpp new file mode 100644 index 0000000000..606f28b874 --- /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_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 index c340b107b6..3a40c58aaa 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp @@ -36,5 +36,734 @@ using rclcpp::dynamic_typesupport::DynamicMessageType; using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; using rclcpp::dynamic_typesupport::DynamicSerializationSupport; +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_ +// Template specialization implementations +#include "rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp" +#endif + + +// CONSTRUCTION ================================================================================== +DynamicMessage::DynamicMessage(const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder) +: DynamicMessage::DynamicMessage( + dynamic_type_builder, dynamic_type_builder->get_rosidl_dynamic_type_builder().allocator) {} + + +DynamicMessage::DynamicMessage( + const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder, + rcl_allocator_t allocator) +: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data()), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic message could not bind serialization support!"); + } + if (!dynamic_type_builder) { + throw std::runtime_error("dynamic message type builder cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + dynamic_type_builder->get_rosidl_dynamic_type_builder(); + + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type_builder( + &rosidl_dynamic_type_builder, &allocator, &get_rosidl_dynamic_data()); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic data object from dynamic type builder"); + } +} + + +DynamicMessage::DynamicMessage(const DynamicMessageType::SharedPtr dynamic_type) +: DynamicMessage::DynamicMessage( + dynamic_type, dynamic_type->get_rosidl_dynamic_type().allocator) {} + + +DynamicMessage::DynamicMessage( + const DynamicMessageType::SharedPtr dynamic_type, + rcl_allocator_t allocator) +: serialization_support_(dynamic_type->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data()), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic type could not bind serialization support!"); + } + if (!dynamic_type) { + throw std::runtime_error("dynamic message type cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = + dynamic_type->get_rosidl_dynamic_type(); + + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( + &rosidl_dynamic_type, &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not init new dynamic data object from dynamic type") + + rcl_get_error_string().str); + } +} + + +DynamicMessage::DynamicMessage( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_data_t && rosidl_dynamic_data) +: serialization_support_(serialization_support), + rosidl_dynamic_data_(std::move(rosidl_dynamic_data)), + is_loaned_(false), + parent_data_(nullptr) +{ + if (serialization_support) { + if (!match_serialization_support_(*serialization_support, rosidl_dynamic_data)) { + throw std::runtime_error( + "serialization support library identifier does not match dynamic data's!"); + } + } +} + + +DynamicMessage::DynamicMessage( + DynamicMessage::SharedPtr parent_data, + rosidl_dynamic_typesupport_dynamic_data_t && rosidl_loaned_data) +: serialization_support_(parent_data->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(std::move(rosidl_loaned_data)), + is_loaned_(true), + parent_data_(nullptr) +{ + if (!parent_data) { + throw std::runtime_error("parent dynamic data cannot be nullptr!"); + } + if (serialization_support_) { + if (!match_serialization_support_(*serialization_support_, rosidl_loaned_data)) { + throw std::runtime_error( + "serialization support library identifier does not match loaned dynamic data's!"); + } + } + parent_data_ = parent_data; +} + + +DynamicMessage::DynamicMessage(DynamicMessage && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_data_(std::exchange( + other.rosidl_dynamic_data_, + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data())), + is_loaned_(other.is_loaned_), + parent_data_(std::exchange(other.parent_data_, nullptr)) +{} + + +DynamicMessage & +DynamicMessage::operator=(DynamicMessage && other) noexcept +{ + std::swap(serialization_support_, other.serialization_support_); + std::swap(rosidl_dynamic_data_, other.rosidl_dynamic_data_); + is_loaned_ = other.is_loaned_; + std::swap(parent_data_, other.parent_data_); + return *this; +} + + DynamicMessage::~DynamicMessage() -{} // STUBBED +{ + if (!is_loaned_) { + if (rosidl_dynamic_typesupport_dynamic_data_fini(&get_rosidl_dynamic_data()) != + RCUTILS_RET_OK) + { + RCUTILS_LOG_ERROR("could not fini rosidl dynamic data"); + } + return; + } + + // Loaned case + if (!parent_data_) { + RCUTILS_LOG_ERROR("dynamic data is loaned, but parent is missing!!"); + } else { + rosidl_dynamic_typesupport_dynamic_data_return_loaned_value( + &parent_data_->get_rosidl_dynamic_data(), &get_rosidl_dynamic_data()); + } +} + + +bool +DynamicMessage::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_data_t & rosidl_dynamic_type_data) +{ + if (serialization_support.get_serialization_library_identifier() != std::string( + rosidl_dynamic_type_data.serialization_support->serialization_library_identifier)) + { + RCUTILS_LOG_ERROR("serialization support library identifier does not match dynamic data's"); + return false; + } + return true; +} + + +// GETTERS ======================================================================================= +const std::string +DynamicMessage::get_serialization_library_identifier() const +{ + return std::string( + get_rosidl_dynamic_data().serialization_support->serialization_library_identifier); +} + + +const std::string +DynamicMessage::get_name() const +{ + size_t buf_length; + const char * buf; + if ( + rosidl_dynamic_typesupport_dynamic_data_get_name( + &get_rosidl_dynamic_data(), &buf, + &buf_length) != + RCUTILS_RET_OK) + { + throw std::runtime_error( + std::string("could not get name for dynamic data") + rcl_get_error_string().str); + } + return std::string(buf, buf_length); +} + + +rosidl_dynamic_typesupport_dynamic_data_t & +DynamicMessage::get_rosidl_dynamic_data() +{ + return rosidl_dynamic_data_; +} + + +const rosidl_dynamic_typesupport_dynamic_data_t & +DynamicMessage::get_rosidl_dynamic_data() const +{ + return rosidl_dynamic_data_; +} + + +DynamicSerializationSupport::SharedPtr +DynamicMessage::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessage::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +size_t +DynamicMessage::get_item_count() const +{ + size_t item_count; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_item_count( + &get_rosidl_dynamic_data(), &item_count); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not get item count of dynamic data"); + } + return item_count; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::get_member_id(size_t index) const +{ + rosidl_dynamic_typesupport_member_id_t member_id; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_at_index( + &get_rosidl_dynamic_data(), index, &member_id); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not member id of dynamic data element by index"); + } + return member_id; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::get_member_id(const std::string & name) const +{ + rosidl_dynamic_typesupport_member_id_t member_id; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_by_name( + &get_rosidl_dynamic_data(), name.c_str(), name.size(), &member_id); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not member id of dynamic data element by name"); + } + return member_id; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::get_array_index(size_t index) const +{ + rosidl_dynamic_typesupport_member_id_t array_index; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_array_index( + &get_rosidl_dynamic_data(), index, &array_index); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not array index of dynamic data element by index"); + } + return array_index; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::get_array_index(const std::string & name) const +{ + return get_array_index(get_member_id(name)); +} + + +// METHODS ======================================================================================= +DynamicMessage +DynamicMessage::clone(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone( + &get_rosidl_dynamic_data(), &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage(get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_data)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::clone_shared(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone( + &get_rosidl_dynamic_data(), &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage::make_shared( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_data)); +} + + +DynamicMessage +DynamicMessage::init_from_type(DynamicMessageType & type, rcl_allocator_t allocator) const +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( + &type.get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic data object from dynamic type"); + } + return DynamicMessage(serialization_support_, std::move(rosidl_dynamic_data)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::init_from_type_shared(DynamicMessageType & type, rcl_allocator_t allocator) const +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( + &type.get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic data object from dynamic type"); + } + return DynamicMessage::make_shared(serialization_support_, std::move(rosidl_dynamic_data)); +} + + +bool +DynamicMessage::equals(const DynamicMessage & other) const +{ + if (get_serialization_library_identifier() != other.get_serialization_library_identifier()) { + throw std::runtime_error("library identifiers don't match"); + } + bool equals; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_equals( + &get_rosidl_dynamic_data(), &other.get_rosidl_dynamic_data(), &equals); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not equate dynamic messages"); + } + return equals; +} + + +DynamicMessage::SharedPtr +DynamicMessage::loan_value( + rosidl_dynamic_typesupport_member_id_t id, + rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_loan_value( + &get_rosidl_dynamic_data(), id, &allocator, &rosidl_dynamic_data); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not loan dynamic data: ") + rcl_get_error_string().str); + } + return DynamicMessage::make_shared(shared_from_this(), std::move(rosidl_dynamic_data)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::loan_value(const std::string & name, rcl_allocator_t allocator) +{ + return loan_value(get_member_id(name), allocator); +} + + +void +DynamicMessage::clear_all_values() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_all_values(&get_rosidl_dynamic_data()); +} + + +void +DynamicMessage::clear_nonkey_values() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_nonkey_values(&get_rosidl_dynamic_data()); +} + + +void +DynamicMessage::clear_value(rosidl_dynamic_typesupport_member_id_t id) +{ + rosidl_dynamic_typesupport_dynamic_data_clear_value(&get_rosidl_dynamic_data(), id); +} + + +void +DynamicMessage::clear_value(const std::string & name) +{ + clear_value(get_member_id(name)); +} + + +void +DynamicMessage::clear_sequence() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_sequence_data(&get_rosidl_dynamic_data()); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_sequence_data() +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_sequence_data(&get_rosidl_dynamic_data(), &out); + return out; +} + + +void +DynamicMessage::remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index) +{ + rosidl_dynamic_typesupport_dynamic_data_remove_sequence_data( + &get_rosidl_dynamic_data(), index); +} + + +bool +DynamicMessage::serialize(rcl_serialized_message_t & buffer) +{ + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_serialize( + &get_rosidl_dynamic_data(), &buffer); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could serialize loan dynamic data: ") + rcl_get_error_string().str); + } + return true; +} + + +bool +DynamicMessage::deserialize(rcl_serialized_message_t & buffer) +{ + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_deserialize( + &get_rosidl_dynamic_data(), &buffer); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could deserialize loan dynamic data: ") + rcl_get_error_string().str); + } + return true; +} + + +// MEMBER ACCESS =================================================================================== +// Defined in "detail/dynamic_message_impl.hpp" + + +// FIXED STRING MEMBER ACCESS ====================================================================== +const std::string +DynamicMessage::get_fixed_string_value( + rosidl_dynamic_typesupport_member_id_t id, size_t string_length) +{ + size_t buf_length; + char * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_fixed_string_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length, string_length); + auto out = std::string(buf, buf_length); + delete buf; + return out; +} + + +const std::string +DynamicMessage::get_fixed_string_value(const std::string & name, size_t string_length) +{ + return get_fixed_string_value(get_member_id(name), string_length); +} + + +const std::u16string +DynamicMessage::get_fixed_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length) +{ + size_t buf_length; + char16_t * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_fixed_wstring_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_length); + auto out = std::u16string(buf, buf_length); + delete buf; + return out; +} + + +const std::u16string +DynamicMessage::get_fixed_wstring_value(const std::string & name, size_t wstring_length) +{ + return get_fixed_wstring_value(get_member_id(name), wstring_length); +} + + +void +DynamicMessage::set_fixed_string_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length) +{ + rosidl_dynamic_typesupport_dynamic_data_set_fixed_string_value( + &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_length); +} + + +void +DynamicMessage::set_fixed_string_value( + const std::string & name, const std::string value, size_t string_length) +{ + set_fixed_string_value(get_member_id(name), value, string_length); +} + + +void +DynamicMessage::set_fixed_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length) +{ + rosidl_dynamic_typesupport_dynamic_data_set_fixed_wstring_value( + &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_length); +} + + +void +DynamicMessage::set_fixed_wstring_value( + const std::string & name, const std::u16string value, size_t wstring_length) +{ + set_fixed_wstring_value(get_member_id(name), value, wstring_length); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_fixed_string_value(const std::string value, size_t string_length) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_fixed_string_value( + &get_rosidl_dynamic_data(), value.c_str(), value.size(), string_length, &out); + return out; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_fixed_wstring_value(const std::u16string value, size_t wstring_length) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_fixed_wstring_value( + &get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_length, &out); + return out; +} + + +// BOUNDED STRING MEMBER ACCESS ==================================================================== +const std::string +DynamicMessage::get_bounded_string_value( + rosidl_dynamic_typesupport_member_id_t id, size_t string_bound) +{ + size_t buf_length; + char * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_bounded_string_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length, string_bound); + auto out = std::string(buf, buf_length); + delete buf; + return out; +} + + +const std::string +DynamicMessage::get_bounded_string_value(const std::string & name, size_t string_bound) +{ + return get_bounded_string_value(get_member_id(name), string_bound); +} + + +const std::u16string +DynamicMessage::get_bounded_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound) +{ + size_t buf_length; + char16_t * buf = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_bounded_wstring_value( + &get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_bound); + auto out = std::u16string(buf, buf_length); + delete buf; + return out; +} + + +const std::u16string +DynamicMessage::get_bounded_wstring_value(const std::string & name, size_t wstring_bound) +{ + return get_bounded_wstring_value(get_member_id(name), wstring_bound); +} + + +void +DynamicMessage::set_bounded_string_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound) +{ + rosidl_dynamic_typesupport_dynamic_data_set_bounded_string_value( + &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_bound); +} + + +void +DynamicMessage::set_bounded_string_value( + const std::string & name, const std::string value, size_t string_bound) +{ + set_bounded_string_value(get_member_id(name), value, string_bound); +} + + +void +DynamicMessage::set_bounded_wstring_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound) +{ + rosidl_dynamic_typesupport_dynamic_data_set_bounded_wstring_value( + &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_bound); +} + + +void +DynamicMessage::set_bounded_wstring_value( + const std::string & name, const std::u16string value, size_t wstring_bound) +{ + set_bounded_wstring_value(get_member_id(name), value, wstring_bound); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_bounded_string_value(const std::string value, size_t string_bound) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_bounded_string_value( + &get_rosidl_dynamic_data(), value.c_str(), value.size(), string_bound, &out); + return out; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_bounded_wstring_value( + &get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_bound, &out); + return out; +} + + +// NESTED MEMBER ACCESS ============================================================================ +DynamicMessage +DynamicMessage::get_nested_data( + rosidl_dynamic_typesupport_member_id_t id, rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t out = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rosidl_dynamic_typesupport_dynamic_data_get_nested_data( + &get_rosidl_dynamic_data(), id, &allocator, &out); + return DynamicMessage(get_shared_dynamic_serialization_support(), std::move(out)); +} + + +DynamicMessage +DynamicMessage::get_nested_data(const std::string & name, rcl_allocator_t allocator) +{ + return get_nested_data(get_member_id(name), allocator); +} + + +DynamicMessage::SharedPtr +DynamicMessage::get_nested_data_shared( + rosidl_dynamic_typesupport_member_id_t id, rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_data_t out = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data(); + rosidl_dynamic_typesupport_dynamic_data_get_nested_data( + &get_rosidl_dynamic_data(), id, &allocator, &out); + return DynamicMessage::make_shared(get_shared_dynamic_serialization_support(), std::move(out)); +} + + +DynamicMessage::SharedPtr +DynamicMessage::get_nested_data_shared(const std::string & name, rcl_allocator_t allocator) +{ + return get_nested_data_shared(get_member_id(name), allocator); +} + + +void +DynamicMessage::set_nested_data( + rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_nested_data( + &get_rosidl_dynamic_data(), id, &value.get_rosidl_dynamic_data()); +} + + +void +DynamicMessage::set_nested_data(const std::string & name, DynamicMessage & value) +{ + set_nested_data(get_member_id(name), value); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_nested_data_copy(const DynamicMessage & value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_nested_data_copy( + &get_rosidl_dynamic_data(), &value.get_rosidl_dynamic_data(), &out); + return out; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicMessage::insert_nested_data(DynamicMessage & value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_nested_data( + &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 index 069aff03aa..21a67ebbe0 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp @@ -34,5 +34,248 @@ using rclcpp::dynamic_typesupport::DynamicMessageType; using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +// CONSTRUCTION ==================================================================================== +DynamicMessageType::DynamicMessageType(DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder) +: DynamicMessageType::DynamicMessageType( + dynamic_type_builder, dynamic_type_builder->get_rosidl_dynamic_type_builder().allocator) {} + + +DynamicMessageType::DynamicMessageType( + DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder, + rcl_allocator_t allocator) +: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), + rosidl_dynamic_type_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type()) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic type could not bind serialization support!"); + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + dynamic_type_builder->get_rosidl_dynamic_type_builder(); + + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_init_from_dynamic_type_builder( + &rosidl_dynamic_type_builder, &allocator, &get_rosidl_dynamic_type()); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic type object"); + } +} + + +DynamicMessageType::DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_t && rosidl_dynamic_type) +: serialization_support_(serialization_support), + rosidl_dynamic_type_(std::move(rosidl_dynamic_type)) +{ + if (serialization_support) { + if (!match_serialization_support_(*serialization_support, rosidl_dynamic_type)) { + throw std::runtime_error( + "serialization support library identifier does not match dynamic type's!"); + } + } +} + + +DynamicMessageType::DynamicMessageType( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + rosidl_dynamic_type_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type()) +{ + init_from_description(description, allocator, serialization_support); +} + + +DynamicMessageType::DynamicMessageType(DynamicMessageType && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_type_(std::exchange( + other.rosidl_dynamic_type_, rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type())) +{} + + +DynamicMessageType & +DynamicMessageType::operator=(DynamicMessageType && other) noexcept +{ + std::swap(serialization_support_, other.serialization_support_); + std::swap(rosidl_dynamic_type_, other.rosidl_dynamic_type_); + return *this; +} + + DynamicMessageType::~DynamicMessageType() -{} // STUBBED +{ + if (rosidl_dynamic_typesupport_dynamic_type_fini(&get_rosidl_dynamic_type()) != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR("could not fini rosidl dynamic type"); + } +} + + +void +DynamicMessageType::init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator, + DynamicSerializationSupport::SharedPtr serialization_support) +{ + if (serialization_support) { + // Swap serialization support if serialization support is given + serialization_support_ = serialization_support; + } + + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_init_from_description( + &serialization_support_->get_rosidl_serialization_support(), + &description, + &allocator, + &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic type object"); + } + + rosidl_dynamic_type_ = std::move(rosidl_dynamic_type); +} + + +bool +DynamicMessageType::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type) +{ + if (serialization_support.get_serialization_library_identifier() != std::string( + rosidl_dynamic_type.serialization_support->serialization_library_identifier)) + { + RCUTILS_LOG_ERROR( + "serialization support library identifier does not match dynamic type's (%s vs %s)", + serialization_support.get_serialization_library_identifier().c_str(), + rosidl_dynamic_type.serialization_support->serialization_library_identifier); + return false; + } + return true; +} + + +// GETTERS ========================================================================================= +const std::string +DynamicMessageType::get_serialization_library_identifier() const +{ + return std::string( + get_rosidl_dynamic_type().serialization_support->serialization_library_identifier); +} + + +const std::string +DynamicMessageType::get_name() const +{ + size_t buf_length; + const char * buf; + rosidl_dynamic_typesupport_dynamic_type_get_name(&get_rosidl_dynamic_type(), &buf, &buf_length); + return std::string(buf, buf_length); +} + + +size_t +DynamicMessageType::get_member_count() const +{ + size_t out; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_get_member_count( + &get_rosidl_dynamic_type(), &out); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not get member count: ") + rcl_get_error_string().str); + } + return out; +} + + +rosidl_dynamic_typesupport_dynamic_type_t & +DynamicMessageType::get_rosidl_dynamic_type() +{ + return rosidl_dynamic_type_; +} + + +const rosidl_dynamic_typesupport_dynamic_type_t & +DynamicMessageType::get_rosidl_dynamic_type() const +{ + return rosidl_dynamic_type_; +} + + +DynamicSerializationSupport::SharedPtr +DynamicMessageType::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessageType::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +// METHODS ========================================================================================= +DynamicMessageType +DynamicMessageType::clone(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone( + &get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic type: ") + rcl_get_error_string().str); + } + return DynamicMessageType( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type)); +} + + +DynamicMessageType::SharedPtr +DynamicMessageType::clone_shared(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone( + &get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_type); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic type: ") + rcl_get_error_string().str); + } + return DynamicMessageType::make_shared( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type)); +} + + +bool +DynamicMessageType::equals(const DynamicMessageType & other) const +{ + if (get_serialization_library_identifier() != other.get_serialization_library_identifier()) { + throw std::runtime_error("library identifiers don't match"); + } + bool out; + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_equals( + &get_rosidl_dynamic_type(), &other.get_rosidl_dynamic_type(), &out); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not equate dynamic message types: ") + rcl_get_error_string().str); + } + return out; +} + + +DynamicMessage +DynamicMessageType::build_dynamic_message(rcl_allocator_t allocator) +{ + return DynamicMessage(shared_from_this(), allocator); +} + + +DynamicMessage::SharedPtr +DynamicMessageType::build_dynamic_message_shared(rcl_allocator_t allocator) +{ + return DynamicMessage::make_shared(shared_from_this(), allocator); +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp index 1c5d3708a1..ca225cf107 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp @@ -33,5 +33,582 @@ using rclcpp::dynamic_typesupport::DynamicMessageType; using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; using rclcpp::dynamic_typesupport::DynamicSerializationSupport; +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_ +// Template specialization implementations +#include "rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp" +#endif + +// CONSTRUCTION ==================================================================================== +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, const std::string & name) +: DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + serialization_support, + name, + serialization_support->get_rosidl_serialization_support().allocator) {} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_( + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) +{ + init_from_serialization_support_(serialization_support, name, allocator); +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_builder_t && rosidl_dynamic_type_builder) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_( + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!match_serialization_support_(*serialization_support, rosidl_dynamic_type_builder)) { + throw std::runtime_error( + "serialization support library does not match dynamic type builder's!"); + } +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_( + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder()) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + init_from_description(description, allocator, serialization_support); +} + + +DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_type_builder_(std::exchange( + other.rosidl_dynamic_type_builder_, + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder())) {} + + +DynamicMessageTypeBuilder & +DynamicMessageTypeBuilder::operator=(DynamicMessageTypeBuilder && other) noexcept +{ + std::swap(serialization_support_, other.serialization_support_); + std::swap(rosidl_dynamic_type_builder_, other.rosidl_dynamic_type_builder_); + return *this; +} + + DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder() -{} // STUBBED +{ + if (rosidl_dynamic_typesupport_dynamic_type_builder_fini(&get_rosidl_dynamic_type_builder()) != + RCUTILS_RET_OK) + { + RCUTILS_LOG_ERROR("could not fini rosidl dynamic type builder"); + } +} + + +void +DynamicMessageTypeBuilder::init_from_description( + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator, + DynamicSerializationSupport::SharedPtr serialization_support) +{ + if (serialization_support) { + // Swap serialization support if serialization support is given + serialization_support_ = serialization_support; + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_init_from_description( + &serialization_support_->get_rosidl_serialization_support(), + &description, + &allocator, + &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init new dynamic type builder object"); + } + + rosidl_dynamic_type_builder_ = std::move(rosidl_dynamic_type_builder); +} + + +void +DynamicMessageTypeBuilder::init_from_serialization_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name, + rcl_allocator_t allocator) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!&serialization_support->get_rosidl_serialization_support()) { + throw std::runtime_error("serialization support raw pointer cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_init( + &serialization_support->get_rosidl_serialization_support(), + name.c_str(), name.size(), + &allocator, + &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not init dynamic type builder: ") + rcl_get_error_string().str); + } +} + + +bool +DynamicMessageTypeBuilder::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_builder_t & rosidl_dynamic_type_builder) +{ + if (serialization_support.get_serialization_library_identifier() != std::string( + rosidl_dynamic_type_builder.serialization_support->serialization_library_identifier)) + { + RCUTILS_LOG_ERROR( + "serialization support library identifier does not match dynamic type builder's"); + return false; + } + return true; +} + + +// GETTERS ======================================================================================= +const std::string +DynamicMessageTypeBuilder::get_serialization_library_identifier() const +{ + return std::string( + get_rosidl_dynamic_type_builder().serialization_support->serialization_library_identifier); +} + + +const std::string +DynamicMessageTypeBuilder::get_name() const +{ + size_t buf_length; + const char * buf; + rosidl_dynamic_typesupport_dynamic_type_builder_get_name( + &get_rosidl_dynamic_type_builder(), &buf, &buf_length); + return std::string(buf, buf_length); +} + + +rosidl_dynamic_typesupport_dynamic_type_builder_t & +DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() +{ + return rosidl_dynamic_type_builder_; +} + + +const rosidl_dynamic_typesupport_dynamic_type_builder_t & +DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() const +{ + return rosidl_dynamic_type_builder_; +} + + +DynamicSerializationSupport::SharedPtr +DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +// METHODS ======================================================================================= +void +DynamicMessageTypeBuilder::set_name(const std::string & name) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_set_name( + &get_rosidl_dynamic_type_builder(), name.c_str(), name.size()); +} + + +DynamicMessageTypeBuilder +DynamicMessageTypeBuilder::clone(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone( + &get_rosidl_dynamic_type_builder(), &allocator, &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str); + } + return DynamicMessageTypeBuilder( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type_builder)); +} + + +DynamicMessageTypeBuilder::SharedPtr +DynamicMessageTypeBuilder::clone_shared(rcl_allocator_t allocator) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder(); + rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone( + &get_rosidl_dynamic_type_builder(), &allocator, &rosidl_dynamic_type_builder); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error( + std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str); + } + return DynamicMessageTypeBuilder::make_shared( + get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type_builder)); +} + + +void +DynamicMessageTypeBuilder::clear(rcl_allocator_t allocator) +{ + if (!serialization_support_) { + throw std::runtime_error( + "cannot call clear() on a dynamic type builder with uninitialized serialization support" + ); + } + + const std::string & name = get_name(); + init_from_serialization_support_(serialization_support_, name, allocator); +} + + +DynamicMessage +DynamicMessageTypeBuilder::build_dynamic_message(rcl_allocator_t allocator) +{ + return DynamicMessage(shared_from_this(), allocator); +} + + +DynamicMessage::SharedPtr +DynamicMessageTypeBuilder::build_dynamic_message_shared(rcl_allocator_t allocator) +{ + return DynamicMessage::make_shared(shared_from_this(), allocator); +} + + +DynamicMessageType +DynamicMessageTypeBuilder::build_dynamic_message_type(rcl_allocator_t allocator) +{ + return DynamicMessageType(shared_from_this(), allocator); +} + + +DynamicMessageType::SharedPtr +DynamicMessageTypeBuilder::build_dynamic_message_type_shared(rcl_allocator_t allocator) +{ + return DynamicMessageType::make_shared(shared_from_this(), allocator); +} + + +// ADD MEMBERS ===================================================================================== +// Defined in "detail/dynamic_message_type_builder_impl.hpp" + + +// ADD FIXED STRING MEMBERS ======================================================================== +void +DynamicMessageTypeBuilder::add_fixed_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_length, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_length, array_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_length, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_length, array_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_length); +} + + +void +DynamicMessageTypeBuilder::add_fixed_string_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_length, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_length, sequence_bound); +} + + +void +DynamicMessageTypeBuilder::add_fixed_wstring_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_length, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_length, sequence_bound); +} + + +// ADD BOUNDED STRING MEMBERS ====================================================================== +void +DynamicMessageTypeBuilder::add_bounded_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_bound, array_length); +} + + +void +DynamicMessageTypeBuilder::add_bounded_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_bound, array_length); +} + + +void +DynamicMessageTypeBuilder::add_bounded_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_string_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + string_bound, sequence_bound); +} + + +void +DynamicMessageTypeBuilder::add_bounded_wstring_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + wstring_bound, sequence_bound); +} + + +// ADD NESTED MEMBERS ============================================================================== +void +DynamicMessageTypeBuilder::add_complex_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type.get_rosidl_dynamic_type()); +} + + +void +DynamicMessageTypeBuilder::add_complex_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, size_t array_length, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type.get_rosidl_dynamic_type(), array_length); +} + + +void +DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type.get_rosidl_dynamic_type()); +} + + +void +DynamicMessageTypeBuilder::add_complex_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageType & nested_type, size_t sequence_bound, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type.get_rosidl_dynamic_type(), sequence_bound); +} + + +void +DynamicMessageTypeBuilder::add_complex_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member_builder( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type_builder.get_rosidl_dynamic_type_builder()); +} + + +void +DynamicMessageTypeBuilder::add_complex_array_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, size_t array_length, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member_builder( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type_builder.get_rosidl_dynamic_type_builder(), array_length); +} + + +void +DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member_builder( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type_builder.get_rosidl_dynamic_type_builder()); +} + + +void +DynamicMessageTypeBuilder::add_complex_bounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound, + const std::string & default_value) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member_builder( + &get_rosidl_dynamic_type_builder(), + id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), + &nested_type_builder.get_rosidl_dynamic_type_builder(), sequence_bound); +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp index 3a920c2805..b840ab52c3 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp @@ -45,5 +45,273 @@ using rclcpp::dynamic_typesupport::DynamicMessageType; using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport; using rclcpp::dynamic_typesupport::DynamicSerializationSupport; +// CONSTRUCTION ==================================================================================== +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + const rosidl_runtime_c__type_description__TypeDescription & description, + const std::string & serialization_library_name, + rcl_allocator_t allocator) +: serialization_support_(nullptr), + dynamic_message_type_(nullptr), + dynamic_message_(nullptr), + rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) +{ + rcl_ret_t ret; + if (serialization_library_name.empty()) { + ret = rcl_dynamic_message_type_support_handle_init( + nullptr, &description, &allocator, &rosidl_message_type_support_); + } else { + ret = rcl_dynamic_message_type_support_handle_init( + serialization_library_name.c_str(), &description, &allocator, &rosidl_message_type_support_); + } + if (ret != RCL_RET_OK) { + std::string error_msg = + std::string("error initializing rosidl message type support: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(error_msg); + } + if (rosidl_message_type_support_.typesupport_identifier != + rosidl_get_dynamic_typesupport_identifier()) + { + throw std::runtime_error("rosidl message type support is of the wrong type"); + } + + auto ts_impl = static_cast(const_cast( + rosidl_message_type_support_.data)); + + serialization_support_ = DynamicSerializationSupport::make_shared( + std::move(ts_impl->serialization_support)); + + dynamic_message_type_ = DynamicMessageType::make_shared( + get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message_type)); + + dynamic_message_ = DynamicMessage::make_shared( + get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message)); +} + +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + dynamic_message_type_(nullptr), + dynamic_message_(nullptr), + rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) +{ + // Check null + if (!serialization_support) { + throw std::runtime_error("serialization_support cannot be nullptr."); + } + + rosidl_type_hash_t type_hash; + rcutils_ret_t hash_ret = rcl_calculate_type_hash( + // TODO(methylDragon): Swap this out with the conversion function when it is ready + reinterpret_cast(&description), + &type_hash); + if (hash_ret != RCL_RET_OK) { + throw std::runtime_error("failed to get type hash"); + } + + rcutils_ret_t ret = rosidl_dynamic_message_type_support_handle_init( + &serialization_support->get_rosidl_serialization_support(), + &type_hash, // type_hash + &description, // type_description + nullptr, // type_description_sources (not implemented for dynamic types) + &allocator, + &rosidl_message_type_support_); + if (ret != RCUTILS_RET_OK) { + throw std::runtime_error("could not init rosidl message type support"); + } + if (rosidl_message_type_support_.typesupport_identifier != + rosidl_get_dynamic_typesupport_identifier()) + { + throw std::runtime_error("rosidl message type support is of the wrong type"); + } + + auto ts_impl = static_cast( + rosidl_message_type_support_.data); + + dynamic_message_type_ = DynamicMessageType::make_shared( + get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message_type)); + + dynamic_message_ = DynamicMessage::make_shared( + get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message)); +} + +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + const rosidl_runtime_c__type_description__TypeDescription & description, + rcl_allocator_t allocator) +: serialization_support_(serialization_support), + dynamic_message_type_(dynamic_message_type), + dynamic_message_(dynamic_message), + rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle()) +{ + // Check null + if (!serialization_support) { + throw std::runtime_error("serialization_support cannot be nullptr."); + } + if (!dynamic_message_type) { + throw std::runtime_error("dynamic_message_type cannot be nullptr."); + } + if (!dynamic_message) { + throw std::runtime_error("dynamic_message cannot be nullptr."); + } + + // Check identifiers + if (serialization_support->get_serialization_library_identifier() != + dynamic_message_type->get_serialization_library_identifier()) + { + throw std::runtime_error( + "serialization support library identifier does not match " + "dynamic message type library identifier."); + } + if (dynamic_message_type->get_serialization_library_identifier() != + dynamic_message->get_serialization_library_identifier()) + { + throw std::runtime_error( + "dynamic message type library identifier does not match " + "dynamic message library identifier."); + } + + rosidl_type_hash_t type_hash; + rcutils_ret_t hash_ret = rcl_calculate_type_hash( + // TODO(methylDragon): Swap this out with the conversion function when it is ready + // from https://github.com/ros2/rcl/pull/1052 + reinterpret_cast(&description), + &type_hash); + if (hash_ret != RCL_RET_OK) { + std::string error_msg = std::string("failed to get type hash: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(error_msg); + } + + auto ts_impl = static_cast( + allocator.zero_allocate(1, sizeof(rosidl_dynamic_message_type_support_impl_t), allocator.state) + ); + if (!ts_impl) { + throw std::runtime_error("could not allocate rosidl_message_type_support_t"); + } + + ts_impl->allocator = allocator; + ts_impl->type_hash = type_hash; + if (!rosidl_runtime_c__type_description__TypeDescription__copy( + &description, &ts_impl->type_description)) + { + throw std::runtime_error("could not copy type description"); + } + // ts_impl->type_description_sources = // Not used + + ts_impl->serialization_support = serialization_support->get_rosidl_serialization_support(); + ts_impl->dynamic_message_type = &dynamic_message_type->get_rosidl_dynamic_type(); + ts_impl->dynamic_message = &dynamic_message->get_rosidl_dynamic_data(); + + rosidl_message_type_support_ = { + rosidl_get_dynamic_typesupport_identifier(), // typesupport_identifier + ts_impl, // data + get_message_typesupport_handle_function, // func + // get_type_hash_func + rosidl_get_dynamic_message_type_support_type_hash_function, + // get_type_description_func + rosidl_get_dynamic_message_type_support_type_description_function, + // get_type_description_sources_func + rosidl_get_dynamic_message_type_support_type_description_sources_function + }; +} + DynamicMessageTypeSupport::~DynamicMessageTypeSupport() -{} // STUBBED +{ + // These must go first + serialization_support_.reset(); + dynamic_message_type_.reset(); + dynamic_message_.reset(); + + // Early return if type support isn't populated to avoid segfaults + if (!rosidl_message_type_support_.data) { + return; + } + + // We only partially finalize the rosidl_message_type_support->data since its pointer members are + // managed by their respective SharedPtr wrapper classes. + auto ts_impl = static_cast( + const_cast(rosidl_message_type_support_.data) + ); + rcutils_allocator_t allocator = ts_impl->allocator; + + rosidl_runtime_c__type_description__TypeDescription__fini(&ts_impl->type_description); + rosidl_runtime_c__type_description__TypeSource__Sequence__fini( + &ts_impl->type_description_sources); + allocator.deallocate(static_cast(ts_impl), allocator.state); // Always C allocated +} + + +// GETTERS ========================================================================================= +const std::string +DynamicMessageTypeSupport::get_serialization_library_identifier() const +{ + return serialization_support_->get_serialization_library_identifier(); +} + +rosidl_message_type_support_t & +DynamicMessageTypeSupport::get_rosidl_message_type_support() +{ + return rosidl_message_type_support_; +} + +const rosidl_message_type_support_t & +DynamicMessageTypeSupport::get_const_rosidl_message_type_support() +{ + return rosidl_message_type_support_; +} + +const rosidl_message_type_support_t & +DynamicMessageTypeSupport::get_const_rosidl_message_type_support() const +{ + return rosidl_message_type_support_; +} + +const rosidl_runtime_c__type_description__TypeDescription & +DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() const +{ + auto ts_impl = static_cast( + get_const_rosidl_message_type_support().data + ); + return ts_impl->type_description; +} + +DynamicSerializationSupport::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + +DynamicMessageType::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message_type() +{ + return dynamic_message_type_; +} + +DynamicMessageType::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message_type() const +{ + return dynamic_message_type_; +} + +DynamicMessage::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message() +{ + return dynamic_message_; +} + +DynamicMessage::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message() const +{ + return dynamic_message_; +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp index 769f5fb8cf..7ffde1591d 100644 --- a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -28,19 +28,71 @@ using rclcpp::dynamic_typesupport::DynamicSerializationSupport; // CONSTRUCTION ==================================================================================== DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator) -: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) -{ - throw std::runtime_error("Unimplemented"); -} +: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) {} DynamicSerializationSupport::DynamicSerializationSupport( - const std::string & /*serialization_library_name*/, - rcl_allocator_t /*allocator*/) + const std::string & serialization_library_name, + rcl_allocator_t allocator) : rosidl_serialization_support_( rosidl_dynamic_typesupport_get_zero_initialized_serialization_support()) { - throw std::runtime_error("Unimplemented"); + rmw_ret_t ret = RMW_RET_ERROR; + + if (serialization_library_name.empty()) { + ret = rmw_serialization_support_init(NULL, &allocator, &rosidl_serialization_support_); + } else { + ret = rmw_serialization_support_init( + serialization_library_name.c_str(), &allocator, &rosidl_serialization_support_); + } + if (ret != RCL_RET_OK) { + std::string error_msg = + std::string("could not initialize new serialization support object: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(error_msg); + } +} + +DynamicSerializationSupport::DynamicSerializationSupport( + rosidl_dynamic_typesupport_serialization_support_t && rosidl_serialization_support) +: rosidl_serialization_support_(std::move(rosidl_serialization_support)) {} + +DynamicSerializationSupport::DynamicSerializationSupport( + DynamicSerializationSupport && other) noexcept +: rosidl_serialization_support_(std::exchange( + other.rosidl_serialization_support_, + rosidl_dynamic_typesupport_get_zero_initialized_serialization_support())) {} + +DynamicSerializationSupport & +DynamicSerializationSupport::operator=(DynamicSerializationSupport && other) noexcept +{ + std::swap(rosidl_serialization_support_, other.rosidl_serialization_support_); + return *this; } DynamicSerializationSupport::~DynamicSerializationSupport() -{} // STUBBED +{ + rosidl_dynamic_typesupport_serialization_support_fini(&rosidl_serialization_support_); +} + + +// GETTERS ========================================================================================= +const std::string +DynamicSerializationSupport::get_serialization_library_identifier() const +{ + return std::string( + rosidl_dynamic_typesupport_serialization_support_get_library_identifier( + &rosidl_serialization_support_)); +} + +rosidl_dynamic_typesupport_serialization_support_t & +DynamicSerializationSupport::get_rosidl_serialization_support() +{ + return rosidl_serialization_support_; +} + +const rosidl_dynamic_typesupport_serialization_support_t & +DynamicSerializationSupport::get_rosidl_serialization_support() const +{ + return rosidl_serialization_support_; +} diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index fd2a46368e..965bc7e401 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -610,10 +610,25 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) } // DYNAMIC SUBSCRIPTION ======================================================================== - // Deliver dynamic message + // 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 case rclcpp::DeliveredMessageKind::DYNAMIC_MESSAGE: { - throw std::runtime_error("Unimplemented"); + 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; } default: diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index ae28354b98..9f623d43b9 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -46,8 +46,7 @@ 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 & message_info) { diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 7dca16a1a9..8d7df32ca4 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -573,9 +573,30 @@ SubscriptionBase::get_content_filter() const // DYNAMIC TYPE ================================================================================== bool SubscriptionBase::take_dynamic_message( - rclcpp::dynamic_typesupport::DynamicMessage & /*message_out*/, - rclcpp::MessageInfo & /*message_info_out*/) + rclcpp::dynamic_typesupport::DynamicMessage & message_out, + rclcpp::MessageInfo & message_info_out) { - throw std::runtime_error("Unimplemented"); - return false; + if (rmw_feature_supported(RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE)) { + 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, "Error when taking dynamic message"); + } + } else { // Fall back to serialized conversion if direct dynamic message taking isn't supported + std::shared_ptr serialized_msg = this->create_serialized_message(); + if (!this->take_serialized(*serialized_msg.get(), message_info_out)) { + std::runtime_error("Couldn't take serialized message when attempting to take dynamic data!"); + } + bool ret = message_out.deserialize(serialized_msg->get_rcl_serialized_message()); + if (!ret) { + throw std::runtime_error("Couldn't convert serialized message to dynamic data!"); + } + this->return_serialized_message(serialized_msg); + } + return true; } diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index 79fbfcc33a..a4ef5480e7 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; }