Skip to content

Commit

Permalink
Temporary buffer remove (#207)
Browse files Browse the repository at this point in the history 
* Refs #3043. Estimate serialization size.

* Refs #3043. Max bounded size support.
  • Loading branch information
@MiguelCompany @dirk-thomas
MiguelCompany authored and dirk-thomas committed Jun 19, 2018
1 parent 825c6bc commit 3e653ec
Show file tree
Hide file tree
Showing 10 changed files with 287 additions and 19 deletions.
12 changes: 7 additions & 5 deletions rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,12 +38,14 @@ MessageTypeSupport<MembersType>::MessageTypeSupport(const MembersType * members)
members->message_name_ + "_";
this->setName(name.c_str());

// TODO(wjwwood): this could be more intelligent, setting m_typeSize to the
// maximum serialized size of the message, when the message is a bounded one.
if (members->member_count_ != 0) {
this->m_typeSize = static_cast<uint32_t>(this->calculateMaxSerializedSize(members, 0));
// Fully bound by default
this->max_size_bound_ = true;
// Encapsulation size
this->m_typeSize = 4;
if (this->members_->member_count_ != 0) {
this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(members, 4));
} else {
this->m_typeSize = 4;
this->m_typeSize++;
}
}

Expand Down
20 changes: 12 additions & 8 deletions rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,12 +42,14 @@ RequestTypeSupport<ServiceMembersType, MessageMembersType>::RequestTypeSupport(
members->service_name_ + "_Request_";
this->setName(name.c_str());

// TODO(wjwwood): this could be more intelligent, setting m_typeSize to the
// maximum serialized size of the message, when the message is a bounded one.
// Fully bound by default
this->max_size_bound_ = true;
// Encapsulation size
this->m_typeSize = 4;
if (this->members_->member_count_ != 0) {
this->m_typeSize = static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 0));
this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 4));
} else {
this->m_typeSize = 4;
this->m_typeSize++;
}
}

Expand All @@ -62,12 +64,14 @@ ResponseTypeSupport<ServiceMembersType, MessageMembersType>::ResponseTypeSupport
members->service_name_ + "_Response_";
this->setName(name.c_str());

// TODO(wjwwood): this could be more intelligent, setting m_typeSize to the
// maximum serialized size of the message, when the message is a bounded one.
// Fully bound by default
this->max_size_bound_ = true;
// Encapsulation size
this->m_typeSize = 4;
if (this->members_->member_count_ != 0) {
this->m_typeSize = static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 0));
this->m_typeSize += static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 4));
} else {
this->m_typeSize = 4;
this->m_typeSize++;
}
}

Expand Down
27 changes: 27 additions & 0 deletions rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,27 @@ struct StringHelper<rosidl_typesupport_introspection_c__MessageMembers>
{
using type = rosidl_generator_c__String;

static size_t next_field_align(void * data, size_t current_alignment)
{
auto c_string = static_cast<rosidl_generator_c__String *>(data);
if (!c_string) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_cpp",
"Failed to cast data as rosidl_generator_c__String")
return current_alignment;
}
if (!c_string->data) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_fastrtps_cpp",
"rosidl_generator_c_String had invalid data")
return current_alignment;
}

current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
current_alignment += 4;
return current_alignment + strlen(c_string->data) + 1;
}

static std::string convert_to_std_string(void * data)
{
auto c_string = static_cast<rosidl_generator_c__String *>(data);
Expand Down Expand Up @@ -111,6 +132,8 @@ template<typename MembersType>
class TypeSupport : public eprosima::fastrtps::TopicDataType
{
public:
size_t getEstimatedSerializedSize(const void * ros_message);

bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser);

bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message);
Expand All @@ -131,8 +154,12 @@ class TypeSupport : public eprosima::fastrtps::TopicDataType
size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment);

const MembersType * members_;
bool max_size_bound_;

private:
size_t getEstimatedSerializedSize(
const MembersType * members, const void * ros_message, size_t current_alignment);

bool serializeROSmessage(
eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message);

