diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp index 61cb629a1..9235761fd 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp @@ -203,12 +203,14 @@ static size_t calculateMaxAlign(const MembersType * members) // C++ specialization template -void serialize_array( +void serialize_field( const rosidl_typesupport_introspection_cpp::MessageMember * member, void * field, eprosima::fastcdr::Cdr & ser) { - if (member->array_size_ && !member->is_upper_bound_) { + if (!member->is_array_) { + ser << *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { ser.serializeArray(static_cast(field), member->array_size_); } else { std::vector & data = *reinterpret_cast *>(field); @@ -218,12 +220,14 @@ void serialize_array( // C specialization template -void serialize_array( +void serialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, eprosima::fastcdr::Cdr & ser) { - if (member->array_size_ && !member->is_upper_bound_) { + if (!member->is_array_) { + ser << *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { ser.serializeArray(static_cast(field), member->array_size_); } else { auto & data = *reinterpret_cast::type *>(field); @@ -233,31 +237,40 @@ void serialize_array( template<> inline -void serialize_array( +void serialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, eprosima::fastcdr::Cdr & ser) { using CStringHelper = StringHelper; - // First, cast field to rosidl_generator_c - // Then convert to a std::string using StringHelper and serialize the std::string - if (member->array_size_ && !member->is_upper_bound_) { - // tmpstring is defined here and not below to avoid - // memory allocation in every iteration of the for loop - std::string tmpstring; - auto string_field = static_cast(field); - for (size_t i = 0; i < member->array_size_; ++i) { - tmpstring = string_field[i].data; - ser.serialize(tmpstring); + if (!member->is_array_) { + auto && str = CStringHelper::convert_to_std_string(field); + // Control maximum length. + if (member->string_upper_bound_ && str.length() > member->string_upper_bound_ + 1) { + throw std::runtime_error("string overcomes the maximum length"); } + ser << str; } else { - auto & string_array_field = *reinterpret_cast(field); - std::vector cpp_string_vector; - for (size_t i = 0; i < string_array_field.size; ++i) { - cpp_string_vector.push_back( - CStringHelper::convert_to_std_string(string_array_field.data[i])); + // First, cast field to rosidl_generator_c + // Then convert to a std::string using StringHelper and serialize the std::string + if (member->array_size_ && !member->is_upper_bound_) { + // tmpstring is defined here and not below to avoid + // memory allocation in every iteration of the for loop + std::string tmpstring; + auto string_field = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + tmpstring = string_field[i].data; + ser.serialize(tmpstring); + } + } else { + auto & string_array_field = *reinterpret_cast(field); + std::vector cpp_string_vector; + for (size_t i = 0; i < string_array_field.size; ++i) { + cpp_string_vector.push_back( + CStringHelper::convert_to_std_string(string_array_field.data[i])); + } + ser << cpp_string_vector; } - ser << cpp_string_vector; } } @@ -304,114 +317,57 @@ bool TypeSupport::serializeROSmessage( for (uint32_t i = 0; i < members->member_count_; ++i) { const auto member = members->members_ + i; void * field = const_cast(static_cast(ros_message)) + member->offset_; - - if (!member->is_array_) { - switch (member->type_id_) { - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: - { - bool sb = false; - // don't cast to bool here because if the bool is - // uninitialized the random value can't be deserialized - if (*static_cast(field)) {sb = true;} - ser << sb; - } - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: - ser << *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: - { - auto && str = StringHelper::convert_to_std_string(field); - - // Control maximum length. - if (member->string_upper_bound_ && str.length() > member->string_upper_bound_ + 1) { - throw std::runtime_error("string overcomes the maximum length"); - } - ser << str; - } - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: - { - auto sub_members = (const MembersType *)member->members_->data; + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + if (!member->is_array_) { + // don't cast to bool here because if the bool is + // uninitialized the random value can't be deserialized + ser << (*static_cast(field) ? true : false); + } else { + serialize_field(member, field, ser); + } + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + auto sub_members = static_cast(member->members_->data); + if (!member->is_array_) { serializeROSmessage(ser, sub_members, field); - } - break; - default: - throw std::runtime_error("unknown type"); - } - } else { - switch (member->type_id_) { - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: - serialize_array(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: - { - auto sub_members = - static_cast(member->members_->data); + } else { void * subros_message = nullptr; size_t array_size = 0; size_t sub_members_size = sub_members->size_of_; @@ -434,10 +390,10 @@ bool TypeSupport::serializeROSmessage( subros_message = align_(max_align, subros_message); } } - break; - default: - throw std::runtime_error("unknown type"); - } + } + break; + default: + throw std::runtime_error("unknown type"); } } @@ -445,13 +401,15 @@ bool TypeSupport::serializeROSmessage( } template -void deserialize_array( +void deserialize_field( const rosidl_typesupport_introspection_cpp::MessageMember * member, void * field, eprosima::fastcdr::Cdr & deser, bool call_new) { - if (member->array_size_ && !member->is_upper_bound_) { + if (!member->is_array_) { + deser >> *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { deser.deserializeArray(static_cast(field), member->array_size_); } else { auto & vector = *reinterpret_cast *>(field); @@ -463,14 +421,16 @@ void deserialize_array( } template -void deserialize_array( +void deserialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, eprosima::fastcdr::Cdr & deser, bool call_new) { (void)call_new; - if (member->array_size_ && !member->is_upper_bound_) { + if (!member->is_array_) { + deser >> *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { deser.deserializeArray(static_cast(field), member->array_size_); } else { auto & data = *reinterpret_cast::type *>(field); @@ -482,38 +442,43 @@ void deserialize_array( } template<> -inline void deserialize_array( +inline void deserialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, eprosima::fastcdr::Cdr & deser, bool call_new) { (void)call_new; - if (member->array_size_ && !member->is_upper_bound_) { - auto deser_field = static_cast(field); - // tmpstring is defined here and not below to avoid - // memory allocation in every iteration of the for loop - std::string tmpstring; - for (size_t i = 0; i < member->array_size_; ++i) { - deser.deserialize(tmpstring); - if (!rosidl_generator_c__String__assign(&deser_field[i], tmpstring.c_str())) { - throw std::runtime_error("unable to assign rosidl_generator_c__String"); - } - } + if (!member->is_array_) { + using CStringHelper = StringHelper; + CStringHelper::assign(deser, field, call_new); } else { - std::vector cpp_string_vector; - deser >> cpp_string_vector; + if (member->array_size_ && !member->is_upper_bound_) { + auto deser_field = static_cast(field); + // tmpstring is defined here and not below to avoid + // memory allocation in every iteration of the for loop + std::string tmpstring; + for (size_t i = 0; i < member->array_size_; ++i) { + deser.deserialize(tmpstring); + if (!rosidl_generator_c__String__assign(&deser_field[i], tmpstring.c_str())) { + throw std::runtime_error("unable to assign rosidl_generator_c__String"); + } + } + } else { + std::vector cpp_string_vector; + deser >> cpp_string_vector; - auto & string_array_field = *reinterpret_cast(field); - if (!rosidl_generator_c__String__Array__init(&string_array_field, cpp_string_vector.size())) { - throw std::runtime_error("unable to initialize rosidl_generator_c__String array"); - } + auto & string_array_field = *reinterpret_cast(field); + if (!rosidl_generator_c__String__Array__init(&string_array_field, cpp_string_vector.size())) { + throw std::runtime_error("unable to initialize rosidl_generator_c__String array"); + } - for (size_t i = 0; i < cpp_string_vector.size(); ++i) { - if (!rosidl_generator_c__String__assign(&string_array_field.data[i], - cpp_string_vector[i].c_str())) - { - throw std::runtime_error("unable to assign rosidl_generator_c__String"); + for (size_t i = 0; i < cpp_string_vector.size(); ++i) { + if (!rosidl_generator_c__String__assign(&string_array_field.data[i], + cpp_string_vector[i].c_str())) + { + throw std::runtime_error("unable to assign rosidl_generator_c__String"); + } } } } @@ -571,101 +536,51 @@ bool TypeSupport::deserializeROSmessage( for (uint32_t i = 0; i < members->member_count_; ++i) { const auto * member = members->members_ + i; void * field = static_cast(ros_message) + member->offset_; - - if (!member->is_array_) { - switch (member->type_id_) { - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: - deser >> *static_cast(field); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: - { - StringHelper::assign(deser, field, call_new); - } - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: - { - auto sub_members = (const MembersType *)member->members_->data; + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + deserialize_field(member, field, deser, call_new); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + auto sub_members = (const MembersType *)member->members_->data; + if (!member->is_array_) { deserializeROSmessage(deser, sub_members, field, call_new); - } - break; - default: - throw std::runtime_error("unknown type"); - } - } else { - switch (member->type_id_) { - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: - deserialize_array(member, field, deser, call_new); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: - { - auto sub_members = (const MembersType *)member->members_->data; + } else { void * subros_message = nullptr; size_t array_size = 0; size_t sub_members_size = sub_members->size_of_; @@ -688,10 +603,10 @@ bool TypeSupport::deserializeROSmessage( subros_message = align_(max_align, subros_message); } } - break; - default: - throw std::runtime_error("unknown type"); - } + } + break; + default: + throw std::runtime_error("unknown type"); } }