From 3e0aea7522b29259c117abe6fb3024cd34224534 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 14 Jan 2020 20:08:06 +0100 Subject: [PATCH] Passing down type support information (#342) * Add rmw implementation specific pointer. Signed-off-by: Miguel Company * Changes on rmw_serialize Signed-off-by: Miguel Company * Changes for publishers Signed-off-by: Miguel Company * Changes for subscriptions Signed-off-by: Miguel Company * Changes for services and clients. Signed-off-by: Miguel Company * Solving linters Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_cpp/TypeSupport.hpp | 8 ++-- rmw_fastrtps_cpp/src/rmw_client.cpp | 3 ++ rmw_fastrtps_cpp/src/rmw_publisher.cpp | 1 + rmw_fastrtps_cpp/src/rmw_serialize.cpp | 6 +-- rmw_fastrtps_cpp/src/rmw_service.cpp | 3 ++ rmw_fastrtps_cpp/src/rmw_subscription.cpp | 1 + .../src/ros_message_serialization.cpp | 36 ---------------- .../src/ros_message_serialization.hpp | 42 ------------------- rmw_fastrtps_cpp/src/type_support_common.cpp | 21 +++++++--- .../rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 8 ++-- .../TypeSupport_impl.hpp | 24 +++++++---- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 3 ++ .../src/rmw_publisher.cpp | 1 + .../src/rmw_serialize.cpp | 6 +-- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 3 ++ .../src/rmw_subscription.cpp | 1 + .../src/ros_message_serialization.cpp | 36 ---------------- .../src/ros_message_serialization.hpp | 42 ------------------- .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 9 ++-- .../custom_client_info.hpp | 3 ++ .../custom_publisher_info.hpp | 1 + .../custom_service_info.hpp | 3 ++ .../custom_subscriber_info.hpp | 1 + .../src/TypeSupport_impl.cpp | 7 ++-- rmw_fastrtps_shared_cpp/src/rmw_publish.cpp | 2 + rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 4 +- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 5 ++- rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 2 + 28 files changed, 90 insertions(+), 192 deletions(-) delete mode 100644 rmw_fastrtps_cpp/src/ros_message_serialization.cpp delete mode 100644 rmw_fastrtps_cpp/src/ros_message_serialization.hpp delete mode 100644 rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.cpp delete mode 100644 rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.hpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index f406bc650..a276d48df 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -35,11 +35,13 @@ namespace rmw_fastrtps_cpp class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport { public: - size_t getEstimatedSerializedSize(const void * ros_message); + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl); - bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser); + bool serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl); - bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message); + bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl); protected: TypeSupport(); diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 4bf8d8cf5..ec81fa301 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -106,6 +106,9 @@ rmw_create_client( response_members = static_cast( service_members->response_members_->data); + info->request_type_support_impl_ = request_members; + info->response_type_support_impl_ = response_members; + std::string request_type_name = _create_type_name(request_members); std::string response_type_name = _create_type_name(response_members); diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index f4b46a909..dba510c25 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -134,6 +134,7 @@ rmw_create_publisher( } info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support->data; auto callbacks = static_cast(type_support->data); std::string type_name = _create_type_name(callbacks); diff --git a/rmw_fastrtps_cpp/src/rmw_serialize.cpp b/rmw_fastrtps_cpp/src/rmw_serialize.cpp index dd954efcc..6714377df 100644 --- a/rmw_fastrtps_cpp/src/rmw_serialize.cpp +++ b/rmw_fastrtps_cpp/src/rmw_serialize.cpp @@ -41,7 +41,7 @@ rmw_serialize( auto callbacks = static_cast(ts->data); auto tss = new MessageTypeSupport_cpp(callbacks); - auto data_length = tss->getEstimatedSerializedSize(ros_message); + auto data_length = tss->getEstimatedSerializedSize(ros_message, callbacks); if (serialized_message->buffer_capacity < data_length) { if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) { RMW_SET_ERROR_MSG("unable to dynamically resize serialized message"); @@ -54,7 +54,7 @@ rmw_serialize( eprosima::fastcdr::Cdr ser( buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - auto ret = tss->serializeROSmessage(ros_message, ser); + auto ret = tss->serializeROSmessage(ros_message, ser, callbacks); serialized_message->buffer_length = data_length; serialized_message->buffer_capacity = data_length; delete tss; @@ -85,7 +85,7 @@ rmw_deserialize( eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - auto ret = tss->deserializeROSmessage(deser, ros_message); + auto ret = tss->deserializeROSmessage(deser, ros_message, callbacks); delete tss; return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 3f61bb95a..1226c2cef 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -116,6 +116,9 @@ rmw_create_service( response_members = static_cast( service_members->response_members_->data); + info->request_type_support_impl_ = request_members; + info->response_type_support_impl_ = response_members; + std::string request_type_name = _create_type_name(request_members); std::string response_type_name = _create_type_name(response_members); diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index fd0f0e1c6..17ec03807 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -131,6 +131,7 @@ rmw_create_subscription( } info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support->data; auto callbacks = static_cast(type_support->data); std::string type_name = _create_type_name(callbacks); diff --git a/rmw_fastrtps_cpp/src/ros_message_serialization.cpp b/rmw_fastrtps_cpp/src/ros_message_serialization.cpp deleted file mode 100644 index 9e1994806..000000000 --- a/rmw_fastrtps_cpp/src/ros_message_serialization.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "fastcdr/Cdr.h" -#include "fastcdr/FastBuffer.h" - -#include "./type_support_common.hpp" - -bool -_deserialize_ros_message( - eprosima::fastcdr::Cdr & deser, - void * ros_message, - void * untyped_typesupport, - const char * typesupport_identifier) -{ - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - return typed_typesupport->deserializeROSmessage(deser, ros_message); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - return typed_typesupport->deserializeROSmessage(deser, ros_message); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return false; -} diff --git a/rmw_fastrtps_cpp/src/ros_message_serialization.hpp b/rmw_fastrtps_cpp/src/ros_message_serialization.hpp deleted file mode 100644 index 73006fafc..000000000 --- a/rmw_fastrtps_cpp/src/ros_message_serialization.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ROS_MESSAGE_SERIALIZATION_HPP_ -#define ROS_MESSAGE_SERIALIZATION_HPP_ - -namespace eprosima -{ -namespace fastcdr -{ -class Cdr; -class FastBuffer; -} // namespace fastcdr -} // namespace eprosima - -bool -_serialize_ros_message( - const void * ros_message, - eprosima::fastcdr::FastBuffer & buffer, - eprosima::fastcdr::Cdr & ser, - void * untyped_typesupport, - const char * typesupport_identifier); - -bool -_deserialize_ros_message( - eprosima::fastcdr::Cdr & deser, - void * ros_message, - void * untyped_typesupport, - const char * typesupport_identifier); - -#endif // ROS_MESSAGE_SERIALIZATION_HPP_ diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index d7b7d85a8..981dd518f 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -47,28 +47,34 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) m_typeSize = 4 + data_size; } -size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message) +size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) { if (max_size_bound_) { return m_typeSize; } assert(ros_message); + assert(impl); + + auto callbacks = static_cast(impl); // Encapsulation size + message size - return 4 + members_->get_serialized_size(ros_message); + return 4 + callbacks->get_serialized_size(ros_message); } -bool TypeSupport::serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser) +bool TypeSupport::serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) { assert(ros_message); + assert(impl); // Serialize encapsulation ser.serialize_encapsulation(); // If type is not empty, serialize message if (has_data_) { - return members_->cdr_serialize(ros_message, ser); + auto callbacks = static_cast(impl); + return callbacks->cdr_serialize(ros_message, ser); } // Otherwise, add a dummy byte @@ -76,16 +82,19 @@ bool TypeSupport::serializeROSmessage(const void * ros_message, eprosima::fastcd return true; } -bool TypeSupport::deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message) +bool TypeSupport::deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) { assert(ros_message); + assert(impl); // Deserialize encapsulation. deser.read_encapsulation(); // If type is not empty, deserialize message if (has_data_) { - return members_->cdr_deserialize(deser, ros_message); + auto callbacks = static_cast(impl); + return callbacks->cdr_deserialize(deser, ros_message); } // Otherwise, consume dummy byte diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp index 0cdde224d..e0bd7400c 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp @@ -134,11 +134,13 @@ template class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport { public: - size_t getEstimatedSerializedSize(const void * ros_message); + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl); - bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser); + bool serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl); - bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message); + bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl); protected: TypeSupport(); diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp index 93a5f53c0..7d27aa578 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp @@ -1116,19 +1116,21 @@ size_t TypeSupport::calculateMaxSerializedSize( template size_t TypeSupport::getEstimatedSerializedSize( - const void * ros_message) + const void * ros_message, const void * impl) { if (max_size_bound_) { return m_typeSize; } assert(ros_message); + assert(impl); // Encapsulation size size_t ret_val = 4; - if (members_->member_count_ != 0) { - ret_val += TypeSupport::getEstimatedSerializedSize(members_, ros_message, 0); + auto members = static_cast(impl); + if (members->member_count_ != 0) { + ret_val += TypeSupport::getEstimatedSerializedSize(members, ros_message, 0); } else { ret_val += 1; } @@ -1138,15 +1140,17 @@ size_t TypeSupport::getEstimatedSerializedSize( template bool TypeSupport::serializeROSmessage( - const void * ros_message, eprosima::fastcdr::Cdr & ser) + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) { assert(ros_message); + assert(impl); // Serialize encapsulation ser.serialize_encapsulation(); - if (members_->member_count_ != 0) { - TypeSupport::serializeROSmessage(ser, members_, ros_message); + auto members = static_cast(impl); + if (members->member_count_ != 0) { + TypeSupport::serializeROSmessage(ser, members, ros_message); } else { ser << (uint8_t)0; } @@ -1156,15 +1160,17 @@ bool TypeSupport::serializeROSmessage( template bool TypeSupport::deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, void * ros_message) + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) { assert(ros_message); + assert(impl); // Deserialize encapsulation. deser.read_encapsulation(); - if (members_->member_count_ != 0) { - TypeSupport::deserializeROSmessage(deser, members_, ros_message, false); + auto members = static_cast(impl); + if (members->member_count_ != 0) { + TypeSupport::deserializeROSmessage(deser, members, ros_message, false); } else { uint8_t dump = 0; deser >> dump; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 93ce2761e..f2bf46dc2 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -109,6 +109,9 @@ rmw_create_client( untyped_response_members = get_response_ptr(type_support->data, info->typesupport_identifier_); + info->request_type_support_impl_ = untyped_request_members; + info->response_type_support_impl_ = untyped_response_members; + std::string request_type_name = _create_type_name(untyped_request_members, info->typesupport_identifier_); std::string response_type_name = _create_type_name(untyped_response_members, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index e58d0e25c..50093dc96 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -133,6 +133,7 @@ rmw_create_publisher( return nullptr; } info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support->data; std::string type_name = _create_type_name( type_support->data, info->typesupport_identifier_); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp index 5e67c73a9..ec0cb8378 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp @@ -40,7 +40,7 @@ rmw_serialize( } auto tss = _create_message_type_support(ts->data, ts->typesupport_identifier); - auto data_length = tss->getEstimatedSerializedSize(ros_message); + auto data_length = tss->getEstimatedSerializedSize(ros_message, ts->data); if (serialized_message->buffer_capacity < data_length) { if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) { RMW_SET_ERROR_MSG("unable to dynamically resize serialized message"); @@ -54,7 +54,7 @@ rmw_serialize( eprosima::fastcdr::Cdr ser( buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - auto ret = tss->serializeROSmessage(ros_message, ser); + auto ret = tss->serializeROSmessage(ros_message, ser, ts->data); serialized_message->buffer_length = data_length; serialized_message->buffer_capacity = data_length; delete tss; @@ -84,7 +84,7 @@ rmw_deserialize( eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - auto ret = tss->deserializeROSmessage(deser, ros_message); + auto ret = tss->deserializeROSmessage(deser, ros_message, ts->data); delete tss; return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index ad0c9076a..8a04615e7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -119,6 +119,9 @@ rmw_create_service( untyped_response_members = get_response_ptr(type_support->data, info->typesupport_identifier_); + info->request_type_support_impl_ = untyped_request_members; + info->response_type_support_impl_ = untyped_response_members; + std::string request_type_name = _create_type_name(untyped_request_members, info->typesupport_identifier_); std::string response_type_name = _create_type_name(untyped_response_members, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index adbce65e0..b07f98e99 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -135,6 +135,7 @@ rmw_create_subscription( return nullptr; } info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support->data; std::string type_name = _create_type_name( type_support->data, info->typesupport_identifier_); diff --git a/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.cpp b/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.cpp deleted file mode 100644 index 9e1994806..000000000 --- a/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "fastcdr/Cdr.h" -#include "fastcdr/FastBuffer.h" - -#include "./type_support_common.hpp" - -bool -_deserialize_ros_message( - eprosima::fastcdr::Cdr & deser, - void * ros_message, - void * untyped_typesupport, - const char * typesupport_identifier) -{ - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - return typed_typesupport->deserializeROSmessage(deser, ros_message); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - return typed_typesupport->deserializeROSmessage(deser, ros_message); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return false; -} diff --git a/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.hpp b/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.hpp deleted file mode 100644 index 73006fafc..000000000 --- a/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ROS_MESSAGE_SERIALIZATION_HPP_ -#define ROS_MESSAGE_SERIALIZATION_HPP_ - -namespace eprosima -{ -namespace fastcdr -{ -class Cdr; -class FastBuffer; -} // namespace fastcdr -} // namespace eprosima - -bool -_serialize_ros_message( - const void * ros_message, - eprosima::fastcdr::FastBuffer & buffer, - eprosima::fastcdr::Cdr & ser, - void * untyped_typesupport, - const char * typesupport_identifier); - -bool -_deserialize_ros_message( - eprosima::fastcdr::Cdr & deser, - void * ros_message, - void * untyped_typesupport, - const char * typesupport_identifier); - -#endif // ROS_MESSAGE_SERIALIZATION_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp index 85c50dc90..c670be90c 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -35,16 +35,19 @@ struct SerializedData { bool is_cdr_buffer; // Whether next field is a pointer to a Cdr or to a plain ros message void * data; + const void * impl; // RMW implementation specific data }; class TypeSupport : public eprosima::fastrtps::TopicDataType { public: - virtual size_t getEstimatedSerializedSize(const void * ros_message) = 0; + virtual size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) = 0; - virtual bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser) = 0; + virtual bool serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) = 0; - virtual bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message) = 0; + virtual bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) = 0; RMW_FASTRTPS_SHARED_CPP_PUBLIC bool getKey( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 133ad5acc..07318a095 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -42,7 +42,9 @@ class ClientPubListener; typedef struct CustomClientInfo { rmw_fastrtps_shared_cpp::TypeSupport * request_type_support_; + const void * request_type_support_impl_; rmw_fastrtps_shared_cpp::TypeSupport * response_type_support_; + const void * response_type_support_impl_; eprosima::fastrtps::Subscriber * response_subscriber_; eprosima::fastrtps::Publisher * request_publisher_; ClientListener * listener_; @@ -81,6 +83,7 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; data.data = response.buffer_.get(); + data.impl = nullptr; // not used when is_cdr_buffer is true if (sub->takeNextData(&data, &sinfo)) { if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { response.sample_identity_ = sinfo.related_sample_identity; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 1a5ed0e2b..2e9045089 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -39,6 +39,7 @@ typedef struct CustomPublisherInfo : public CustomEventInfo eprosima::fastrtps::Publisher * publisher_; PubListener * listener_; rmw_fastrtps_shared_cpp::TypeSupport * type_support_; + const void * type_support_impl_; rmw_gid_t publisher_gid; const char * typesupport_identifier_; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index c54e21cc8..34d37667a 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -38,7 +38,9 @@ class ServiceListener; typedef struct CustomServiceInfo { rmw_fastrtps_shared_cpp::TypeSupport * request_type_support_; + const void * request_type_support_impl_; rmw_fastrtps_shared_cpp::TypeSupport * response_type_support_; + const void * response_type_support_impl_; eprosima::fastrtps::Subscriber * request_subscriber_; eprosima::fastrtps::Publisher * response_publisher_; ServiceListener * listener_; @@ -78,6 +80,7 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; data.data = request.buffer_; + data.impl = nullptr; // not used when is_cdr_buffer is true if (sub->takeNextData(&data, &sinfo)) { if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { request.sample_identity_ = sinfo.sample_identity; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index d1e37b2a4..7d213f742 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -41,6 +41,7 @@ typedef struct CustomSubscriberInfo : public CustomEventInfo eprosima::fastrtps::Subscriber * subscriber_; SubListener * listener_; rmw_fastrtps_shared_cpp::TypeSupport * type_support_; + const void * type_support_impl_; const char * typesupport_identifier_; RMW_FASTRTPS_SHARED_CPP_PUBLIC diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp index 607129481..73b31b107 100644 --- a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -62,7 +62,7 @@ bool TypeSupport::serialize( payload->max_size); // Object that manages the raw buffer. eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); // Object that serializes the data. - if (this->serializeROSmessage(ser_data->data, ser)) { + if (this->serializeROSmessage(ser_data->data, ser, ser_data->impl)) { payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; payload->length = (uint32_t)ser.getSerializedDataLength(); @@ -97,7 +97,7 @@ bool TypeSupport::deserialize( fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - return deserializeROSmessage(deser, ser_data->data); + return deserializeROSmessage(deser, ser_data->data, ser_data->impl); } std::function TypeSupport::getSerializedSizeProvider(void * data) @@ -111,7 +111,8 @@ std::function TypeSupport::getSerializedSizeProvider(void * data) auto ser = static_cast(ser_data->data); return static_cast(ser->getSerializedDataLength()); } - return static_cast(this->getEstimatedSerializedSize(ser_data->data)); + return static_cast(this->getEstimatedSerializedSize(ser_data->data, + ser_data->impl)); }; return ser_size; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp index 443d1f4d9..c8c8b6444 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp @@ -48,6 +48,7 @@ __rmw_publish( rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = false; data.data = const_cast(ros_message); + data.impl = info->type_support_impl_; if (!info->publisher_->write(&data)) { RMW_SET_ERROR_MSG("cannot publish data"); return RMW_RET_ERROR; @@ -88,6 +89,7 @@ __rmw_publish_serialized_message( rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; data.data = &ser; + data.impl = nullptr; // not used when is_cdr_buffer is true if (!info->publisher_->write(&data)) { RMW_SET_ERROR_MSG("cannot publish data"); return RMW_RET_ERROR; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index 3425008de..778b0bdb4 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -55,6 +55,7 @@ __rmw_send_request( rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = false; data.data = const_cast(ros_request); + data.impl = info->request_type_support_impl_; if (info->request_publisher_->write(&data, wparams)) { returnedValue = RMW_RET_OK; *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | @@ -94,7 +95,8 @@ __rmw_take_request( if (request.buffer_ != nullptr) { eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - info->request_type_support_->deserializeROSmessage(deser, ros_request); + info->request_type_support_->deserializeROSmessage( + deser, ros_request, info->request_type_support_impl_); // Get header memcpy(request_header->writer_guid, &request.sample_identity_.writer_guid(), diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 9287a423c..758af8e32 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -58,7 +58,8 @@ __rmw_take_response( *response.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - info->response_type_support_->deserializeROSmessage(deser, ros_response); + info->response_type_support_->deserializeROSmessage( + deser, ros_response, info->response_type_support_impl_); request_header->sequence_number = ((int64_t)response.sample_identity_.sequence_number().high) << 32 | response.sample_identity_.sequence_number().low; @@ -101,7 +102,7 @@ __rmw_send_response( rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = false; data.data = const_cast(ros_response); - + data.impl = info->response_type_support_impl_; if (info->response_publisher_->write(&data, wparams)) { returnedValue = RMW_RET_OK; } else { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 0e533f86f..9258ca221 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -68,6 +68,7 @@ _take( rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = false; data.data = ros_message; + data.impl = info->type_support_impl_; if (info->subscriber_->takeNextData(&data, &sinfo)) { info->listener_->data_taken(info->subscriber_); @@ -173,6 +174,7 @@ _take_serialized_message( rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; data.data = &buffer; + data.impl = nullptr; // not used when is_cdr_buffer is true if (info->subscriber_->takeNextData(&data, &sinfo)) { info->listener_->data_taken(info->subscriber_);