Expand Down
233 changes: 231 additions & 2 deletions rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,7 @@ template<typename MembersType>
TypeSupport<MembersType>::TypeSupport()
{
m_isGetKeyDefined = false;
max_size_bound_ = false;
}

template<typename MembersType>
Expand Down Expand Up @@ -400,6 +401,212 @@ bool TypeSupport<MembersType>::serializeROSmessage(
return true;
}

// C++ specialization
template<typename T>
size_t next_field_align(
const rosidl_typesupport_introspection_cpp::MessageMember * member,
void * field,
size_t current_alignment)
{
const size_t padding = 4;
size_t item_size = sizeof(T);
if (!member->is_array_) {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);
current_alignment += item_size;
} else if (member->array_size_ && !member->is_upper_bound_) {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);
current_alignment += item_size * member->array_size_;
} else {
std::vector<T> & data = *reinterpret_cast<std::vector<T> *>(field);
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
current_alignment += padding;
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);
current_alignment += item_size * data.size();
}
return current_alignment;
}

template<>
inline
size_t next_field_align<std::string>(
const rosidl_typesupport_introspection_cpp::MessageMember * member,
void * field,
size_t current_alignment)
{
const size_t padding = 4;
if (!member->is_array_) {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
current_alignment += padding;
auto str = *static_cast<std::string *>(field);
current_alignment += str.size() + 1;
} else if (member->array_size_ && !member->is_upper_bound_) {
auto str_arr = static_cast<std::string *>(field);
for (size_t index = 0; index < member->array_size_; ++index) {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
current_alignment += padding;
current_alignment += str_arr[index].size() + 1;
}
} else {
auto & data = *reinterpret_cast<std::vector<std::string> *>(field);
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
current_alignment += padding;
for (auto & it : data) {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
current_alignment += padding;
current_alignment += it.size() + 1;
}
}
return current_alignment;
}

// C specialization
template<typename T>
size_t next_field_align(
const rosidl_typesupport_introspection_c__MessageMember * member,
void * field,
size_t current_alignment)
{
const size_t padding = 4;
size_t item_size = sizeof(T);
if (!member->is_array_) {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);
current_alignment += item_size;
} else if (member->array_size_ && !member->is_upper_bound_) {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);
current_alignment += item_size * member->array_size_;
} else {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
current_alignment += padding;

auto & data = *reinterpret_cast<typename GenericCArray<T>::type *>(field);
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size);
current_alignment += item_size * data.size;
}
return current_alignment;
}

template<>
inline
size_t next_field_align<std::string>(
const rosidl_typesupport_introspection_c__MessageMember * member,
void * field,
size_t current_alignment)
{
const size_t padding = 4;
using CStringHelper = StringHelper<rosidl_typesupport_introspection_c__MessageMembers>;
if (!member->is_array_) {
current_alignment = CStringHelper::next_field_align(field, current_alignment);
} else {
if (member->array_size_ && !member->is_upper_bound_) {
auto string_field = static_cast<rosidl_generator_c__String *>(field);
for (size_t i = 0; i < member->array_size_; ++i) {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
current_alignment += padding;
current_alignment += strlen(string_field[i].data) + 1;
}
} else {
current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
current_alignment += padding;
auto & string_array_field = *reinterpret_cast<rosidl_generator_c__String__Array *>(field);
for (size_t i = 0; i < string_array_field.size; ++i) {
current_alignment = CStringHelper::next_field_align(
&(string_array_field.data[i]), current_alignment);
}
}
}
return current_alignment;
}

