diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.h b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.h index 16d549420..0dc77926e 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.h +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.h @@ -93,20 +93,14 @@ namespace rmw_fastrtps_cpp } }; - typedef struct Buffer - { - uint32_t length; - char *pointer; - } Buffer; - template class TypeSupport : public eprosima::fastrtps::TopicDataType { public: - bool serializeROSmessage(const void *ros_message, Buffer *data); + bool serializeROSmessage(const void *ros_message, eprosima::fastcdr::FastBuffer *data); - bool deserializeROSmessage(const Buffer* data, void *ros_message); + bool deserializeROSmessage(eprosima::fastcdr::FastBuffer *data, void *ros_message); bool serialize(void *data, SerializedPayload_t *payload); diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.h b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.h index 7e1bfdf5a..c1ad41f6d 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.h +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.h @@ -60,7 +60,7 @@ template void TypeSupport::deleteData(void* data) { assert(data); - free(data); + delete static_cast(data); } static inline void* @@ -832,33 +832,31 @@ size_t TypeSupport::calculateMaxSerializedSize( template bool TypeSupport::serializeROSmessage( - const void *ros_message, Buffer *buffer) + const void *ros_message, eprosima::fastcdr::FastBuffer *buffer) { assert(buffer); assert(ros_message); // eprosima::fastcdr::FastBuffer fastbuffer(buffer->pointer, m_typeSize); - eprosima::fastcdr::FastBuffer fastbuffer; - eprosima::fastcdr::Cdr ser(fastbuffer); + // eprosima::fastcdr::FastBuffer fastbuffer; + eprosima::fastcdr::Cdr ser(*buffer); if(members_->member_count_ != 0) TypeSupport::serializeROSmessage(ser, members_, ros_message); else ser << (uint8_t)0; - buffer->length = (uint32_t)ser.getSerializedDataLength(); return true; } template bool TypeSupport::deserializeROSmessage( - const Buffer* buffer, void *ros_message) + eprosima::fastcdr::FastBuffer* buffer, void *ros_message) { assert(buffer); assert(ros_message); - eprosima::fastcdr::FastBuffer fastbuffer(buffer->pointer, buffer->length); - eprosima::fastcdr::Cdr deser(fastbuffer); + eprosima::fastcdr::Cdr deser(*buffer); if(members_->member_count_ != 0) TypeSupport::deserializeROSmessage(deser, members_, ros_message, false); @@ -875,15 +873,7 @@ bool TypeSupport::deserializeROSmessage( template void* TypeSupport::createData() { - Buffer *buffer = static_cast(malloc(sizeof(Buffer) + m_typeSize)); - - if(buffer) - { - buffer->length = 0; - buffer->pointer = (char*)(buffer + 1); - } - - return buffer; + return new eprosima::fastcdr::FastBuffer(); } template @@ -893,10 +883,10 @@ bool TypeSupport::serialize( assert(data); assert(payload); - Buffer *buffer = static_cast(data); - payload->length = buffer->length; + eprosima::fastcdr::FastBuffer *buffer = static_cast(data); + payload->length = buffer->getBufferSize(); payload->encapsulation = CDR_LE; - memcpy(payload->data, buffer->pointer, buffer->length); + memcpy(payload->data, buffer->getBuffer(), buffer->getBufferSize()); return true; } @@ -906,9 +896,9 @@ bool TypeSupport::deserialize(SerializedPayload_t *payload, void *d assert(data); assert(payload); - Buffer *buffer = static_cast(data); - buffer->length = payload->length; - memcpy(buffer->pointer, payload->data, payload->length); + eprosima::fastcdr::FastBuffer *buffer = static_cast(data); + buffer->resize(payload->length); + memcpy(buffer->getBuffer(), payload->data, payload->length); return true; } diff --git a/rmw_fastrtps_cpp/src/functions.cpp b/rmw_fastrtps_cpp/src/functions.cpp index f05a56496..6b6996767 100644 --- a/rmw_fastrtps_cpp/src/functions.cpp +++ b/rmw_fastrtps_cpp/src/functions.cpp @@ -249,7 +249,7 @@ void _delete_typesupport(void * untyped_typesupport, const char* typesupport_identifier) { if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); + auto typed_typesupport = static_cast(untyped_typesupport); if (typed_typesupport != nullptr) delete typed_typesupport; } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { @@ -261,39 +261,39 @@ _delete_typesupport(void * untyped_typesupport, const char* typesupport_identifi } } -rmw_fastrtps_cpp::Buffer * -_create_data(void * untyped_typesupport, const char* typesupport_identifier) -{ - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - return static_cast(typed_typesupport->createData()); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - return static_cast(typed_typesupport->createData()); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; -} - -void -_delete_data( - rmw_fastrtps_cpp::Buffer * buffer, void * untyped_typesupport, - const char* typesupport_identifier) -{ - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - typed_typesupport->deleteData(buffer); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - typed_typesupport->deleteData(buffer); - } else { - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - } -} +// rmw_fastrtps_cpp::Buffer * +// _create_data(void * untyped_typesupport, const char* typesupport_identifier) +// { +// if (using_introspection_c_typesupport(typesupport_identifier)) { +// auto typed_typesupport = static_cast(untyped_typesupport); +// return static_cast(typed_typesupport->createData()); +// } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { +// auto typed_typesupport = static_cast(untyped_typesupport); +// return static_cast(typed_typesupport->createData()); +// } +// RMW_SET_ERROR_MSG("Unknown typesupport identifier"); +// return nullptr; +// } + +// void +// _delete_data( +// rmw_fastrtps_cpp::Buffer * buffer, void * untyped_typesupport, +// const char* typesupport_identifier) +// { +// if (using_introspection_c_typesupport(typesupport_identifier)) { +// auto typed_typesupport = static_cast(untyped_typesupport); +// typed_typesupport->deleteData(buffer); +// } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { +// auto typed_typesupport = static_cast(untyped_typesupport); +// typed_typesupport->deleteData(buffer); +// } else { +// RMW_SET_ERROR_MSG("Unknown typesupport identifier"); +// } +// } bool _serialize_ros_message( - const void *ros_message, rmw_fastrtps_cpp::Buffer * buffer, void * untyped_typesupport, + const void *ros_message, eprosima::fastcdr::FastBuffer * buffer, void * untyped_typesupport, const char* typesupport_identifier) { if (using_introspection_c_typesupport(typesupport_identifier)) { @@ -309,7 +309,7 @@ _serialize_ros_message( bool _deserialize_ros_message( - const rmw_fastrtps_cpp::Buffer * buffer, void *ros_message, void * untyped_typesupport, + eprosima::fastcdr::FastBuffer * buffer, void *ros_message, void * untyped_typesupport, const char* typesupport_identifier) { if (using_introspection_c_typesupport(typesupport_identifier)) { @@ -346,7 +346,7 @@ typedef struct CustomClientInfo typedef struct CustomClientResponse { eprosima::fastrtps::rtps::SampleIdentity sample_identity_; - rmw_fastrtps_cpp::Buffer *buffer_; + eprosima::fastcdr::FastBuffer *buffer_; CustomClientResponse() : buffer_(nullptr) {} } CustomClientResponse; @@ -364,7 +364,8 @@ class ClientListener : public SubscriberListener assert(sub); CustomClientResponse response; - response.buffer_ = _create_data(info_->response_type_support_, info_->typesupport_identifier_); + // response.buffer_ = _create_data(info_->response_type_support_, info_->typesupport_identifier_); + response.buffer_ = new eprosima::fastcdr::FastBuffer(); SampleInfo_t sinfo; if(sub->takeNextData(response.buffer_, &sinfo)) @@ -833,11 +834,12 @@ extern "C" CustomPublisherInfo *info = (CustomPublisherInfo*)publisher->data; assert(info); - rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->type_support_, info->typesupport_identifier_); + // rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->type_support_, info->typesupport_identifier_); + eprosima::fastcdr::FastBuffer buffer; - if(_serialize_ros_message(ros_message, buffer, info->type_support_, info->typesupport_identifier_)) + if(_serialize_ros_message(ros_message, &buffer, info->type_support_, info->typesupport_identifier_)) { - if(info->publisher_->write((void*)buffer)) + if(info->publisher_->write(&buffer)) returnedValue = RMW_RET_OK; else RMW_SET_ERROR_MSG("cannot publish data"); @@ -845,7 +847,7 @@ extern "C" else RMW_SET_ERROR_MSG("cannot serialize data"); - _delete_data(buffer, info->type_support_, info->typesupport_identifier_); + // _delete_data(buffer, info->type_support_, info->typesupport_identifier_); return returnedValue; } @@ -1085,21 +1087,22 @@ extern "C" CustomSubscriberInfo *info = (CustomSubscriberInfo*)subscription->data; assert(info); - rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->type_support_, info->typesupport_identifier_); + // rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->type_support_, info->typesupport_identifier_); + eprosima::fastcdr::FastBuffer buffer; SampleInfo_t sinfo; - if(info->subscriber_->takeNextData(buffer, &sinfo)) + if(info->subscriber_->takeNextData(&buffer, &sinfo)) { info->listener_->data_taken(); if(sinfo.sampleKind == ALIVE) { - _deserialize_ros_message(buffer, ros_message, info->type_support_, info->typesupport_identifier_); + _deserialize_ros_message(&buffer, ros_message, info->type_support_, info->typesupport_identifier_); *taken = true; } } - _delete_data(buffer, info->type_support_, info->typesupport_identifier_); + // _delete_data(buffer, info->type_support_, info->typesupport_identifier_); return RMW_RET_OK; } @@ -1130,16 +1133,17 @@ extern "C" CustomSubscriberInfo *info = (CustomSubscriberInfo*)subscription->data; assert(info); - rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->type_support_, info->typesupport_identifier_); + // rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->type_support_, info->typesupport_identifier_); + eprosima::fastcdr::FastBuffer buffer; SampleInfo_t sinfo; - if(info->subscriber_->takeNextData(buffer, &sinfo)) + if(info->subscriber_->takeNextData(&buffer, &sinfo)) { info->listener_->data_taken(); if(sinfo.sampleKind == ALIVE) { - _deserialize_ros_message(buffer, ros_message, info->type_support_, info->typesupport_identifier_); + _deserialize_ros_message(&buffer, ros_message, info->type_support_, info->typesupport_identifier_); rmw_gid_t * sender_gid = &message_info->publisher_gid; sender_gid->implementation_identifier = eprosima_fastrtps_identifier; memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); @@ -1148,7 +1152,7 @@ extern "C" } } - _delete_data(buffer, info->type_support_, info->typesupport_identifier_); + // _delete_data(buffer, info->type_support_, info->typesupport_identifier_); return RMW_RET_OK; } @@ -1335,7 +1339,7 @@ extern "C" typedef struct CustomServiceRequest { eprosima::fastrtps::rtps::SampleIdentity sample_identity_; - rmw_fastrtps_cpp::Buffer *buffer_; + eprosima::fastcdr::FastBuffer *buffer_; CustomServiceRequest() : buffer_(nullptr) {} } CustomServiceRequest; @@ -1353,7 +1357,8 @@ extern "C" assert(sub); CustomServiceRequest request; - request.buffer_ = _create_data(info_->request_type_support_, info_->typesupport_identifier_); + // request.buffer_ = _create_data(info_->request_type_support_, info_->typesupport_identifier_); + request.buffer_ = new eprosima::fastcdr::FastBuffer(); SampleInfo_t sinfo; if(sub->takeNextData(request.buffer_, &sinfo)) @@ -1605,13 +1610,14 @@ extern "C" CustomClientInfo *info = (CustomClientInfo*)client->data; assert(info); - rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->request_type_support_, info->typesupport_identifier_); + // rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->request_type_support_, info->typesupport_identifier_); + eprosima::fastcdr::FastBuffer buffer; - if(_serialize_ros_message(ros_request, buffer, info->request_type_support_, info->typesupport_identifier_)) + if(_serialize_ros_message(ros_request, &buffer, info->request_type_support_, info->typesupport_identifier_)) { eprosima::fastrtps::rtps::WriteParams wparams; - if(info->request_publisher_->write((void*)buffer, wparams)) + if(info->request_publisher_->write(&buffer, wparams)) { returnedValue = RMW_RET_OK; *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | wparams.sample_identity().sequence_number().low; @@ -1622,7 +1628,7 @@ extern "C" else RMW_SET_ERROR_MSG("cannot serialize data"); - _delete_data(buffer, info->request_type_support_, info->typesupport_identifier_); + // _delete_data(buffer, info->request_type_support_, info->typesupport_identifier_); return returnedValue; } @@ -1658,7 +1664,7 @@ extern "C" memcpy(request_header->writer_guid, &request.sample_identity_.writer_guid(), sizeof(eprosima::fastrtps::rtps::GUID_t)); request_header->sequence_number = ((int64_t)request.sample_identity_.sequence_number().high) << 32 | request.sample_identity_.sequence_number().low; - _delete_data(request.buffer_, info->request_type_support_, info->typesupport_identifier_); + // _delete_data(request.buffer_, info->request_type_support_, info->typesupport_identifier_); *taken = true; } @@ -1697,7 +1703,7 @@ extern "C" *taken = true; - _delete_data(response.buffer_, info->request_type_support_, info->typesupport_identifier_); + // _delete_data(response.buffer_, info->request_type_support_, info->typesupport_identifier_); } return RMW_RET_OK; @@ -1722,24 +1728,26 @@ extern "C" CustomServiceInfo *info = (CustomServiceInfo*)service->data; assert(info); - rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->response_type_support_, info->typesupport_identifier_); + // rmw_fastrtps_cpp::Buffer *buffer = _create_data(info->response_type_support_, info->typesupport_identifier_); + eprosima::fastcdr::FastBuffer buffer; - if(buffer != nullptr) + // if(buffer != nullptr) + if(true) { - _serialize_ros_message(ros_response, buffer, info->response_type_support_, info->typesupport_identifier_); + _serialize_ros_message(ros_response, &buffer, info->response_type_support_, info->typesupport_identifier_); eprosima::fastrtps::rtps::WriteParams wparams; memcpy(&wparams.related_sample_identity().writer_guid(), request_header->writer_guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); wparams.related_sample_identity().sequence_number().high = (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); wparams.related_sample_identity().sequence_number().low = (int32_t)(request_header->sequence_number & 0xFFFFFFFF); - if(info->response_publisher_->write((void*)buffer, wparams)) + if(info->response_publisher_->write(&buffer, wparams)) { returnedValue = RMW_RET_OK; } else RMW_SET_ERROR_MSG("cannot publish data"); - _delete_data(buffer, info->response_type_support_, info->typesupport_identifier_); + // _delete_data(buffer, info->response_type_support_, info->typesupport_identifier_); }