diff --git a/rmw_dps_cpp/include/rmw_dps_cpp/TypeSupport.hpp b/rmw_dps_cpp/include/rmw_dps_cpp/TypeSupport.hpp index 0f4da8f..131a5da 100644 --- a/rmw_dps_cpp/include/rmw_dps_cpp/TypeSupport.hpp +++ b/rmw_dps_cpp/include/rmw_dps_cpp/TypeSupport.hpp @@ -15,10 +15,8 @@ #ifndef RMW_DPS_CPP__TYPESUPPORT_HPP_ #define RMW_DPS_CPP__TYPESUPPORT_HPP_ -#include -#include -#include -#include +#include "rosidl_runtime_c/string_functions.h" +#include "rosidl_runtime_c/u16string_functions.h" #include #include @@ -50,27 +48,27 @@ struct StringHelper; template<> struct StringHelper { - using type = rosidl_generator_c__String; + using type = rosidl_runtime_c__String; static std::string convert_to_std_string(void * data) { - auto c_string = static_cast(data); + auto c_string = static_cast(data); if (!c_string) { RCUTILS_LOG_ERROR_NAMED( "rmw_dps_cpp", - "Failed to cast data as rosidl_generator_c__String"); + "Failed to cast data as rosidl_runtime_c__String"); return ""; } if (!c_string->data) { RCUTILS_LOG_ERROR_NAMED( "rmw_dps_cpp", - "rosidl_generator_c_String had invalid data"); + "rosidl_runtime_c_String had invalid data"); return ""; } return std::string(c_string->data); } - static std::string convert_to_std_string(rosidl_generator_c__String & data) + static std::string convert_to_std_string(rosidl_runtime_c__String & data) { return std::string(data.data); } @@ -79,8 +77,8 @@ struct StringHelper { std::string str; deser >> str; - rosidl_generator_c__String * c_str = static_cast(field); - rosidl_generator_c__String__assign(c_str, str.c_str()); + rosidl_runtime_c__String * c_str = static_cast(field); + rosidl_runtime_c__String__assign(c_str, str.c_str()); } }; @@ -113,27 +111,27 @@ struct U16StringHelper; template<> struct U16StringHelper { - using type = rosidl_generator_c__U16String; + using type = rosidl_runtime_c__U16String; static std::u16string convert_to_std_u16string(void * data) { - auto c_u16string = static_cast(data); + auto c_u16string = static_cast(data); if (!c_u16string) { RCUTILS_LOG_ERROR_NAMED( "rmw_dps_cpp", - "Failed to cast data as rosidl_generator_c__U16String"); + "Failed to cast data as rosidl_runtime_c__U16String"); return u""; } if (!c_u16string->data) { RCUTILS_LOG_ERROR_NAMED( "rmw_dps_cpp", - "rosidl_generator_c_U16String had invalid data"); + "rosidl_runtime_c_U16String had invalid data"); return u""; } return std::u16string(reinterpret_cast(c_u16string->data)); } - static std::u16string convert_to_std_u16string(rosidl_generator_c__U16String & data) + static std::u16string convert_to_std_u16string(rosidl_runtime_c__U16String & data) { return std::u16string(reinterpret_cast(data.data)); } @@ -142,8 +140,8 @@ struct U16StringHelper { std::u16string str; deser >> str; - rosidl_generator_c__U16String * c_str = static_cast(field); - rosidl_generator_c__U16String__assign(c_str, reinterpret_cast(str.c_str())); + rosidl_runtime_c__U16String * c_str = static_cast(field); + rosidl_runtime_c__U16String__assign(c_str, reinterpret_cast(str.c_str())); } }; diff --git a/rmw_dps_cpp/include/rmw_dps_cpp/TypeSupport_impl.hpp b/rmw_dps_cpp/include/rmw_dps_cpp/TypeSupport_impl.hpp index 1baad75..8245a61 100644 --- a/rmw_dps_cpp/include/rmw_dps_cpp/TypeSupport_impl.hpp +++ b/rmw_dps_cpp/include/rmw_dps_cpp/TypeSupport_impl.hpp @@ -21,7 +21,7 @@ #include "rmw_dps_cpp/macros.hpp" -#include "rosidl_generator_c/primitives_sequence_functions.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" namespace rmw_dps_cpp { @@ -43,19 +43,19 @@ SPECIALIZE_GENERIC_C_SEQUENCE(uint32, uint32_t) SPECIALIZE_GENERIC_C_SEQUENCE(int64, int64_t) SPECIALIZE_GENERIC_C_SEQUENCE(uint64, uint64_t) -typedef struct rosidl_generator_c__void__Sequence +typedef struct rosidl_runtime_c__void__Sequence { void * data; /// The number of valid items in data size_t size; /// The number of allocated items in data size_t capacity; -} rosidl_generator_c__void__Sequence; +} rosidl_runtime_c__void__Sequence; inline bool -rosidl_generator_c__void__Sequence__init( - rosidl_generator_c__void__Sequence * sequence, size_t size, size_t member_size) +rosidl_runtime_c__void__Sequence__init( + rosidl_runtime_c__void__Sequence * sequence, size_t size, size_t member_size) { if (!sequence) { return false; @@ -75,7 +75,7 @@ rosidl_generator_c__void__Sequence__init( inline void -rosidl_generator_c__void__Sequence__fini(rosidl_generator_c__void__Sequence * sequence) +rosidl_runtime_c__void__Sequence__fini(rosidl_runtime_c__void__Sequence * sequence) { if (!sequence) { return; @@ -152,18 +152,18 @@ void serialize_field( } ser << str; } else { - // First, cast field to rosidl_generator_c + // First, cast field to rosidl_runtime_c // Then convert to a std::string using StringHelper and serialize the std::string std::vector cpp_string_vector; if (member->array_size_ && !member->is_upper_bound_) { - auto string_field = static_cast(field); + auto string_field = static_cast(field); for (size_t i = 0; i < member->array_size_; ++i) { cpp_string_vector.push_back( CStringHelper::convert_to_std_string(string_field[i])); } } else { auto & string_sequence_field = - *reinterpret_cast(field); + *reinterpret_cast(field); for (size_t i = 0; i < string_sequence_field.size; ++i) { cpp_string_vector.push_back( CStringHelper::convert_to_std_string(string_sequence_field.data[i])); @@ -189,18 +189,18 @@ void serialize_field( } ser << str; } else { - // First, cast field to rosidl_generator_c + // First, cast field to rosidl_runtime_c // Then convert to a std::u16string using U16StringHelper and serialize the std::u16string std::vector cpp_u16string_vector; if (member->array_size_ && !member->is_upper_bound_) { - auto u16string_field = static_cast(field); + auto u16string_field = static_cast(field); for (size_t i = 0; i < member->array_size_; ++i) { cpp_u16string_vector.push_back( CU16StringHelper::convert_to_std_u16string(u16string_field[i])); } } else { auto & u16string_sequence_field = - *reinterpret_cast(field); + *reinterpret_cast(field); for (size_t i = 0; i < u16string_sequence_field.size; ++i) { cpp_u16string_vector.push_back( CU16StringHelper::convert_to_std_u16string(u16string_sequence_field.data[i])); @@ -396,27 +396,27 @@ void deserialize_field( deser >> cpp_string_vector; if (member->array_size_ && !member->is_upper_bound_) { - auto deser_field = static_cast(field); + auto deser_field = static_cast(field); for (size_t i = 0; i < member->array_size_; ++i) { - if (!rosidl_generator_c__String__assign(&deser_field[i], + if (!rosidl_runtime_c__String__assign(&deser_field[i], cpp_string_vector[i].c_str())) { - throw std::runtime_error("unable to assign rosidl_generator_c__String"); + throw std::runtime_error("unable to assign rosidl_runtime_c__String"); } } } else { auto & string_sequence_field = - *reinterpret_cast(field); - if (!rosidl_generator_c__String__Sequence__init(&string_sequence_field, + *reinterpret_cast(field); + if (!rosidl_runtime_c__String__Sequence__init(&string_sequence_field, cpp_string_vector.size())) { - throw std::runtime_error("unable to initialize rosidl_generator_c__String sequence"); + throw std::runtime_error("unable to initialize rosidl_runtime_c__String sequence"); } for (size_t i = 0; i < cpp_string_vector.size(); ++i) { - if (!rosidl_generator_c__String__assign(&string_sequence_field.data[i], + if (!rosidl_runtime_c__String__assign(&string_sequence_field.data[i], cpp_string_vector[i].c_str())) { - throw std::runtime_error("unable to assign rosidl_generator_c__String"); + throw std::runtime_error("unable to assign rosidl_runtime_c__String"); } } } @@ -439,27 +439,27 @@ void deserialize_field( deser >> cpp_u16string_vector; if (member->array_size_ && !member->is_upper_bound_) { - auto deser_field = static_cast(field); + auto deser_field = static_cast(field); for (size_t i = 0; i < member->array_size_; ++i) { - if (!rosidl_generator_c__U16String__assign(&deser_field[i], + if (!rosidl_runtime_c__U16String__assign(&deser_field[i], reinterpret_cast(cpp_u16string_vector[i].c_str()))) { - throw std::runtime_error("unable to assign rosidl_generator_c__U16String"); + throw std::runtime_error("unable to assign rosidl_runtime_c__U16String"); } } } else { auto & u16string_sequence_field = - *reinterpret_cast(field); - if (!rosidl_generator_c__U16String__Sequence__init(&u16string_sequence_field, + *reinterpret_cast(field); + if (!rosidl_runtime_c__U16String__Sequence__init(&u16string_sequence_field, cpp_u16string_vector.size())) { - throw std::runtime_error("unable to initialize rosidl_generator_c__U16String sequence"); + throw std::runtime_error("unable to initialize rosidl_runtime_c__U16String sequence"); } for (size_t i = 0; i < cpp_u16string_vector.size(); ++i) { - if (!rosidl_generator_c__U16String__assign(&u16string_sequence_field.data[i], + if (!rosidl_runtime_c__U16String__assign(&u16string_sequence_field.data[i], reinterpret_cast(cpp_u16string_vector[i].c_str()))) { - throw std::runtime_error("unable to assign rosidl_generator_c__U16String"); + throw std::runtime_error("unable to assign rosidl_runtime_c__U16String"); } } } diff --git a/rmw_dps_cpp/include/rmw_dps_cpp/macros.hpp b/rmw_dps_cpp/include/rmw_dps_cpp/macros.hpp index 4902fcc..865dc52 100644 --- a/rmw_dps_cpp/include/rmw_dps_cpp/macros.hpp +++ b/rmw_dps_cpp/include/rmw_dps_cpp/macros.hpp @@ -22,14 +22,14 @@ template<> \ struct GenericCSequence \ { \ - using type = rosidl_generator_c__ ## C_NAME ## __Sequence; \ + using type = rosidl_runtime_c__ ## C_NAME ## __Sequence; \ \ static void fini(type * sequence) { \ - rosidl_generator_c__ ## C_NAME ## __Sequence__fini(sequence); \ + rosidl_runtime_c__ ## C_NAME ## __Sequence__fini(sequence); \ } \ \ static bool init(type * sequence, size_t size) { \ - return rosidl_generator_c__ ## C_NAME ## __Sequence__init(sequence, size); \ + return rosidl_runtime_c__ ## C_NAME ## __Sequence__init(sequence, size); \ } \ }; diff --git a/rmw_dps_cpp/src/rmw_node.cpp b/rmw_dps_cpp/src/rmw_node.cpp index c9e431c..f400133 100644 --- a/rmw_dps_cpp/src/rmw_node.cpp +++ b/rmw_dps_cpp/src/rmw_node.cpp @@ -264,17 +264,14 @@ rmw_create_node( const char * name, const char * namespace_, size_t domain_id, - const rmw_node_security_options_t * security_options, bool localhost_only) { // TODO(malsbat): implement RMW_SECURITY_ENFORCEMENT_PERMISSIVE, ENFORCE // TODO(malsbat): implement localhost_only RCUTILS_LOG_DEBUG_NAMED( "rmw_dps_cpp", - "%s(name=%s,namespace_=%s,domain_id=%zu," - "security_options={enforce_security=%d,security_root_path=%s},localhost_only=%d)", - __FUNCTION__, name, namespace_, domain_id, security_options->enforce_security, - security_options->security_root_path, localhost_only); + "%s(name=%s,namespace_=%s,domain_id=%zu,localhost_only=%d)", + __FUNCTION__, name, namespace_, domain_id, localhost_only); RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -287,10 +284,6 @@ rmw_create_node( RMW_SET_ERROR_MSG("name is null"); return nullptr; } - if (!security_options) { - RMW_SET_ERROR_MSG("security_options is null"); - return nullptr; - } return create_node(context, name, namespace_, domain_id); } diff --git a/rmw_dps_cpp/src/rmw_publisher.cpp b/rmw_dps_cpp/src/rmw_publisher.cpp index adf5ac5..bf0bcd6 100644 --- a/rmw_dps_cpp/src/rmw_publisher.cpp +++ b/rmw_dps_cpp/src/rmw_publisher.cpp @@ -32,7 +32,7 @@ extern "C" rmw_ret_t rmw_init_publisher_allocation( const rosidl_message_type_support_t * type_support, - const rosidl_message_bounds_t * message_bounds, + const rosidl_runtime_c__Sequence__bound * message_bounds, rmw_publisher_allocation_t * allocation) { // Unused in current implementation. diff --git a/rmw_dps_cpp/src/rmw_request.cpp b/rmw_dps_cpp/src/rmw_request.cpp index 0badb12..b7fe553 100644 --- a/rmw_dps_cpp/src/rmw_request.cpp +++ b/rmw_dps_cpp/src/rmw_request.cpp @@ -76,7 +76,7 @@ rmw_send_request( rmw_ret_t rmw_take_request( const rmw_service_t * service, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_request, bool * taken) { @@ -108,16 +108,16 @@ rmw_take_request( info->typesupport_identifier_); // Get header - memset(request_header->writer_guid, 0, sizeof(request_header->writer_guid)); + memset(request_header->request_id.writer_guid, 0, sizeof(request_header->request_id.writer_guid)); const DPS_UUID * uuid = DPS_PublicationGetUUID(pub.get()); if (uuid) { - memcpy(request_header->writer_guid, uuid, sizeof(DPS_UUID)); + memcpy(request_header->request_id.writer_guid, uuid, sizeof(DPS_UUID)); } - request_header->sequence_number = DPS_PublicationGetSequenceNum(pub.get()); - + request_header->request_id.sequence_number = DPS_PublicationGetSequenceNum(pub.get()); + *taken = true; - info->requests_.emplace(std::make_pair(*request_header, std::move(pub))); + info->requests_.emplace(std::make_pair(request_header->request_id, std::move(pub))); } return RMW_RET_OK; diff --git a/rmw_dps_cpp/src/rmw_response.cpp b/rmw_dps_cpp/src/rmw_response.cpp index 593bd0b..30b0a98 100644 --- a/rmw_dps_cpp/src/rmw_response.cpp +++ b/rmw_dps_cpp/src/rmw_response.cpp @@ -31,7 +31,7 @@ extern "C" rmw_ret_t rmw_take_response( const rmw_client_t * client, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_response, bool * taken) { @@ -63,12 +63,12 @@ rmw_take_response( info->typesupport_identifier_); // Get header - memset(request_header->writer_guid, 0, sizeof(request_header->writer_guid)); + memset(request_header->request_id.writer_guid, 0, sizeof(request_header->request_id.writer_guid)); const DPS_UUID * uuid = DPS_PublicationGetUUID(pub.get()); if (uuid) { - memcpy(request_header->writer_guid, uuid, sizeof(DPS_UUID)); + memcpy(request_header->request_id.writer_guid, uuid, sizeof(DPS_UUID)); } - request_header->sequence_number = DPS_AckGetSequenceNum(pub.get()); + request_header->request_id.sequence_number = DPS_AckGetSequenceNum(pub.get()); *taken = true; } diff --git a/rmw_dps_cpp/src/rmw_serialize.cpp b/rmw_dps_cpp/src/rmw_serialize.cpp index 92f7205..9f633d4 100644 --- a/rmw_dps_cpp/src/rmw_serialize.cpp +++ b/rmw_dps_cpp/src/rmw_serialize.cpp @@ -95,7 +95,7 @@ rmw_deserialize( rmw_ret_t rmw_get_serialized_message_size( const rosidl_message_type_support_t * /*type_support*/, - const rosidl_message_bounds_t * /*message_bounds*/, + const rosidl_runtime_c__Sequence__bound * /*message_bounds*/, size_t * /*size*/) { RMW_SET_ERROR_MSG("unimplemented"); diff --git a/rmw_dps_cpp/src/rmw_subscription.cpp b/rmw_dps_cpp/src/rmw_subscription.cpp index 25fd487..73d37be 100644 --- a/rmw_dps_cpp/src/rmw_subscription.cpp +++ b/rmw_dps_cpp/src/rmw_subscription.cpp @@ -33,7 +33,7 @@ extern "C" rmw_ret_t rmw_init_subscription_allocation( const rosidl_message_type_support_t * type_support, - const rosidl_message_bounds_t * message_bounds, + const rosidl_runtime_c__Sequence__bound * message_bounds, rmw_subscription_allocation_t * allocation) { // Unused in current implementation.