template<typename MembersType>
size_t TypeSupport<MembersType>::getEstimatedSerializedSize(
const MembersType * members, const void * ros_message, size_t current_alignment)
{
assert(members);
assert(ros_message);

size_t initial_alignment = current_alignment;

for (uint32_t i = 0; i < members->member_count_; ++i) {
const auto member = members->members_ + i;
void * field = const_cast<char *>(static_cast<const char *>(ros_message)) + member->offset_;
switch (member->type_id_) {
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL:
current_alignment = next_field_align<bool>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE:
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8:
current_alignment = next_field_align<uint8_t>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR:
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8:
current_alignment = next_field_align<char>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32:
current_alignment = next_field_align<float>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64:
current_alignment = next_field_align<double>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16:
current_alignment = next_field_align<int16_t>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16:
current_alignment = next_field_align<uint16_t>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32:
current_alignment = next_field_align<int32_t>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32:
current_alignment = next_field_align<uint32_t>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64:
current_alignment = next_field_align<int64_t>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64:
current_alignment = next_field_align<uint64_t>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
current_alignment = next_field_align<std::string>(member, field, current_alignment);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE:
{
auto sub_members = static_cast<const MembersType *>(member->members_->data);
if (!member->is_array_) {
current_alignment += getEstimatedSerializedSize(sub_members, field, current_alignment);
} else {
void * subros_message = nullptr;
size_t array_size = 0;
size_t sub_members_size = sub_members->size_of_;
size_t max_align = calculateMaxAlign(sub_members);

if (member->array_size_ && !member->is_upper_bound_) {
subros_message = field;
array_size = member->array_size_;
} else {
array_size = get_array_size_and_assign_field(
member, field, subros_message, sub_members_size, max_align);

// Length serialization
current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4);
}

for (size_t index = 0; index < array_size; ++index) {
current_alignment += getEstimatedSerializedSize(
sub_members, subros_message, current_alignment);
subros_message = static_cast<char *>(subros_message) + sub_members_size;
subros_message = align_(max_align, subros_message);
}
}
}
break;
default:
throw std::runtime_error("unknown type");
}
}

return current_alignment - initial_alignment;
}

template<typename T>
void deserialize_field(
const rosidl_typesupport_introspection_cpp::MessageMember * member,
Expand Down Expand Up @@ -647,9 +854,7 @@ size_t TypeSupport<MembersType>::calculateMaxSerializedSize(

size_t initial_alignment = current_alignment;

// Encapsulation
const size_t padding = 4;
current_alignment += padding;

for (uint32_t i = 0; i < members->member_count_; ++i) {
const auto * member = members->members_ + i;
Expand All @@ -659,6 +864,7 @@ size_t TypeSupport<MembersType>::calculateMaxSerializedSize(
array_size = member->array_size_;
// Whether it is a sequence.
if (0 == array_size || member->is_upper_bound_) {
this->max_size_bound_ = false;
current_alignment += padding +
eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
}
Expand Down Expand Up @@ -691,6 +897,7 @@ size_t TypeSupport<MembersType>::calculateMaxSerializedSize(
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
{
this->max_size_bound_ = false;
for (size_t index = 0; index < array_size; ++index) {
current_alignment += padding +
eprosima::fastcdr::Cdr::alignment(current_alignment, padding) +
Expand All @@ -714,6 +921,28 @@ size_t TypeSupport<MembersType>::calculateMaxSerializedSize(
return current_alignment - initial_alignment;
}

template<typename MembersType>
size_t TypeSupport<MembersType>::getEstimatedSerializedSize(
const void * ros_message)
{
if (max_size_bound_) {
return m_typeSize;
}

assert(ros_message);

// Encapsulation size
size_t ret_val = 4;

if (members_->member_count_ != 0) {
ret_val += TypeSupport::getEstimatedSerializedSize(members_, ros_message, ret_val);
} else {
ret_val += 1;
}

return ret_val;
}

template<typename MembersType>
bool TypeSupport<MembersType>::serializeROSmessage(
const void * ros_message, eprosima::fastcdr::Cdr & ser)
Expand Down
2 changes: 1 addition & 1 deletion rmw_fastrtps_cpp/src/rmw_publish.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message)
eprosima::fastcdr::Cdr ser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
eprosima::fastcdr::Cdr::DDS_CDR);

if (!_serialize_ros_message(ros_message, ser, info->type_support_,
if (!_serialize_ros_message(ros_message, buffer, ser, info->type_support_,
info->typesupport_identifier_))
{
RMW_SET_ERROR_MSG("cannot serialize data");
Expand Down
Loading

0 comments on commit 3e653ec

Please sign in to comment.