diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 6979d980f8..848fbec174 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -49,6 +49,11 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp src/rclcpp/detail/utilities.cpp src/rclcpp/duration.cpp + src/rclcpp/dynamic_typesupport/dynamic_data.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp + src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp + src/rclcpp/dynamic_typesupport/dynamic_type.cpp + src/rclcpp/dynamic_typesupport/dynamic_type_builder.cpp src/rclcpp/event.cpp src/rclcpp/exceptions/exceptions.cpp src/rclcpp/executable_list.cpp diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_data_impl.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_data_impl.hpp new file mode 100644 index 0000000000..d2e77247ae --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_data_impl.hpp @@ -0,0 +1,326 @@ +// 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_DATA_IMPL_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_DATA_IMPL_HPP_ + +#include +#include +#include +#include + +#include +#include +#include "rclcpp/exceptions.hpp" + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_DATA_HPP_ +#include "rclcpp/dynamic_typesupport/dynamic_data.hpp" +#endif + + +#define __DYNAMIC_DATA_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + template<> \ + ValueT \ + DynamicData::get_value(rosidl_dynamic_typesupport_member_id_t id) \ + { \ + ValueT out; \ + rosidl_dynamic_typesupport_dynamic_data_get_ ## FunctionT ## _value( \ + rosidl_dynamic_data_.get(), id, &out); \ + return out; \ + } + +#define __DYNAMIC_DATA_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + template<> \ + ValueT \ + DynamicData::get_value(const std::string & name) \ + { \ + return get_value(get_member_id(name)); \ + } + +#define __DYNAMIC_DATA_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + template<> \ + void \ + DynamicData::set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value) \ + { \ + rosidl_dynamic_typesupport_dynamic_data_set_ ## FunctionT ## _value( \ + rosidl_dynamic_data_.get(), id, value); \ + } + +#define __DYNAMIC_DATA_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + template<> \ + void \ + DynamicData::set_value(const std::string & name, ValueT value) \ + { \ + set_value(get_member_id(name), value); \ + } + +#define __DYNAMIC_DATA_INSERT_VALUE(ValueT, FunctionT) \ + template<> \ + rosidl_dynamic_typesupport_member_id_t \ + DynamicData::insert_value(ValueT value) \ + { \ + rosidl_dynamic_typesupport_member_id_t out; \ + rosidl_dynamic_typesupport_dynamic_data_insert_ ## FunctionT ## _value( \ + rosidl_dynamic_data_.get(), value, &out); \ + return out; \ + } + +#define DYNAMIC_DATA_DEFINITIONS(ValueT, FunctionT) \ + __DYNAMIC_DATA_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + __DYNAMIC_DATA_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + __DYNAMIC_DATA_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \ + __DYNAMIC_DATA_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \ + __DYNAMIC_DATA_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_DATA_DEFINITIONS(bool, bool); +// DYNAMIC_DATA_DEFINITIONS(std::byte, byte); +DYNAMIC_DATA_DEFINITIONS(char, char); +DYNAMIC_DATA_DEFINITIONS(float, float32); +DYNAMIC_DATA_DEFINITIONS(double, float64); +DYNAMIC_DATA_DEFINITIONS(int8_t, int8); +DYNAMIC_DATA_DEFINITIONS(int16_t, int16); +DYNAMIC_DATA_DEFINITIONS(int32_t, int32); +DYNAMIC_DATA_DEFINITIONS(int64_t, int64); +DYNAMIC_DATA_DEFINITIONS(uint8_t, uint8); +DYNAMIC_DATA_DEFINITIONS(uint16_t, uint16); +DYNAMIC_DATA_DEFINITIONS(uint32_t, uint32); +DYNAMIC_DATA_DEFINITIONS(uint64_t, uint64); +// DYNAMIC_DATA_DEFINITIONS(std::string, std::string); +// DYNAMIC_DATA_DEFINITIONS(std::u16string, std::u16string); + +// Byte and String getters have a different implementation and are defined below + + +// BYTE ============================================================================================ +template<> +std::byte +DynamicData::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 +DynamicData::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +void +DynamicData::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::byte value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_byte_value( + rosidl_dynamic_data_.get(), id, static_cast(value)); +} + + +template<> +void +DynamicData::set_value(const std::string & name, const std::byte value) +{ + set_value(get_member_id(name), value); +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicData::insert_value(const std::byte value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_byte_value( + rosidl_dynamic_data_.get(), static_cast(value), &out); + return out; +} + + +// STRINGS ========================================================================================= +template<> +std::string +DynamicData::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); + free(buf); + return out; +} + + +template<> +std::u16string +DynamicData::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); + free(buf); + return out; +} + + +template<> +std::string +DynamicData::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +std::u16string +DynamicData::get_value(const std::string & name) +{ + return get_value(get_member_id(name)); +} + + +template<> +void +DynamicData::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::string value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_string_value( + rosidl_dynamic_data_.get(), id, value.c_str(), value.size()); +} + + +template<> +void +DynamicData::set_value( + rosidl_dynamic_typesupport_member_id_t id, const std::u16string value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_wstring_value( + rosidl_dynamic_data_.get(), id, value.c_str(), value.size()); +} + + +template<> +void +DynamicData::set_value(const std::string & name, const std::string value) +{ + set_value(get_member_id(name), value); +} + + +template<> +void +DynamicData::set_value(const std::string & name, const std::u16string value) +{ + set_value(get_member_id(name), value); +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicData::insert_value(const std::string value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_string_value( + rosidl_dynamic_data_.get(), value.c_str(), value.size(), &out); + return out; +} + + +template<> +rosidl_dynamic_typesupport_member_id_t +DynamicData::insert_value(const std::u16string value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_wstring_value( + rosidl_dynamic_data_.get(), value.c_str(), value.size(), &out); + return out; +} + + +// THROW FOR UNSUPPORTED TYPES ===================================================================== +template +ValueT +DynamicData::get_value(rosidl_dynamic_typesupport_member_id_t id) +{ + throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type"); +} + + +template +ValueT +DynamicData::get_value(const std::string & name) +{ + throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type"); +} + + +template +void +DynamicData::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 +DynamicData::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 +DynamicData::insert_value(ValueT value) +{ + throw rclcpp::exceptions::UnimplementedError("insert_value is not implemented for input type"); +} + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#undef __DYNAMIC_DATA_GET_VALUE_BY_ID_FN +#undef __DYNAMIC_DATA_GET_VALUE_BY_NAME_FN +#undef __DYNAMIC_DATA_SET_VALUE_BY_ID_FN +#undef __DYNAMIC_DATA_SET_VALUE_BY_NAME_FN +#undef __DYNAMIC_DATA_INSERT_VALUE +#undef DYNAMIC_DATA_DEFINITIONS + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_DATA_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_type_builder_impl.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_type_builder_impl.hpp new file mode 100644 index 0000000000..e71d4702bd --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_type_builder_impl.hpp @@ -0,0 +1,162 @@ +// 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_TYPE_BUILDER_IMPL_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_TYPE_BUILDER_IMPL_HPP_ + +#include +#include +#include +#include + +#include +#include +#include "rclcpp/exceptions.hpp" + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_BUILDER_HPP_ +#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp" +#endif + + +#define __DYNAMIC_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicTypeBuilder::add_member( \ + rosidl_dynamic_typesupport_member_id_t id, const std::string & name) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _member( \ + rosidl_dynamic_type_builder_.get(), id, name.c_str(), name.size()); \ + } + +#define __DYNAMIC_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicTypeBuilder::add_array_member( \ + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _array_member( \ + rosidl_dynamic_type_builder_.get(), id, name.c_str(), name.size(), array_length); \ + } + +#define __DYNAMIC_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicTypeBuilder::add_unbounded_sequence_member( \ + rosidl_dynamic_typesupport_member_id_t id, const std::string & name) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _unbounded_sequence_member( \ + rosidl_dynamic_type_builder_.get(), id, name.c_str(), name.size()); \ + } + +#define __DYNAMIC_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + template<> \ + void \ + DynamicTypeBuilder::add_bounded_sequence_member( \ + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound) \ + { \ + rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _bounded_sequence_member( \ + rosidl_dynamic_type_builder_.get(), id, name.c_str(), name.size(), sequence_bound); \ + } + +#define DYNAMIC_TYPE_BUILDER_DEFINITIONS(MemberT, FunctionT) \ + __DYNAMIC_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \ + __DYNAMIC_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_TYPE_BUILDER_DEFINITIONS(bool, bool); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(std::byte, byte); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(char, char); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(float, float32); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(double, float64); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(int8_t, int8); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(int16_t, int16); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(int32_t, int32); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(int64_t, int64); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(uint8_t, uint8); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(uint16_t, uint16); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(uint32_t, uint32); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(uint64_t, uint64); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(std::string, string); +DYNAMIC_TYPE_BUILDER_DEFINITIONS(std::u16string, wstring); + + +// THROW FOR UNSUPPORTED TYPES ===================================================================== +template +void +DynamicTypeBuilder::add_member(rosidl_dynamic_typesupport_member_id_t id, const std::string & name) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_member is not implemented for input type"); +} + + +template +void +DynamicTypeBuilder::add_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_array_member is not implemented for input type"); +} + + +template +void +DynamicTypeBuilder::add_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_unbounded_sequence_member is not implemented for input type"); +} + + +template +void +DynamicTypeBuilder::add_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound) +{ + throw rclcpp::exceptions::UnimplementedError( + "add_bounded_sequence_member is not implemented for input type"); +} + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#undef __DYNAMIC_TYPE_BUILDER_ADD_MEMBER_FN +#undef __DYNAMIC_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN +#undef __DYNAMIC_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN +#undef __DYNAMIC_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN +#undef DYNAMIC_TYPE_BUILDER_DEFINITIONS + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_TYPE_BUILDER_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_data.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_data.hpp new file mode 100644 index 0000000000..36a6de270a --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_data.hpp @@ -0,0 +1,374 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_DATA_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_DATA_HPP_ + + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include +#include + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + + +class DynamicType; +class DynamicTypeBuilder; + +/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t * +/** + * This class: + * - Manages the lifetime of the raw pointer. + * - Exposes getter methods to get the raw pointer and shared pointers + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed + * DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport. + * - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer + * must point to the same location in memory as the stored raw pointer! + */ +class DynamicData : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicData) + + // 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 DynamicData with the provided dynamic type builder + RCLCPP_PUBLIC + explicit DynamicData(std::shared_ptr dynamic_type_builder); + + /// Construct a new DynamicData with the provided dynamic type + RCLCPP_PUBLIC + explicit DynamicData(std::shared_ptr dynamic_type); + + /// Assume ownership of raw pointer + RCLCPP_PUBLIC + DynamicData( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data); + + /// Copy shared pointer + RCLCPP_PUBLIC + DynamicData( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_data); + + /// Loaning constructor + /// Must only be called with raw ptr obtained from loaning! + // NOTE(methylDragon): I'd put this in protected, but I need this exposed to + // enable_shared_from_this... + RCLCPP_PUBLIC + DynamicData( + DynamicData::SharedPtr parent_data, + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_loaned_data); + + // NOTE(methylDragon): Deliberately no constructor from description to nudge users towards using + // construction from dynamic type/builder, which is more efficient + + /// Copy constructor + RCLCPP_PUBLIC + DynamicData(const DynamicData & other); + + /// Move constructor + RCLCPP_PUBLIC + DynamicData(DynamicData && other) noexcept; + + /// Copy assignment + RCLCPP_PUBLIC + DynamicData & operator=(const DynamicData & other); + + /// Move assignment + RCLCPP_PUBLIC + DynamicData & operator=(DynamicData && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicData(); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_data_t * + get_rosidl_dynamic_data(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_data_t * + get_rosidl_dynamic_data() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_dynamic_data(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_dynamic_data() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + RCLCPP_PUBLIC + size_t + get_item_count() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_member_id(size_t index) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_member_id(const std::string & name) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_array_index(size_t index) const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + get_array_index(const std::string & name) const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + DynamicData + clone() const; + + RCLCPP_PUBLIC + DynamicData::SharedPtr + clone_shared() const; + + RCLCPP_PUBLIC + bool + equals(const DynamicData & other) const; + + RCLCPP_PUBLIC + DynamicData::SharedPtr + loan_value(rosidl_dynamic_typesupport_member_id_t id); + + RCLCPP_PUBLIC + DynamicData::SharedPtr + loan_value(const std::string & name); + + RCLCPP_PUBLIC + void + clear_all_values(); + + RCLCPP_PUBLIC + void + clear_nonkey_values(); + + RCLCPP_PUBLIC + void + clear_value(rosidl_dynamic_typesupport_member_id_t id); + + RCLCPP_PUBLIC + void + clear_value(const std::string & name); + + RCLCPP_PUBLIC + void + clear_sequence(); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_sequence_data(); + + RCLCPP_PUBLIC + void + remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index); + + RCLCPP_PUBLIC + void + print() const; + + 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); + + + // 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 + DynamicData + get_complex_value(rosidl_dynamic_typesupport_member_id_t id); + + RCLCPP_PUBLIC + DynamicData + get_complex_value(const std::string & name); + + RCLCPP_PUBLIC + DynamicData::SharedPtr + get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id); + + RCLCPP_PUBLIC + DynamicData::SharedPtr + get_complex_value_shared(const std::string & name); + + RCLCPP_PUBLIC + void + set_complex_value(rosidl_dynamic_typesupport_member_id_t id, DynamicData & value); + + RCLCPP_PUBLIC + void + set_complex_value(const std::string & name, DynamicData & value); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_complex_value_copy(const DynamicData & value); + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_member_id_t + insert_complex_value(DynamicData & value); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // DynamicSerializationSupport + DynamicSerializationSupport::SharedPtr serialization_support_; + + std::shared_ptr rosidl_dynamic_data_; + + bool is_loaned_; + DynamicData::SharedPtr parent_data_; // Used for returning the loaned value, and lifetime management + +private: + RCLCPP_PUBLIC + DynamicData(); + + RCLCPP_PUBLIC + bool + match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_data_t & dynamic_data); +}; + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_DATA_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp new file mode 100644 index 0000000000..9fe612141e --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ + + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_data.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + + +// NOTE(methylDragon): We just alias the type in this case... +// I'd have made a wrapper class but then I'd need to redirect every single +// method (or dynamic cast everywhere else), so.. no thanks. +using DynamicMessage = DynamicData; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp new file mode 100644 index 0000000000..3520179a56 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + + +// NOTE(methylDragon): We just alias the type in this case... +// I'd have made a wrapper class but then I'd need to redirect every single +// method (or dynamic cast everywhere else), so.. no thanks. +using DynamicMessageType = DynamicType; + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp new file mode 100644 index 0000000000..596dead05f --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp @@ -0,0 +1,195 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include +#include +#include + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for rosidl_message_type_support_t * containing managed +/// instances of the typesupport handle impl. +/** + * + * NOTE: This class is the recommended way to obtain the dynamic message type + * support struct, instead of rcl_get_dynamic_message_typesupport_handle, + * because this class will manage the lifetimes for you. + * + * Do NOT call rcl_dynamic_message_typesupport_handle_fini!! + * + * This class: + * - Manages the lifetime of the raw pointer. + * - Exposes getter methods to get the raw pointer and shared pointers + * - Stores shared pointers to wrapper classes that expose the underlying + * serialization support API + * + * Ownership: + * - This class, similarly to the rosidl_dynamic_typesupport_serialization_support_t, must outlive + * all downstream usages of the serialization support. + */ +class DynamicMessageTypeSupport : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport) + + // CONSTRUCTION ================================================================================== + /// From description + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + rosidl_runtime_c__type_description__TypeDescription * description, + const std::string & serialization_library_name = ""); + + /// From description, for provided serialization support + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_runtime_c__type_description__TypeDescription * description); + + /// Assume ownership of managed types + RCLCPP_PUBLIC + DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + rosidl_runtime_c__type_description__TypeDescription * description = nullptr); + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeSupport(); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_library_identifier() const; + + RCLCPP_PUBLIC + rosidl_message_type_support_t * + get_rosidl_message_type_support(); + + RCLCPP_PUBLIC + const rosidl_message_type_support_t * + get_rosidl_message_type_support() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_message_type_support(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_message_type_support() const; + + RCLCPP_PUBLIC + rosidl_runtime_c__type_description__TypeDescription * + get_rosidl_runtime_c_type_description(); + + RCLCPP_PUBLIC + const rosidl_runtime_c__type_description__TypeDescription * + get_rosidl_runtime_c_type_description() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_runtime_c_type_description(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_runtime_c_type_description() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + RCLCPP_PUBLIC + DynamicMessageType::SharedPtr + get_shared_dynamic_message_type(); + + RCLCPP_PUBLIC + DynamicMessageType::ConstSharedPtr + get_shared_dynamic_message_type() const; + + RCLCPP_PUBLIC + DynamicMessage::SharedPtr + get_shared_dynamic_message(); + + RCLCPP_PUBLIC + DynamicMessage::ConstSharedPtr + get_shared_dynamic_message() const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + void + print_description() const; + +protected: + RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport) + + DynamicSerializationSupport::SharedPtr serialization_support_; + DynamicMessageType::SharedPtr dynamic_message_type_; + DynamicMessage::SharedPtr dynamic_message_; + std::shared_ptr description_; + + std::shared_ptr rosidl_message_type_support_; + +private: + RCLCPP_PUBLIC + DynamicMessageTypeSupport(); + + RCLCPP_PUBLIC + void + init_serialization_support_(const std::string & serialization_library_name); + + RCLCPP_PUBLIC + void + init_dynamic_message_type_( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription * description); + + RCLCPP_PUBLIC + void + init_dynamic_message_(DynamicType::SharedPtr dynamic_type); + + RCLCPP_PUBLIC + void + init_rosidl_message_type_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + rosidl_runtime_c__type_description__TypeDescription * description); +}; + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp new file mode 100644 index 0000000000..41964e3e9e --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp @@ -0,0 +1,110 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ + +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t * +/** + * This class: + * - Manages the lifetime of the raw pointer. + * - Exposes getter methods to get the raw pointer and shared pointers + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class, similarly to the rosidl_dynamic_typesupport_serialization_support_t, must outlive + * all downstream usages of the serialization support. + */ +class DynamicSerializationSupport : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport) + + // CONSTRUCTION ================================================================================== + /// Get the rmw middleware implementation specific serialization support (configured by name) + RCLCPP_PUBLIC + explicit DynamicSerializationSupport(const std::string & serialization_library_name = ""); + + /// Assume ownership of raw pointer + RCLCPP_PUBLIC + explicit DynamicSerializationSupport( + rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support); + + /// Copy shared pointer + RCLCPP_PUBLIC + DynamicSerializationSupport( + std::shared_ptr serialization_support); + + /// Move constructor + RCLCPP_PUBLIC + DynamicSerializationSupport(DynamicSerializationSupport && other) noexcept; + + /// Move assignment + RCLCPP_PUBLIC + DynamicSerializationSupport & operator=(DynamicSerializationSupport && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicSerializationSupport(); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_library_identifier() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_serialization_support_t * + get_rosidl_serialization_support(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_serialization_support_t * + get_rosidl_serialization_support() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_serialization_support(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_serialization_support() const; + +protected: + RCLCPP_DISABLE_COPY(DynamicSerializationSupport) + + std::shared_ptr rosidl_serialization_support_; + +private: + RCLCPP_PUBLIC + DynamicSerializationSupport(); +}; + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_type.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_type.hpp new file mode 100644 index 0000000000..d61cdf8196 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_type.hpp @@ -0,0 +1,196 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_HPP_ + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + + +class DynamicData; +class DynamicTypeBuilder; + +/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_type_t * +/** + * This class: + * - Manages the lifetime of the raw pointer. + * - Exposes getter methods to get the raw pointer and shared pointers + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed + * DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport. + * - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer + * must point to the same location in memory as the stored raw pointer! + */ +class DynamicType : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicType) + + // 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 DynamicType with the provided dynamic type builder + RCLCPP_PUBLIC + explicit DynamicType(std::shared_ptr dynamic_type_builder); + + /// Assume ownership of raw pointer + RCLCPP_PUBLIC + DynamicType( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type); + + /// Copy shared pointer + RCLCPP_PUBLIC + DynamicType( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_type); + + /// From description + RCLCPP_PUBLIC + DynamicType( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription * description); + + /// Copy constructor + RCLCPP_PUBLIC + DynamicType(const DynamicType & other); + + /// Move constructor + RCLCPP_PUBLIC + DynamicType(DynamicType && other) noexcept; + + /// Copy assignment + RCLCPP_PUBLIC + DynamicType & operator=(const DynamicType & other); + + /// Move assignment + RCLCPP_PUBLIC + DynamicType & operator=(DynamicType && other) noexcept; + + RCLCPP_PUBLIC + virtual ~DynamicType(); + + /// Swaps the serialization support if serialization_support is populated + RCLCPP_PUBLIC + void + init_from_description( + const rosidl_runtime_c__type_description__TypeDescription * description, + DynamicSerializationSupport::SharedPtr serialization_support = nullptr); + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + size_t + get_member_count() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_type_t * + get_rosidl_dynamic_type(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_type_t * + get_rosidl_dynamic_type() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_dynamic_type(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_dynamic_type() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + DynamicType + clone() const; + + RCLCPP_PUBLIC + DynamicType::SharedPtr + clone_shared() const; + + RCLCPP_PUBLIC + bool + equals(const DynamicType & other) const; + + RCLCPP_PUBLIC + DynamicData + build_data(); + + RCLCPP_PUBLIC + std::shared_ptr + build_data_shared(); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // DynamicSerializationSupport + DynamicSerializationSupport::SharedPtr serialization_support_; + + std::shared_ptr rosidl_dynamic_type_; + +private: + RCLCPP_PUBLIC + DynamicType(); + + RCLCPP_PUBLIC + bool + match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type); +}; + + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_type_builder.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_type_builder.hpp new file mode 100644 index 0000000000..60cd03968f --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_type_builder.hpp @@ -0,0 +1,342 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_BUILDER_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_BUILDER_HPP_ + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include + + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +class DynamicData; +class DynamicType; + +/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_type_builder_t * +/** + * This class: + * - Manages the lifetime of the raw pointer. + * - Exposes getter methods to get the raw pointer and shared pointers + * - Exposes the underlying serialization support API + * + * Ownership: + * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed + * DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport. + * - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer + * must point to the same location in memory as the stored raw pointer! + */ +class DynamicTypeBuilder : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicTypeBuilder) + + // 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 DynamicTypeBuilder with the provided serialization support + RCLCPP_PUBLIC + DynamicTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name); + + /// Assume ownership of raw pointer + RCLCPP_PUBLIC + DynamicTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_builder_t * dynamic_type_builder); + + /// Copy shared pointer + RCLCPP_PUBLIC + DynamicTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr dynamic_type_builder); + + /// Copy constructor + RCLCPP_PUBLIC + DynamicTypeBuilder(const DynamicTypeBuilder & other); + + /// Move constructor + RCLCPP_PUBLIC + DynamicTypeBuilder(DynamicTypeBuilder && other) noexcept; + + /// Copy assignment + RCLCPP_PUBLIC + DynamicTypeBuilder & operator=(const DynamicTypeBuilder & other); + + /// Move assignment + RCLCPP_PUBLIC + DynamicTypeBuilder & operator=(DynamicTypeBuilder && other) noexcept; + + /// From description + RCLCPP_PUBLIC + DynamicTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription * description); + + RCLCPP_PUBLIC + virtual ~DynamicTypeBuilder(); + + /// Swaps the serialization support if serialization_support is populated + RCLCPP_PUBLIC + void + init_from_description( + const rosidl_runtime_c__type_description__TypeDescription * description, + DynamicSerializationSupport::SharedPtr serialization_support = nullptr); + + + // GETTERS ======================================================================================= + RCLCPP_PUBLIC + const std::string + get_library_identifier() const; + + RCLCPP_PUBLIC + const std::string + get_name() const; + + RCLCPP_PUBLIC + rosidl_dynamic_typesupport_dynamic_type_builder_t * + get_rosidl_dynamic_type_builder(); + + RCLCPP_PUBLIC + const rosidl_dynamic_typesupport_dynamic_type_builder_t * + get_rosidl_dynamic_type_builder() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_dynamic_type_builder(); + + RCLCPP_PUBLIC + std::shared_ptr + get_shared_rosidl_dynamic_type_builder() const; + + RCLCPP_PUBLIC + DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support(); + + RCLCPP_PUBLIC + DynamicSerializationSupport::ConstSharedPtr + get_shared_dynamic_serialization_support() const; + + + // METHODS ======================================================================================= + RCLCPP_PUBLIC + void + set_name(const std::string & name); + + RCLCPP_PUBLIC + DynamicTypeBuilder + clone() const; + + RCLCPP_PUBLIC + DynamicTypeBuilder::SharedPtr + clone_shared() const; + + RCLCPP_PUBLIC + void + clear(); + + RCLCPP_PUBLIC + DynamicData + build_data(); + + RCLCPP_PUBLIC + std::shared_ptr + build_data_shared(); + + RCLCPP_PUBLIC + DynamicType + build_type(); + + RCLCPP_PUBLIC + std::shared_ptr + build_type_shared(); + + + // ADD MEMBERS TEMPLATES ========================================================================= + /** + * Since we're in a ROS layer, these should support all ROS interface C++ types as found in: + * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html + * + * Explicitly: + * - Basic types: bool, byte, char + * - Float types: float, double + * - Int types: int8_t, int16_t, int32_t, int64_t + * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t + * - String types: std::string, std::u16string + */ + + template + void + add_member(rosidl_dynamic_typesupport_member_id_t id, const std::string & name); + + template + void + add_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length); + + template + void + add_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name); + + template + void + add_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound); + + + // 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); + + RCLCPP_PUBLIC + void + add_bounded_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound); + + 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); + + 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); + + RCLCPP_PUBLIC + void + add_bounded_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound); + + RCLCPP_PUBLIC + void + add_bounded_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound); + + 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); + + 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); + + + // ADD NESTED MEMBERS ============================================================================ + RCLCPP_PUBLIC + void + add_complex_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicType & nested_type); + + RCLCPP_PUBLIC + void + add_complex_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicType & nested_type, size_t array_length); + + RCLCPP_PUBLIC + void + add_complex_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicType & nested_type); + + RCLCPP_PUBLIC + void + add_complex_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicType & nested_type, size_t sequence_bound); + + RCLCPP_PUBLIC + void + add_complex_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicTypeBuilder & nested_type_builder); + + RCLCPP_PUBLIC + void + add_complex_array_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicTypeBuilder & nested_type_builder, size_t array_length); + + RCLCPP_PUBLIC + void + add_complex_unbounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicTypeBuilder & nested_type_builder); + + RCLCPP_PUBLIC + void + add_complex_bounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicTypeBuilder & nested_type_builder, size_t sequence_bound); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // DynamicSerializationSupport + DynamicSerializationSupport::SharedPtr serialization_support_; + + std::shared_ptr rosidl_dynamic_type_builder_; + +private: + RCLCPP_PUBLIC + DynamicTypeBuilder(); + + RCLCPP_PUBLIC + void + init_from_serialization_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name); + + RCLCPP_PUBLIC + bool + match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_builder_t & dynamic_type_builder); +}; + + +} // namespace dynamic_typesupport +} // namespace rclcpp + + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_BUILDER_HPP_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index f1d751ff3f..d84610d159 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -117,6 +117,22 @@ * - Allocator related items: * - rclcpp/allocator/allocator_common.hpp * - rclcpp/allocator/allocator_deleter.hpp + * - Dynamic typesupport wrappers + * - rclcpp::dynamic_typesupport::DynamicData + * - rclcpp::dynamic_typesupport::DynamicMessage + * - rclcpp::dynamic_typesupport::DynamicMessageType + * - rclcpp::dynamic_typesupport::DynamicSerializationSupport + * - rclcpp::dynamic_typesupport::DynamicTypeBuilder + * - rclcpp::dynamic_typesupport::DynamicType + * - rclcpp/dynamic_typesupport/dynamic_data.hpp + * - rclcpp/dynamic_typesupport/dynamic_message.hpp + * - rclcpp/dynamic_typesupport/dynamic_message_type.hpp + * - rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp + * - rclcpp/dynamic_typesupport/dynamic_type_builder.hpp + * - rclcpp/dynamic_typesupport/dynamic_type.hpp + * - Dynamic typesupport + * - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport + * - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp * - Generic publisher * - rclcpp::Node::create_generic_publisher() * - rclcpp::GenericPublisher diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 38773e2ec2..f7c89260bc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -39,6 +39,7 @@ rcpputils rcutils rmw + rosidl_dynamic_typesupport statistics_msgs tracetools diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_data.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_data.cpp new file mode 100644 index 0000000000..499a8127d8 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_data.cpp @@ -0,0 +1,641 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_data.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp" +#include "rclcpp/exceptions.hpp" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" + +#include +#include +#include + +using rclcpp::dynamic_typesupport::DynamicData; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; +using rclcpp::dynamic_typesupport::DynamicType; +using rclcpp::dynamic_typesupport::DynamicTypeBuilder; + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_DATA_IMPL_HPP_ +// Template specialization implementations +#include "rclcpp/dynamic_typesupport/detail/dynamic_data_impl.hpp" +#endif + + +// CONSTRUCTION ================================================================================== +DynamicData::DynamicData(const DynamicTypeBuilder::SharedPtr dynamic_type_builder) +: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(nullptr), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic type could not bind serialization support!"); + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = + dynamic_type_builder->get_rosidl_dynamic_type_builder(); + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("dynamic type builder cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rosidl_dynamic_data = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type_builder( + rosidl_dynamic_type_builder); + if (!rosidl_dynamic_data) { + throw std::runtime_error("could not create new dynamic data object"); + } + + rosidl_dynamic_data_.reset( + rosidl_dynamic_data, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void { + rosidl_dynamic_typesupport_dynamic_data_fini(rosidl_dynamic_data); + free(rosidl_dynamic_data); + }); +} + + +DynamicData::DynamicData(const DynamicType::SharedPtr dynamic_type) +: serialization_support_(dynamic_type->get_shared_dynamic_serialization_support()), + rosidl_dynamic_data_(nullptr), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic type could not bind serialization support!"); + } + + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = + dynamic_type->get_rosidl_dynamic_type(); + if (!rosidl_dynamic_type) { + throw std::runtime_error("dynamic type cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr; + rosidl_dynamic_data = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type( + rosidl_dynamic_type); + if (!rosidl_dynamic_data) { + throw std::runtime_error("could not create new dynamic data object"); + } + + rosidl_dynamic_data_.reset( + rosidl_dynamic_data, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void { + rosidl_dynamic_typesupport_dynamic_data_fini(rosidl_dynamic_data); + free(rosidl_dynamic_data); + }); +} + + +DynamicData::DynamicData( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data) +: serialization_support_(serialization_support), + rosidl_dynamic_data_(nullptr), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!rosidl_dynamic_data) { + throw std::runtime_error("rosidl dynamic data cannot be nullptr!"); + } + if (serialization_support) { + if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_data)) { + throw std::runtime_error( + "serialization support library identifier does not match dynamic data's!"); + } + } + + rosidl_dynamic_data_.reset( + rosidl_dynamic_data, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void { + rosidl_dynamic_typesupport_dynamic_data_fini(rosidl_dynamic_data); + free(rosidl_dynamic_data); + }); +} + + +DynamicData::DynamicData( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_data) +: serialization_support_(serialization_support), + rosidl_dynamic_data_(rosidl_dynamic_data), + is_loaned_(false), + parent_data_(nullptr) +{ + if (!rosidl_dynamic_data) { + throw std::runtime_error("rosidl dynamic data cannot be nullptr!"); + } + if (serialization_support) { + if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_data)) { + throw std::runtime_error( + "serialization support library identifier does not match dynamic data's!"); + } + } +} + + +DynamicData::DynamicData( + DynamicData::SharedPtr parent_data, + rosidl_dynamic_typesupport_dynamic_data_t * rosidl_loaned_data) +: DynamicData(parent_data->get_shared_dynamic_serialization_support(), rosidl_loaned_data) +{ + if (!parent_data) { + throw std::runtime_error("parent dynamic data cannot be nullptr!"); + } + if (!rosidl_loaned_data) { + throw std::runtime_error("loaned rosidl dynamic data cannot be nullptr!"); + } + + parent_data_ = parent_data; + is_loaned_ = true; +} + + +DynamicData::DynamicData(const DynamicData & other) +: enable_shared_from_this(), + serialization_support_(nullptr), + rosidl_dynamic_data_(nullptr), + is_loaned_(false), + parent_data_(nullptr) +{ + DynamicData out = other.clone(); + // We don't copy is_loaned_ or parent_data_ because it's a fresh copy now + std::swap(serialization_support_, out.serialization_support_); + std::swap(rosidl_dynamic_data_, out.rosidl_dynamic_data_); +} + + +DynamicData::DynamicData(DynamicData && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_data_(std::exchange(other.rosidl_dynamic_data_, nullptr)), + is_loaned_(other.is_loaned_), + parent_data_(std::exchange(other.parent_data_, nullptr)) +{} + + +DynamicData & +DynamicData::operator=(const DynamicData & other) +{ + return *this = DynamicData(other); +} + + +DynamicData & +DynamicData::operator=(DynamicData && 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; +} + + +DynamicData::~DynamicData() +{ + if (is_loaned_) { + if (!parent_data_) { + RCUTILS_LOG_ERROR("dynamic data is loaned, but parent is missing!!"); + } else { + rosidl_dynamic_typesupport_dynamic_data_return_loaned_value( + parent_data_->get_rosidl_dynamic_data(), get_rosidl_dynamic_data()); + } + } +} + + +bool +DynamicData::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_data_t & rosidl_dynamic_type_data) +{ + bool out = true; + + if (serialization_support.get_library_identifier() != std::string( + rosidl_dynamic_type_data.serialization_support->library_identifier)) + { + RCUTILS_LOG_ERROR("serialization support library identifier does not match dynamic data's"); + out = false; + } + + // TODO(methylDragon): Can I do this?? Is it portable? + if (serialization_support.get_rosidl_serialization_support() != + rosidl_dynamic_type_data.serialization_support) + { + RCUTILS_LOG_ERROR("serialization support pointer does not match dynamic data's"); + out = false; + } + + return out; +} + + +// GETTERS ======================================================================================= +const std::string +DynamicData::get_library_identifier() const +{ + return std::string(rosidl_dynamic_data_->serialization_support->library_identifier); +} + + +const std::string +DynamicData::get_name() const +{ + size_t buf_length; + const char * buf = rosidl_dynamic_typesupport_dynamic_data_get_name( + get_rosidl_dynamic_data(), &buf_length); + return std::string(buf, buf_length); +} + + +rosidl_dynamic_typesupport_dynamic_data_t * +DynamicData::get_rosidl_dynamic_data() +{ + return rosidl_dynamic_data_.get(); +} + + +const rosidl_dynamic_typesupport_dynamic_data_t * +DynamicData::get_rosidl_dynamic_data() const +{ + return rosidl_dynamic_data_.get(); +} + + +std::shared_ptr +DynamicData::get_shared_rosidl_dynamic_data() +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_data_.get()); +} + + +std::shared_ptr +DynamicData::get_shared_rosidl_dynamic_data() const +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_data_.get()); +} + + +DynamicSerializationSupport::SharedPtr +DynamicData::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicData::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +size_t +DynamicData::get_item_count() const +{ + return rosidl_dynamic_typesupport_dynamic_data_get_item_count(get_rosidl_dynamic_data()); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicData::get_member_id(size_t index) const +{ + return rosidl_dynamic_typesupport_dynamic_data_get_member_id_at_index( + get_rosidl_dynamic_data(), index); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicData::get_member_id(const std::string & name) const +{ + return rosidl_dynamic_typesupport_dynamic_data_get_member_id_by_name( + get_rosidl_dynamic_data(), name.c_str(), name.size()); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicData::get_array_index(size_t index) const +{ + return rosidl_dynamic_typesupport_dynamic_data_get_array_index( + get_rosidl_dynamic_data(), index); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicData::get_array_index(const std::string & name) const +{ + return get_array_index(get_member_id(name)); +} + + +// METHODS ======================================================================================= +DynamicData +DynamicData::clone() const +{ + return DynamicData( + serialization_support_, + rosidl_dynamic_typesupport_dynamic_data_clone(get_rosidl_dynamic_data())); +} + + +DynamicData::SharedPtr +DynamicData::clone_shared() const +{ + return DynamicData::make_shared( + serialization_support_, + rosidl_dynamic_typesupport_dynamic_data_clone(get_rosidl_dynamic_data())); +} + + +bool +DynamicData::equals(const DynamicData & other) const +{ + if (get_library_identifier() != other.get_library_identifier()) { + throw std::runtime_error("library identifiers don't match"); + } + return rosidl_dynamic_typesupport_dynamic_data_equals( + get_rosidl_dynamic_data(), other.get_rosidl_dynamic_data()); +} + + +DynamicData::SharedPtr +DynamicData::loan_value(rosidl_dynamic_typesupport_member_id_t id) +{ + return DynamicData::make_shared( + shared_from_this(), + rosidl_dynamic_typesupport_dynamic_data_loan_value( + get_rosidl_dynamic_data(), id)); +} + + +DynamicData::SharedPtr +DynamicData::loan_value(const std::string & name) +{ + return loan_value(get_member_id(name)); +} + + +void +DynamicData::clear_all_values() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_all_values(get_rosidl_dynamic_data()); +} + + +void +DynamicData::clear_nonkey_values() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_nonkey_values(get_rosidl_dynamic_data()); +} + + +void +DynamicData::clear_value(rosidl_dynamic_typesupport_member_id_t id) +{ + rosidl_dynamic_typesupport_dynamic_data_clear_value(get_rosidl_dynamic_data(), id); +} + + +void +DynamicData::clear_value(const std::string & name) +{ + clear_value(get_member_id(name)); +} + + +void +DynamicData::clear_sequence() +{ + rosidl_dynamic_typesupport_dynamic_data_clear_sequence_data(get_rosidl_dynamic_data()); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicData::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 +DynamicData::remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index) +{ + rosidl_dynamic_typesupport_dynamic_data_remove_sequence_data( + get_rosidl_dynamic_data(), index); +} + + +void +DynamicData::print() const +{ + rosidl_dynamic_typesupport_dynamic_data_print(get_rosidl_dynamic_data()); +} + + +bool +DynamicData::serialize(rcl_serialized_message_t * buffer) +{ + return rosidl_dynamic_typesupport_dynamic_data_serialize(get_rosidl_dynamic_data(), buffer); +} + + +bool +DynamicData::deserialize(rcl_serialized_message_t * buffer) +{ + return rosidl_dynamic_typesupport_dynamic_data_deserialize(get_rosidl_dynamic_data(), buffer); +} + + +// MEMBER ACCESS =================================================================================== +// Defined in "detail/dynamic_data_impl.hpp" + + +// BOUNDED STRING MEMBER ACCESS ==================================================================== +const std::string +DynamicData::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); + free(buf); + return out; +} + + +const std::string +DynamicData::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 +DynamicData::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); + free(buf); + return out; +} + + +const std::u16string +DynamicData::get_bounded_wstring_value(const std::string & name, size_t wstring_bound) +{ + return get_bounded_wstring_value(get_member_id(name), wstring_bound); +} + + +void +DynamicData::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 +DynamicData::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 +DynamicData::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 +DynamicData::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 +DynamicData::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 +DynamicData::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 ============================================================================ +DynamicData +DynamicData::get_complex_value(rosidl_dynamic_typesupport_member_id_t id) +{ + rosidl_dynamic_typesupport_dynamic_data_t * out_ptr = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_complex_value( + get_rosidl_dynamic_data(), id, &out_ptr); + return DynamicData(get_shared_dynamic_serialization_support(), out_ptr); +} + + +DynamicData +DynamicData::get_complex_value(const std::string & name) +{ + return get_complex_value(get_member_id(name)); +} + + +DynamicData::SharedPtr +DynamicData::get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id) +{ + rosidl_dynamic_typesupport_dynamic_data_t * out_ptr = nullptr; + rosidl_dynamic_typesupport_dynamic_data_get_complex_value( + get_rosidl_dynamic_data(), id, &out_ptr); + return DynamicData::make_shared(get_shared_dynamic_serialization_support(), out_ptr); +} + + +DynamicData::SharedPtr +DynamicData::get_complex_value_shared(const std::string & name) +{ + return get_complex_value_shared(get_member_id(name)); +} + + +void +DynamicData::set_complex_value( + rosidl_dynamic_typesupport_member_id_t id, DynamicData & value) +{ + rosidl_dynamic_typesupport_dynamic_data_set_complex_value( + get_rosidl_dynamic_data(), id, value.get_rosidl_dynamic_data()); +} + + +void +DynamicData::set_complex_value(const std::string & name, DynamicData & value) +{ + set_complex_value(get_member_id(name), value); +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicData::insert_complex_value_copy(const DynamicData & value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_complex_value_copy( + get_rosidl_dynamic_data(), value.get_rosidl_dynamic_data(), &out); + return out; +} + + +rosidl_dynamic_typesupport_member_id_t +DynamicData::insert_complex_value(DynamicData & value) +{ + rosidl_dynamic_typesupport_member_id_t out; + rosidl_dynamic_typesupport_dynamic_data_insert_complex_value( + get_rosidl_dynamic_data(), value.get_rosidl_dynamic_data(), &out); + return out; +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp new file mode 100644 index 0000000000..479be09b52 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp @@ -0,0 +1,396 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rmw/dynamic_typesupport.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp" + +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rcutils/logging_macros.h" + +#include +#include +#include "rosidl_runtime_c/type_description_utils.h" +#include +#include + +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport; + + +// CONSTRUCTION ==================================================================================== +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + rosidl_runtime_c__type_description__TypeDescription * description, + const std::string & serialization_library_name) +: serialization_support_(nullptr), + dynamic_message_type_(nullptr), + dynamic_message_(nullptr), + description_(nullptr), + rosidl_message_type_support_(nullptr) +{ + if (!description) { + throw std::runtime_error("description cannot be nullptr!"); + } + description_.reset( + description, + [](rosidl_runtime_c__type_description__TypeDescription * description) -> void { + rosidl_runtime_c__type_description__TypeDescription__destroy(description); + }); + + init_serialization_support_(serialization_library_name); + if (!serialization_support_) { + throw std::runtime_error("could not init dynamic serialization support!"); + } + + init_dynamic_message_type_(serialization_support_, description); + if (!dynamic_message_type_) { + throw std::runtime_error("could not init dynamic message type!"); + } + + init_dynamic_message_(dynamic_message_type_); + if (!dynamic_message_) { + throw std::runtime_error("could not init dynamic message!"); + } + + init_rosidl_message_type_support_( + serialization_support_, dynamic_message_type_, dynamic_message_, description_.get()); + if (!rosidl_message_type_support_) { + throw std::runtime_error("could not init rosidl message type support!"); + } +} + + +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_runtime_c__type_description__TypeDescription * description) +: serialization_support_(serialization_support), + dynamic_message_type_(nullptr), + dynamic_message_(nullptr), + description_(nullptr), + rosidl_message_type_support_(nullptr) +{ + // Check null + if (!serialization_support) { + throw std::runtime_error("serialization_support cannot be nullptr!"); + } + if (!description) { + throw std::runtime_error("description cannot be nullptr!"); + } + description_.reset( + description, + [](rosidl_runtime_c__type_description__TypeDescription * description) -> void { + rosidl_runtime_c__type_description__TypeDescription__destroy(description); + }); + + // Init + init_dynamic_message_type_(serialization_support_, description); + if (!dynamic_message_type_) { + throw std::runtime_error("could not init dynamic message type!"); + } + + init_dynamic_message_(dynamic_message_type_); + if (!dynamic_message_) { + throw std::runtime_error("could not init dynamic message!"); + } + + init_rosidl_message_type_support_( + serialization_support_, dynamic_message_type_, dynamic_message_, description_.get()); + if (!rosidl_message_type_support_) { + throw std::runtime_error("could not init rosidl message type support!"); + } +} + + +DynamicMessageTypeSupport::DynamicMessageTypeSupport( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + rosidl_runtime_c__type_description__TypeDescription * description) +: serialization_support_(serialization_support), + dynamic_message_type_(dynamic_message_type), + dynamic_message_(dynamic_message), + description_(nullptr), + rosidl_message_type_support_(nullptr) +{ + // Check null + if (!serialization_support) { + throw std::runtime_error("serialization_support cannot be nullptr!"); + } + if (!dynamic_message_type) { + throw std::runtime_error("dynamic_message_type cannot be nullptr!"); + } + if (!dynamic_message) { + throw std::runtime_error("dynamic_message cannot be nullptr!"); + } + + if (description) { + description_.reset( + description, + [](rosidl_runtime_c__type_description__TypeDescription * description) -> void { + rosidl_runtime_c__type_description__TypeDescription__destroy(description); + }); + } + + // Check identifiers + if (serialization_support->get_library_identifier() != + dynamic_message_type->get_library_identifier()) + { + throw std::runtime_error( + "serialization support library identifier does not match " + "dynamic message type library identifier!"); + } + if (dynamic_message_type->get_library_identifier() != dynamic_message->get_library_identifier()) { + throw std::runtime_error( + "dynamic message type library identifier does not match " + "dynamic message library identifier!"); + } + + // Check pointers + /* *INDENT-OFF* */ + if (serialization_support->get_rosidl_serialization_support() != + dynamic_message_type + ->get_shared_dynamic_serialization_support() + ->get_rosidl_serialization_support()) + { + throw std::runtime_error("serialization support pointer dynamic message type's"); + } + if (dynamic_message_type + ->get_shared_dynamic_serialization_support() + ->get_rosidl_serialization_support() != + dynamic_message_type + ->get_shared_dynamic_serialization_support() + ->get_rosidl_serialization_support()) + { + throw std::runtime_error("serialization support pointer dynamic message type's"); + } + /* *INDENT-ON* */ + + init_rosidl_message_type_support_( + serialization_support_, dynamic_message_type_, dynamic_message_, description_.get()); + if (!rosidl_message_type_support_) { + throw std::runtime_error("could not init rosidl message type support!"); + } +} + + +DynamicMessageTypeSupport::~DynamicMessageTypeSupport() {} + + +void +DynamicMessageTypeSupport::init_serialization_support_( + const std::string & serialization_library_name) +{ + serialization_support_ = DynamicSerializationSupport::make_shared(serialization_library_name); +} + + +void +DynamicMessageTypeSupport::init_dynamic_message_type_( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription * description) +{ + dynamic_message_type_ = DynamicMessageType::make_shared(serialization_support, description); +} + + +void +DynamicMessageTypeSupport::init_dynamic_message_(DynamicType::SharedPtr dynamic_type) +{ + dynamic_message_ = DynamicMessage::make_shared(dynamic_type); +} + + +void +DynamicMessageTypeSupport::init_rosidl_message_type_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + DynamicMessageType::SharedPtr dynamic_message_type, + DynamicMessage::SharedPtr dynamic_message, + rosidl_runtime_c__type_description__TypeDescription * description) +{ + bool middleware_supports_type_discovery = + rmw_feature_supported(RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY); + bool middleware_can_take_dynamic_data = + rmw_feature_supported(RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_DATA); + + if (!middleware_supports_type_discovery && !description) { + RCUTILS_LOG_ERROR_NAMED( + rmw_dynamic_typesupport_c__identifier, + "Middleware does not support type discovery! Deferred dynamic type" + "message type support will never be populated! You must provide a type " + "description!"); + return; + } + + // NOTE(methylDragon): We don't finalize the rosidl_message_type_support->data since its members + // are managed by the passed in SharedPtr wrapper classes + rosidl_message_type_support_.reset( + new rosidl_message_type_support_t(), + [](rosidl_message_type_support_t * ts) -> void {free(const_cast(ts->data));} + ); + + if (!rosidl_message_type_support_) { + RCUTILS_LOG_ERROR_NAMED( + rmw_dynamic_typesupport_c__identifier, + "Could not allocate rosidl_message_type_support_t struct"); + return; + } + + rosidl_message_type_support_->typesupport_identifier = rmw_dynamic_typesupport_c__identifier; + + // NOTE(methylDragon): To populate dynamic_type and description if deferred, OUTSIDE + rosidl_message_type_support_->data = calloc(1, sizeof(rmw_dynamic_typesupport_impl_t)); + if (!rosidl_message_type_support_->data) { + RCUTILS_LOG_ERROR_NAMED( + rmw_dynamic_typesupport_c__identifier, + "Could not allocate rmw_dynamic_typesupport_impl_t struct"); + rosidl_message_type_support_.reset(); + } + + rmw_dynamic_typesupport_impl_t * ts_impl = + (rmw_dynamic_typesupport_impl_t *)rosidl_message_type_support_->data; + + ts_impl->take_dynamic_data = middleware_can_take_dynamic_data; + ts_impl->serialization_support = serialization_support->get_rosidl_serialization_support(); + ts_impl->dynamic_type = dynamic_message_type->get_rosidl_dynamic_type(); + ts_impl->dynamic_data = dynamic_message->get_rosidl_dynamic_data(); + ts_impl->description = description; + rosidl_message_type_support_->func = get_message_typesupport_handle_function; +} + + +// GETTERS ========================================================================================= +const std::string +DynamicMessageTypeSupport::get_library_identifier() const +{ + return serialization_support_->get_library_identifier(); +} + + +rosidl_message_type_support_t * +DynamicMessageTypeSupport::get_rosidl_message_type_support() +{ + return rosidl_message_type_support_.get(); +} + + +const rosidl_message_type_support_t * +DynamicMessageTypeSupport::get_rosidl_message_type_support() const +{ + return rosidl_message_type_support_.get(); +} + + +std::shared_ptr +DynamicMessageTypeSupport::get_shared_rosidl_message_type_support() +{ + return rosidl_message_type_support_; +} + + +std::shared_ptr +DynamicMessageTypeSupport::get_shared_rosidl_message_type_support() const +{ + return rosidl_message_type_support_; +} + + +rosidl_runtime_c__type_description__TypeDescription * +DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() +{ + return description_.get(); +} + + +const rosidl_runtime_c__type_description__TypeDescription * +DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() const +{ + return description_.get(); +} + + +std::shared_ptr +DynamicMessageTypeSupport::get_shared_rosidl_runtime_c_type_description() +{ + return description_; +} + + +std::shared_ptr +DynamicMessageTypeSupport::get_shared_rosidl_runtime_c_type_description() const +{ + return description_; +} + + +DynamicSerializationSupport::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +DynamicMessageType::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message_type() +{ + return dynamic_message_type_; +} + + +DynamicMessageType::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message_type() const +{ + return dynamic_message_type_; +} + + +DynamicMessage::SharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message() +{ + return dynamic_message_; +} + + +DynamicMessage::ConstSharedPtr +DynamicMessageTypeSupport::get_shared_dynamic_message() const +{ + return dynamic_message_; +} + + +// METHODS ========================================================================================= +void +DynamicMessageTypeSupport::print_description() const +{ + if (!description_) { + RCUTILS_LOG_ERROR("Can't print description, no bound description!"); + } + rosidl_runtime_c_type_description_utils_print_type_description(description_.get()); +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp new file mode 100644 index 0000000000..f5d61960f1 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -0,0 +1,129 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" +#include "rcutils/logging_macros.h" +#include "rmw/dynamic_typesupport.h" + +#include + +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +// CONSTRUCTION ==================================================================================== +DynamicSerializationSupport::DynamicSerializationSupport( + const std::string & serialization_library_name) +: rosidl_serialization_support_(nullptr) +{ + rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support = nullptr; + + if (serialization_library_name.empty()) { + rosidl_serialization_support = rmw_get_serialization_support(NULL); + } else { + rosidl_serialization_support = + rmw_get_serialization_support(serialization_library_name.c_str()); + } + + if (!rosidl_serialization_support) { + throw std::runtime_error("could not create new serialization support object"); + } + + rosidl_serialization_support_.reset( + rosidl_serialization_support, + // Custom deleter + [](rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) -> void { + rosidl_dynamic_typesupport_serialization_support_destroy(rosidl_serialization_support); + }); +} + + +DynamicSerializationSupport::DynamicSerializationSupport( + rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) +: rosidl_serialization_support_(nullptr) +{ + if (!rosidl_serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + + // Custom deleter + rosidl_serialization_support_.reset( + rosidl_serialization_support, + [](rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) -> void { + rosidl_dynamic_typesupport_serialization_support_destroy(rosidl_serialization_support); + }); +} + + +DynamicSerializationSupport::DynamicSerializationSupport( + std::shared_ptr rosidl_serialization_support) +: rosidl_serialization_support_(rosidl_serialization_support) {} + + +DynamicSerializationSupport::DynamicSerializationSupport( + DynamicSerializationSupport && other) noexcept +: rosidl_serialization_support_(std::exchange(other.rosidl_serialization_support_, nullptr)) {} + + +DynamicSerializationSupport & +DynamicSerializationSupport::operator=(DynamicSerializationSupport && other) noexcept +{ + std::swap(rosidl_serialization_support_, other.rosidl_serialization_support_); + return *this; +} + + +DynamicSerializationSupport::~DynamicSerializationSupport() {} + + +// GETTERS ========================================================================================= +const std::string +DynamicSerializationSupport::get_library_identifier() const +{ + return std::string( + rosidl_dynamic_typesupport_serialization_support_get_library_identifier( + rosidl_serialization_support_.get())); +} + + +rosidl_dynamic_typesupport_serialization_support_t * +DynamicSerializationSupport::get_rosidl_serialization_support() +{ + return rosidl_serialization_support_.get(); +} + + +const rosidl_dynamic_typesupport_serialization_support_t * +DynamicSerializationSupport::get_rosidl_serialization_support() const +{ + return rosidl_serialization_support_.get(); +} + + +std::shared_ptr +DynamicSerializationSupport::get_shared_rosidl_serialization_support() +{ + return std::shared_ptr( + shared_from_this(), rosidl_serialization_support_.get()); +} + + +std::shared_ptr +DynamicSerializationSupport::get_shared_rosidl_serialization_support() const +{ + return std::shared_ptr( + shared_from_this(), rosidl_serialization_support_.get()); +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_type.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_type.cpp new file mode 100644 index 0000000000..a189d88d53 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_type.cpp @@ -0,0 +1,320 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_data.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp" +#include "rclcpp/exceptions.hpp" +#include "rcutils/logging_macros.h" + +#include +#include +#include + +using rclcpp::dynamic_typesupport::DynamicData; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; +using rclcpp::dynamic_typesupport::DynamicType; +using rclcpp::dynamic_typesupport::DynamicTypeBuilder; + + +// CONSTRUCTION ==================================================================================== +DynamicType::DynamicType(DynamicTypeBuilder::SharedPtr dynamic_type_builder) +: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()), + rosidl_dynamic_type_(nullptr) +{ + if (!serialization_support_) { + throw std::runtime_error("dynamic type could not bind serialization support!"); + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = + dynamic_type_builder->get_rosidl_dynamic_type_builder(); + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("dynamic type builder cannot be nullptr!"); + } + + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr; + rosidl_dynamic_type = rosidl_dynamic_typesupport_dynamic_type_init_from_dynamic_type_builder( + rosidl_dynamic_type_builder); + if (!rosidl_dynamic_type) { + throw std::runtime_error("could not create new dynamic type object"); + } + + rosidl_dynamic_type_.reset( + rosidl_dynamic_type, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void { + rosidl_dynamic_typesupport_dynamic_type_fini(rosidl_dynamic_type); + free(rosidl_dynamic_type); + }); +} + + +DynamicType::DynamicType( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type) +: serialization_support_(serialization_support), rosidl_dynamic_type_(nullptr) +{ + if (!rosidl_dynamic_type) { + throw std::runtime_error("rosidl dynamic type cannot be nullptr!"); + } + if (serialization_support) { + if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type)) { + throw std::runtime_error( + "serialization support library identifier does not match dynamic type's!"); + } + } + + rosidl_dynamic_type_.reset( + rosidl_dynamic_type, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void { + rosidl_dynamic_typesupport_dynamic_type_fini(rosidl_dynamic_type); + free(rosidl_dynamic_type); + }); +} + + +DynamicType::DynamicType( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_type) +: serialization_support_(serialization_support), rosidl_dynamic_type_(rosidl_dynamic_type) +{ + if (!rosidl_dynamic_type) { + throw std::runtime_error("rosidl dynamic type cannot be nullptr!"); + } + if (serialization_support) { + if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type)) { + throw std::runtime_error( + "serialization support library identifier does not match dynamic type's!"); + } + } +} + + +DynamicType::DynamicType( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription * description) +: serialization_support_(serialization_support), rosidl_dynamic_type_(nullptr) +{ + init_from_description(description, serialization_support); +} + + +DynamicType::DynamicType(const DynamicType & other) +: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_(nullptr) +{ + DynamicType out = other.clone(); + std::swap(serialization_support_, out.serialization_support_); + std::swap(rosidl_dynamic_type_, out.rosidl_dynamic_type_); +} + + +DynamicType::DynamicType(DynamicType && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_type_(std::exchange(other.rosidl_dynamic_type_, nullptr)) {} + + +DynamicType & +DynamicType::operator=(const DynamicType & other) +{ + return *this = DynamicType(other); +} + + +DynamicType & +DynamicType::operator=(DynamicType && other) noexcept +{ + std::swap(serialization_support_, other.serialization_support_); + std::swap(rosidl_dynamic_type_, other.rosidl_dynamic_type_); + return *this; +} + + +DynamicType::~DynamicType() {} + + +void +DynamicType::init_from_description( + const rosidl_runtime_c__type_description__TypeDescription * description, + DynamicSerializationSupport::SharedPtr serialization_support) +{ + if (!description) { + throw std::runtime_error("description cannot be nullptr!"); + } + if (serialization_support) { + // Swap serialization support if serialization support is given + serialization_support_ = serialization_support; + } + + rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr; + rosidl_dynamic_type = + rosidl_dynamic_typesupport_dynamic_type_init_from_description( + serialization_support_->get_rosidl_serialization_support(), description); + if (!rosidl_dynamic_type) { + throw std::runtime_error("could not create new dynamic type object"); + } + + rosidl_dynamic_type_.reset( + rosidl_dynamic_type, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void { + rosidl_dynamic_typesupport_dynamic_type_fini(rosidl_dynamic_type); + free(rosidl_dynamic_type); + }); +} + + +bool +DynamicType::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type) +{ + bool out = true; + + if (serialization_support.get_library_identifier() != std::string( + rosidl_dynamic_type.serialization_support->library_identifier)) + { + RCUTILS_LOG_ERROR( + "serialization support library identifier does not match dynamic type's"); + out = false; + } + + // TODO(methylDragon): Can I do this?? Is it portable? + if (serialization_support.get_rosidl_serialization_support() != + rosidl_dynamic_type.serialization_support) + { + RCUTILS_LOG_ERROR( + "serialization support pointer does not match dynamic type's"); + out = false; + } + + return out; +} + + +// GETTERS ========================================================================================= +const std::string +DynamicType::get_library_identifier() const +{ + return std::string(rosidl_dynamic_type_->serialization_support->library_identifier); +} + + +const std::string +DynamicType::get_name() const +{ + size_t buf_length; + const char * buf = rosidl_dynamic_typesupport_dynamic_type_get_name( + get_rosidl_dynamic_type(), &buf_length); + return std::string(buf, buf_length); +} + + +size_t +DynamicType::get_member_count() const +{ + return rosidl_dynamic_typesupport_dynamic_type_get_member_count(rosidl_dynamic_type_.get()); +} + + +rosidl_dynamic_typesupport_dynamic_type_t * +DynamicType::get_rosidl_dynamic_type() +{ + return rosidl_dynamic_type_.get(); +} + + +const rosidl_dynamic_typesupport_dynamic_type_t * +DynamicType::get_rosidl_dynamic_type() const +{ + return rosidl_dynamic_type_.get(); +} + + +std::shared_ptr +DynamicType::get_shared_rosidl_dynamic_type() +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_type_.get()); +} + + +std::shared_ptr +DynamicType::get_shared_rosidl_dynamic_type() const +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_type_.get()); +} + + +DynamicSerializationSupport::SharedPtr +DynamicType::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicType::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +// METHODS ========================================================================================= +DynamicType +DynamicType::clone() const +{ + return DynamicType( + serialization_support_, + rosidl_dynamic_typesupport_dynamic_type_clone(get_rosidl_dynamic_type())); +} + + +DynamicType::SharedPtr +DynamicType::clone_shared() const +{ + return DynamicType::make_shared( + serialization_support_, + rosidl_dynamic_typesupport_dynamic_type_clone(get_rosidl_dynamic_type())); +} + + +bool +DynamicType::equals(const DynamicType & other) const +{ + if (get_library_identifier() != other.get_library_identifier()) { + throw std::runtime_error("library identifiers don't match"); + } + return rosidl_dynamic_typesupport_dynamic_type_equals( + get_rosidl_dynamic_type(), other.get_rosidl_dynamic_type()); +} + + +DynamicData +DynamicType::build_data() +{ + return DynamicData(shared_from_this()); +} + + +DynamicData::SharedPtr +DynamicType::build_data_shared() +{ + return DynamicData::make_shared(shared_from_this()); +} diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_type_builder.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_type_builder.cpp new file mode 100644 index 0000000000..6f205ffe2b --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_type_builder.cpp @@ -0,0 +1,536 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_data.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp" +#include "rclcpp/exceptions.hpp" +#include "rcutils/logging_macros.h" + +#include +#include +#include + +using rclcpp::dynamic_typesupport::DynamicData; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; +using rclcpp::dynamic_typesupport::DynamicType; +using rclcpp::dynamic_typesupport::DynamicTypeBuilder; + + +#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_TYPE_BUILDER_IMPL_HPP_ +// Template specialization implementations +#include "rclcpp/dynamic_typesupport/detail/dynamic_type_builder_impl.hpp" +#endif + + +// CONSTRUCTION ================================================================================== +DynamicTypeBuilder::DynamicTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, const std::string & name) +: serialization_support_(serialization_support), rosidl_dynamic_type_builder_(nullptr) +{ + init_from_serialization_support_(serialization_support, name); + if (!rosidl_dynamic_type_builder_) { + throw std::runtime_error("could not create new dynamic type builder object"); + } +} + + +DynamicTypeBuilder::DynamicTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder) +: serialization_support_(serialization_support), rosidl_dynamic_type_builder_(nullptr) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("rosidl dynamic type builder cannot be nullptr!"); + } + if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type_builder)) { + throw std::runtime_error( + "serialization support library does not match dynamic type builder's!"); + } + + rosidl_dynamic_type_builder_.reset( + rosidl_dynamic_type_builder, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void { + rosidl_dynamic_typesupport_dynamic_type_builder_fini(rosidl_dynamic_type_builder); + free(rosidl_dynamic_type_builder); + }); +} + + +DynamicTypeBuilder::DynamicTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + std::shared_ptr rosidl_dynamic_type_builder) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_(rosidl_dynamic_type_builder) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("rosidl dynamic type builder cannot be nullptr!"); + } + if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type_builder.get())) { + throw std::runtime_error( + "serialization support library does not match dynamic type builder's!"); + } +} + + +DynamicTypeBuilder::DynamicTypeBuilder( + DynamicSerializationSupport::SharedPtr serialization_support, + const rosidl_runtime_c__type_description__TypeDescription * description) +: serialization_support_(serialization_support), + rosidl_dynamic_type_builder_(nullptr) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + init_from_description(description, serialization_support); +} + + +DynamicTypeBuilder::DynamicTypeBuilder(const DynamicTypeBuilder & other) +: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_builder_(nullptr) +{ + DynamicTypeBuilder out = other.clone(); + std::swap(serialization_support_, out.serialization_support_); + std::swap(rosidl_dynamic_type_builder_, out.rosidl_dynamic_type_builder_); +} + + +DynamicTypeBuilder::DynamicTypeBuilder(DynamicTypeBuilder && other) noexcept +: serialization_support_(std::exchange(other.serialization_support_, nullptr)), + rosidl_dynamic_type_builder_(std::exchange(other.rosidl_dynamic_type_builder_, nullptr)) {} + + +DynamicTypeBuilder & +DynamicTypeBuilder::operator=(const DynamicTypeBuilder & other) +{ + return *this = DynamicTypeBuilder(other); +} + + +DynamicTypeBuilder & +DynamicTypeBuilder::operator=(DynamicTypeBuilder && other) noexcept +{ + std::swap(serialization_support_, other.serialization_support_); + std::swap(rosidl_dynamic_type_builder_, other.rosidl_dynamic_type_builder_); + return *this; +} + + +DynamicTypeBuilder::~DynamicTypeBuilder() {} + + +void +DynamicTypeBuilder::init_from_description( + const rosidl_runtime_c__type_description__TypeDescription * description, + DynamicSerializationSupport::SharedPtr serialization_support) +{ + if (!description) { + throw std::runtime_error("description cannot be nullptr!"); + } + if (serialization_support) { + // Swap serialization support if serialization support is given + serialization_support_ = serialization_support; + } + + rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr; + rosidl_dynamic_type_builder = + rosidl_dynamic_typesupport_dynamic_type_builder_init_from_description( + serialization_support_->get_rosidl_serialization_support(), description); + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("could not create new dynamic type builder object"); + } + + rosidl_dynamic_type_builder_.reset( + rosidl_dynamic_type_builder, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void { + rosidl_dynamic_typesupport_dynamic_type_builder_fini(rosidl_dynamic_type_builder); + free(rosidl_dynamic_type_builder); + }); +} + + +void +DynamicTypeBuilder::init_from_serialization_support_( + DynamicSerializationSupport::SharedPtr serialization_support, + const std::string & name) +{ + if (!serialization_support) { + throw std::runtime_error("serialization support cannot be nullptr!"); + } + if (!serialization_support->get_rosidl_serialization_support()) { + throw std::runtime_error("serialization support raw pointer cannot be nullptr!"); + } + + + rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr; + rosidl_dynamic_type_builder = rosidl_dynamic_typesupport_dynamic_type_builder_init( + serialization_support->get_rosidl_serialization_support(), name.c_str(), name.size()); + + if (!rosidl_dynamic_type_builder) { + throw std::runtime_error("could not create new dynamic type builder object"); + } + + rosidl_dynamic_type_builder_.reset( + rosidl_dynamic_type_builder, + // Custom deleter + [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void { + rosidl_dynamic_typesupport_dynamic_type_builder_fini(rosidl_dynamic_type_builder); + free(rosidl_dynamic_type_builder); + }); +} + + +bool +DynamicTypeBuilder::match_serialization_support_( + const DynamicSerializationSupport & serialization_support, + const rosidl_dynamic_typesupport_dynamic_type_builder_t & rosidl_dynamic_type_builder) +{ + bool out = true; + + if (serialization_support.get_library_identifier() != std::string( + rosidl_dynamic_type_builder.serialization_support->library_identifier)) + { + RCUTILS_LOG_ERROR( + "serialization support library identifier does not match dynamic type builder's"); + out = false; + } + + // TODO(methylDragon): Can I do this?? Is it portable? + if (serialization_support.get_rosidl_serialization_support() != + rosidl_dynamic_type_builder.serialization_support) + { + RCUTILS_LOG_ERROR( + "serialization support pointer does not match dynamic type builder's"); + out = false; + } + + return out; +} + + +// GETTERS ======================================================================================= +const std::string +DynamicTypeBuilder::get_library_identifier() const +{ + return std::string(rosidl_dynamic_type_builder_->serialization_support->library_identifier); +} + + +const std::string +DynamicTypeBuilder::get_name() const +{ + size_t buf_length; + const char * buf = rosidl_dynamic_typesupport_dynamic_type_builder_get_name( + get_rosidl_dynamic_type_builder(), &buf_length); + return std::string(buf, buf_length); +} + + +rosidl_dynamic_typesupport_dynamic_type_builder_t * +DynamicTypeBuilder::get_rosidl_dynamic_type_builder() +{ + return rosidl_dynamic_type_builder_.get(); +} + + +const rosidl_dynamic_typesupport_dynamic_type_builder_t * +DynamicTypeBuilder::get_rosidl_dynamic_type_builder() const +{ + return rosidl_dynamic_type_builder_.get(); +} + + +std::shared_ptr +DynamicTypeBuilder::get_shared_rosidl_dynamic_type_builder() +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_type_builder_.get()); +} + + +std::shared_ptr +DynamicTypeBuilder::get_shared_rosidl_dynamic_type_builder() const +{ + return std::shared_ptr( + shared_from_this(), rosidl_dynamic_type_builder_.get()); +} + + +DynamicSerializationSupport::SharedPtr +DynamicTypeBuilder::get_shared_dynamic_serialization_support() +{ + return serialization_support_; +} + + +DynamicSerializationSupport::ConstSharedPtr +DynamicTypeBuilder::get_shared_dynamic_serialization_support() const +{ + return serialization_support_; +} + + +// METHODS ======================================================================================= +void +DynamicTypeBuilder::set_name(const std::string & name) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_set_name( + get_rosidl_dynamic_type_builder(), name.c_str(), name.size()); +} + + +DynamicTypeBuilder +DynamicTypeBuilder::clone() const +{ + return DynamicTypeBuilder( + serialization_support_, + rosidl_dynamic_typesupport_dynamic_type_builder_clone(get_rosidl_dynamic_type_builder())); +} + + +DynamicTypeBuilder::SharedPtr +DynamicTypeBuilder::clone_shared() const +{ + return DynamicTypeBuilder::make_shared( + serialization_support_, + rosidl_dynamic_typesupport_dynamic_type_builder_clone(get_rosidl_dynamic_type_builder())); +} + + +void +DynamicTypeBuilder::clear() +{ + if (!serialization_support_) { + throw std::runtime_error( + "cannot call clear() on a dynamic type builder with uninitialized serialization support"); + } + + const std::string & name = get_name(); + init_from_serialization_support_(serialization_support_, name); + if (!rosidl_dynamic_type_builder_) { + throw std::runtime_error("could not create new dynamic type builder object"); + } +} + + +DynamicData +DynamicTypeBuilder::build_data() +{ + return DynamicData(shared_from_this()); +} + + +DynamicData::SharedPtr +DynamicTypeBuilder::build_data_shared() +{ + return DynamicData::make_shared(shared_from_this()); +} + + +DynamicType +DynamicTypeBuilder::build_type() +{ + return DynamicType(shared_from_this()); +} + + +DynamicType::SharedPtr +DynamicTypeBuilder::build_type_shared() +{ + return DynamicType::make_shared(shared_from_this()); +} + + +// ADD MEMBERS ===================================================================================== +// Defined in "detail/dynamic_type_builder_impl.hpp" + + +// ADD BOUNDED STRING MEMBERS ====================================================================== +void +DynamicTypeBuilder::add_bounded_string_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), string_bound); +} + + +void +DynamicTypeBuilder::add_bounded_wstring_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), wstring_bound); +} + + +void +DynamicTypeBuilder::add_bounded_string_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t string_bound, size_t array_length) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_array_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), string_bound, array_length); +} + + +void +DynamicTypeBuilder::add_bounded_wstring_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + size_t wstring_bound, size_t array_length) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_array_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), wstring_bound, array_length); +} + + +void +DynamicTypeBuilder::add_bounded_string_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_unbounded_sequence_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), string_bound); +} + + +void +DynamicTypeBuilder::add_bounded_wstring_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_unbounded_sequence_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), wstring_bound); +} + + +void +DynamicTypeBuilder::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) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_bounded_sequence_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + string_bound, sequence_bound); +} + + +void +DynamicTypeBuilder::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) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_bounded_sequence_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + wstring_bound, sequence_bound); +} + + +// ADD NESTED MEMBERS ============================================================================== +void +DynamicTypeBuilder::add_complex_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicType & nested_type) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + nested_type.get_rosidl_dynamic_type()); +} + + +void +DynamicTypeBuilder::add_complex_array_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicType & nested_type, size_t array_length) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + nested_type.get_rosidl_dynamic_type(), array_length); +} + + +void +DynamicTypeBuilder::add_complex_unbounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicType & nested_type) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + nested_type.get_rosidl_dynamic_type()); +} + + +void +DynamicTypeBuilder::add_complex_bounded_sequence_member( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicType & nested_type, size_t sequence_bound) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + nested_type.get_rosidl_dynamic_type(), sequence_bound); +} + + +void +DynamicTypeBuilder::add_complex_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicTypeBuilder & nested_type_builder) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member_builder( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + nested_type_builder.get_rosidl_dynamic_type_builder()); +} + + +void +DynamicTypeBuilder::add_complex_array_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicTypeBuilder & nested_type_builder, size_t array_length) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member_builder( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + nested_type_builder.get_rosidl_dynamic_type_builder(), array_length); +} + + +void +DynamicTypeBuilder::add_complex_unbounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicTypeBuilder & nested_type_builder) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member_builder( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + nested_type_builder.get_rosidl_dynamic_type_builder()); +} + + +void +DynamicTypeBuilder::add_complex_bounded_sequence_member_builder( + rosidl_dynamic_typesupport_member_id_t id, const std::string & name, + DynamicTypeBuilder & nested_type_builder, size_t sequence_bound) +{ + rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member_builder( + get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), + nested_type_builder.get_rosidl_dynamic_type_builder(), sequence_bound); +}