From ddb21cba90a44a98193b4e0574b276bfe9c77826 Mon Sep 17 00:00:00 2001 From: jwang Date: Tue, 31 Oct 2017 14:09:04 -0700 Subject: [PATCH] Unify and simplify de/serializeROSmessage processing For most type_id, serialization approach is similar which can be handled in a common template function, both single and array field. However, for bool and string type, there are extra check and logic for single but array does not. Although it is weird, this commit keep the logic intact. Signed-off-by: jwang --- .../rmw_fastrtps_cpp/TypeSupport_impl.hpp | 429 +++++++----------- 1 file changed, 172 insertions(+), 257 deletions(-) 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"); } }