Skip to content

Commit

Permalink
all tests pass
Browse files Browse the repository at this point in the history 
Signed-off-by: Knese Karsten <karsten.knese@us.bosch.com>
  • Loading branch information
@Karsten1987
Karsten1987 committed May 23, 2020
1 parent 48cea97 commit f674759
Show file tree
Hide file tree
Showing 7 changed files with 346 additions and 337 deletions.
260 changes: 105 additions & 155 deletions rmw_iceoryx_cpp/src/internal/iceoryx_deserialize_typesupport_c.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,188 +27,107 @@
#include "rosidl_typesupport_introspection_c/field_types.h"
#include "rosidl_typesupport_introspection_c/message_introspection.h"

#include "./iceoryx_serialization_common.hpp"

namespace rmw_iceoryx_cpp
{
namespace details_c
{

const char * deserialize(
const char * serialized_msg,
const rosidl_typesupport_introspection_c__MessageMembers * members,
void * ros_message);

std::pair<const char *, uint32_t> load_array_size(const char * serialized_msg)
{
// This is 64 bit aligned
// REVIEW: Please discuss
const uint32_t array_check = *reinterpret_cast<const uint32_t *>(serialized_msg);
serialized_msg += sizeof(array_check);
const uint32_t array_size = *reinterpret_cast<const uint32_t *>(serialized_msg);
serialized_msg += sizeof(array_size);

if (array_check != 101) {
std::cerr << "deserialization failure: array size is " << array_size <<
" and check failed! (" << array_check <<
")" << std::endl;
}
return std::make_pair(serialized_msg, array_size);
}

inline std::pair<const char *, size_t> get_submessage_array_c(
const rosidl_typesupport_introspection_c__MessageMember * member,
template<
class T,
size_t SizeT = sizeof(T)
>
const char * deserialize_element(
const char * serialized_msg,
void * ros_message_field,
void * & subros_message,
size_t sub_members_size)
{
(void)member;
uint32_t array_elements = 0;
std::tie(serialized_msg, array_elements) = load_array_size(serialized_msg);

auto data_array =
const_cast<rosidl_runtime_c__char__Sequence *>(reinterpret_cast<const
rosidl_runtime_c__char__Sequence
*>(ros_message_field));

data_array->data = static_cast<signed char *>(calloc(array_elements, sub_members_size));
data_array->capacity = array_elements;
data_array->size = array_elements;

subros_message = reinterpret_cast<void *>(data_array->data);

return std::make_pair(serialized_msg, array_elements);
}

template<typename T>
const char * copy_payload_array_c(const char * serialized_msg, void * ros_message_field)
void * ros_message_field)
{
uint32_t size = sizeof(T);
uint32_t array_size = 0;
std::tie(serialized_msg, array_size) = load_array_size(serialized_msg);

uint32_t char_size = size * array_size;

auto data_array =
const_cast<rosidl_runtime_c__char__Sequence *>(reinterpret_cast<const
rosidl_runtime_c__char__Sequence
*>(ros_message_field));

data_array->data = static_cast<signed char *>(calloc(array_size, size));
data_array->capacity = array_size;
T * data = reinterpret_cast<T *>(ros_message_field);
memcpy(data, serialized_msg, SizeT);
serialized_msg += SizeT;

memcpy(data_array->data, serialized_msg, char_size);
serialized_msg += char_size;

data_array->capacity = array_size;
data_array->size = array_size;
return serialized_msg;
}

template<>
const char * copy_payload_array_c<std::string>(
const char * deserialize_element<rosidl_runtime_c__String, sizeof(rosidl_runtime_c__String)>(
const char * serialized_msg,
void * ros_message_field)
{
serialized_msg = copy_payload_array_c<char>(serialized_msg, ros_message_field);

auto data_array =
const_cast<rosidl_runtime_c__char__Sequence *>(reinterpret_cast<const
rosidl_runtime_c__char__Sequence
*>(ros_message_field));

// Set size of string to \0
auto array_size = strlen((const char *) const_cast<const signed char *>(data_array->data));
data_array->size = array_size;
uint32_t string_size = 0;
std::tie(serialized_msg, string_size) = pop_sequence_size(serialized_msg);

auto string = reinterpret_cast<rosidl_runtime_c__String *>(ros_message_field);
// valgrind reports a memory leak here
rosidl_runtime_c__String__assignn(string, serialized_msg, string_size);
serialized_msg += string_size;
return serialized_msg;
}

template<typename T>
const char * copy_payload_fixed_array_c(
const char * serialized_msg, void * ros_message_field,
const char * deserialize_array(
const char * serialized_msg,
void * ros_message_field,
uint32_t size)
{
T * ros_message_field_data = reinterpret_cast<std::array<T, 1> *>(ros_message_field)->data();
auto char_size = size * sizeof(T);
memcpy(ros_message_field_data, serialized_msg, char_size);
serialized_msg += char_size;

auto array = reinterpret_cast<T *>(ros_message_field);
for (size_t i = 0; i < size; ++i) {
auto data = reinterpret_cast<char *>(&array[i]);
serialized_msg = deserialize_element<T>(serialized_msg, data);
}
return serialized_msg;
}

template<typename T>
const char * copy_payload_c(
const rosidl_typesupport_introspection_c__MessageMember * member,
const char * serialized_msg, void * ros_message_field)
template<
class T,
size_t SizeT = sizeof(T)
>
const char * deserialize_sequence(const char * serialized_msg, void * ros_message_field)
{
if (!member->is_array_) {
uint32_t size = sizeof(T);
T * data = reinterpret_cast<T *>(ros_message_field);
memcpy(data, serialized_msg, size);
serialized_msg += size;
} else {
if (member->array_size_ > 0 && !member->is_upper_bound_) {
serialized_msg = copy_payload_fixed_array_c<T>(
serialized_msg, ros_message_field,
member->array_size_);
} else {
serialized_msg = copy_payload_array_c<T>(serialized_msg, ros_message_field);
}
uint32_t array_size = 0;
std::tie(serialized_msg, array_size) = pop_sequence_size(serialized_msg);

if (array_size > 0) {
auto sequence = reinterpret_cast<typename traits::sequence_type<T>::type *>(ros_message_field);
sequence->data = static_cast<T *>(calloc(array_size, SizeT));
sequence->size = array_size;
sequence->capacity = array_size;

return deserialize_array<T>(serialized_msg, sequence->data, array_size);
}
return serialized_msg;
}

template<>
const char * copy_payload_c<std::string>(
const rosidl_typesupport_introspection_c__MessageMember * member, const char * serialized_msg,
void * ros_message_field)
const char * deserialize_sequence<char, sizeof(char)>(const char * serialized_msg, void * ros_message_field)
{
if (!member->is_array_) {
serialized_msg = copy_payload_array_c<std::string>(serialized_msg, ros_message_field);
} else {
if (member->array_size_ > 0 && !member->is_upper_bound_) {
std::string * ros_message_field_data =
reinterpret_cast<std::array<std::string, 1> *>(ros_message_field)->data();
for (auto i = 0u; i < member->array_size_; ++i) {
serialized_msg = copy_payload_array_c<std::string>(serialized_msg, ros_message_field_data);
}
} else {
uint32_t array_size = 0;
std::tie(serialized_msg, array_size) = load_array_size(serialized_msg);
for (uint32_t i = 0; i < array_size; ++i) {
serialized_msg = copy_payload_array_c<std::string>(serialized_msg, ros_message_field);
}
}
uint32_t array_size = 0;
std::tie(serialized_msg, array_size) = pop_sequence_size(serialized_msg);

if (array_size > 0) {
auto sequence = reinterpret_cast<rosidl_runtime_c__char__Sequence *>(ros_message_field);
sequence->data = static_cast<signed char *>(calloc(array_size, sizeof(char)));
sequence->size = array_size;
sequence->capacity = array_size;

return deserialize_array<char>(serialized_msg, sequence->data, array_size);
}
return serialized_msg;
}

const char * copy_payload_c_ros_message(
const rosidl_typesupport_introspection_c__MessageMember * member, const char * serialized_msg,
void * ros_message_field)
template<typename T>
const char * deserialize_message_field(
const rosidl_typesupport_introspection_c__MessageMember * member,
const char * serialized_msg, void * ros_message_field)
{
auto sub_members =
(const rosidl_typesupport_introspection_c__MessageMembers *)member->members_->data;
debug_log("deserializing %s\n", member->name_);
if (!member->is_array_) {
serialized_msg = deserialize(serialized_msg, sub_members, ros_message_field);
return deserialize_element<T>(serialized_msg, ros_message_field);
} else if (member->array_size_ > 0 && !member->is_upper_bound_) {
return deserialize_array<T>(serialized_msg, ros_message_field, member->array_size_);
} else {
void * subros_message = nullptr;
size_t array_elememts = 0;
size_t sub_members_size = sub_members->size_of_;

if (member->array_size_ && !member->is_upper_bound_) {
subros_message = ros_message_field;
array_elememts = member->array_size_;
} else {
std::tie(serialized_msg, array_elememts) = get_submessage_array_c(
member, serialized_msg, ros_message_field, subros_message, sub_members_size);
}
for (size_t index = 0; index < array_elememts; ++index) {
serialized_msg = deserialize(serialized_msg, sub_members, subros_message);
subros_message = static_cast<char *>(subros_message) + sub_members_size;
}
return deserialize_sequence<T>(serialized_msg, ros_message_field);
}
return serialized_msg;
}

const char * deserialize(
Expand All @@ -221,45 +140,76 @@ const char * deserialize(
char * ros_message_field = static_cast<char *>(ros_message) + member->offset_;
switch (member->type_id_) {
case ::rosidl_typesupport_introspection_c__ROS_TYPE_BOOL:
serialized_msg = copy_payload_c<bool>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<bool>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_BYTE:
case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT8:
serialized_msg = copy_payload_c<uint8_t>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<uint8_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_CHAR:
case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT8:
serialized_msg = copy_payload_c<char>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<char>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT32:
serialized_msg = copy_payload_c<float>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<float>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT64:
serialized_msg = copy_payload_c<double>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<double>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT16:
serialized_msg = copy_payload_c<int16_t>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<int16_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT16:
serialized_msg = copy_payload_c<uint16_t>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<uint16_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT32:
serialized_msg = copy_payload_c<int32_t>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<int32_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT32:
serialized_msg = copy_payload_c<uint32_t>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<uint32_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT64:
serialized_msg = copy_payload_c<int64_t>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<int64_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT64:
serialized_msg = copy_payload_c<uint64_t>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<uint64_t>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_STRING:
serialized_msg = copy_payload_c<std::string>(member, serialized_msg, ros_message_field);
serialized_msg = deserialize_message_field<rosidl_runtime_c__String>(member, serialized_msg, ros_message_field);
break;
case ::rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE:
serialized_msg = copy_payload_c_ros_message(member, serialized_msg, ros_message_field);
{
auto sub_members =
(const rosidl_typesupport_introspection_c__MessageMembers *)member->members_->data;

void * subros_message = nullptr;
size_t sequence_size = 0;
size_t sub_members_size = sub_members->size_of_;
// It's a single message
if (!member->is_array_) {
subros_message = ros_message_field;
sequence_size = 1;
} else if (member->array_size_ > 0 && !member->is_upper_bound_) {
subros_message = ros_message_field;
sequence_size = member->array_size_;
} else {
debug_log("deserializing ROS message %s\n", member->name_);

std::tie(serialized_msg, sequence_size) = pop_sequence_size(serialized_msg);

auto sequence =
const_cast<rosidl_runtime_c__char__Sequence *>(reinterpret_cast<const
rosidl_runtime_c__char__Sequence
*>(ros_message_field));

subros_message = reinterpret_cast<void *>(sequence->data);
}

for (size_t index = 0; index < sequence_size; ++index) {
serialized_msg = deserialize(serialized_msg, sub_members, subros_message);
subros_message = static_cast<char *>(subros_message) + sub_members_size;
}
}
break;
default:
throw std::runtime_error("unknown type");
Expand Down
Loading

0 comments on commit f674759

Please sign in to comment.