From 2167eef0359a9fc60df90c9ad1e6a478c6959843 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 14 Jan 2020 20:08:06 +0100 Subject: [PATCH 1/2] 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 32c257945..aee72fdc4 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 2f39c31c7..c1abd92e0 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -127,6 +127,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 01e597338..2216377b6 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 693390a34..1f42d1077 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -130,6 +130,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 83a488ba2..d7c11a56b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -126,6 +126,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 d397b4b9e..a9a3e3de3 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -129,6 +129,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 b239031a5..4ce6f3a3f 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_); From f26c1e44f8fcddfb7391d16ec82e819855f38bb8 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 28 Feb 2020 19:17:36 +0100 Subject: [PATCH 2/2] Fixing type support C/CPP mix on rmw_fastrtps_dynamic_cpp (#350) * Making TypeSupport interface methods const. Signed-off-by: Miguel Company * Created TypeSupport proxy class Signed-off-by: Miguel Company * Changes on rmw_publisher Signed-off-by: Miguel Company * Changes on rmw_subscription. Signed-off-by: Miguel Company * Changes on rmw_client Signed-off-by: Miguel Company * Changes on rmw_service Signed-off-by: Miguel Company * linters Signed-off-by: Miguel Company * Moving LockedObject template to its own header. Signed-off-by: Miguel Company * class TypeSupportProxy on its own source file. Signed-off-by: Miguel Company * Added TypeSupportRegistry Signed-off-by: Miguel Company * Added generic pointer to ros type support so we can reference the registry key. Signed-off-by: Miguel Company * Creator functions passed to type_support_registry. Signed-off-by: Miguel Company * Added base class for all rmw_fastrtps_dynamic_cpp type support classes. Signed-off-by: Miguel Company * using TypeSupportRegistry on services and messages Signed-off-by: Miguel Company * using TypeSupportRegistry on rmw_serialize Signed-off-by: Miguel Company * Removed unused functions from type_support_common Signed-off-by: Miguel Company * Fixing assert code. Signed-off-by: Miguel Company * Removing unused argument. Signed-off-by: Miguel Company * Cleanup instead of asserting on destructor. Signed-off-by: Miguel Company * Improving singleton behavior Signed-off-by: Miguel Company --- .../include/rmw_fastrtps_cpp/TypeSupport.hpp | 6 +- rmw_fastrtps_cpp/src/type_support_common.cpp | 6 +- rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 2 + .../MessageTypeSupport.hpp | 2 +- .../MessageTypeSupport_impl.hpp | 4 +- .../ServiceTypeSupport.hpp | 15 +- .../ServiceTypeSupport_impl.hpp | 11 +- .../rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 56 ++++++- .../TypeSupport_impl.hpp | 46 ++--- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 72 ++++++-- .../src/rmw_publisher.cpp | 35 +++- .../src/rmw_serialize.cpp | 12 +- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 75 +++++++-- .../src/rmw_subscription.cpp | 35 +++- .../src/type_support_common.cpp | 50 +----- .../src/type_support_common.hpp | 9 - .../src/type_support_proxy.cpp | 47 ++++++ .../src/type_support_registry.cpp | 158 ++++++++++++++++++ .../src/type_support_registry.hpp | 74 ++++++++ .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 6 +- .../rmw_fastrtps_shared_cpp/locked_object.hpp | 49 ++++++ .../rmw_fastrtps_shared_cpp/topic_cache.hpp | 31 +--- 22 files changed, 625 insertions(+), 176 deletions(-) create mode 100644 rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/type_support_registry.hpp create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/locked_object.hpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index a276d48df..1f5bb641a 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -35,13 +35,13 @@ namespace rmw_fastrtps_cpp class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport { public: - size_t getEstimatedSerializedSize(const void * ros_message, const void * impl); + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const override; bool serializeROSmessage( - const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl); + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const override; bool deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl); + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; protected: TypeSupport(); diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index 981dd518f..9cc671b29 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -47,7 +47,7 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) m_typeSize = 4 + data_size; } -size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) +size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const { if (max_size_bound_) { return m_typeSize; @@ -63,7 +63,7 @@ size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const v } bool TypeSupport::serializeROSmessage( - const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const { assert(ros_message); assert(impl); @@ -83,7 +83,7 @@ bool TypeSupport::serializeROSmessage( } bool TypeSupport::deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const { assert(ros_message); assert(impl); diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 99767ad84..a0141289d 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -84,6 +84,8 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_wait.cpp src/rmw_wait_set.cpp src/type_support_common.cpp + src/type_support_proxy.cpp + src/type_support_registry.cpp src/serialization_format.cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp index db9158643..b63518852 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp @@ -32,7 +32,7 @@ template class MessageTypeSupport : public TypeSupport { public: - explicit MessageTypeSupport(const MembersType * members); + MessageTypeSupport(const MembersType * members, const void * ros_type_support); }; } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp index b0a26cb13..980888915 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp @@ -32,7 +32,9 @@ namespace rmw_fastrtps_dynamic_cpp { template -MessageTypeSupport::MessageTypeSupport(const MembersType * members) +MessageTypeSupport::MessageTypeSupport( + const MembersType * members, const void * ros_type_support) +: TypeSupport(ros_type_support) { assert(members); this->members_ = members; diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp index 3b908f1e9..607bf86ef 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp @@ -25,25 +25,18 @@ namespace rmw_fastrtps_dynamic_cpp { -template -class ServiceTypeSupport : public TypeSupport -{ -protected: - ServiceTypeSupport(); -}; - template -class RequestTypeSupport : public ServiceTypeSupport +class RequestTypeSupport : public TypeSupport { public: - explicit RequestTypeSupport(const ServiceMembersType * members); + RequestTypeSupport(const ServiceMembersType * members, const void * ros_type_support); }; template -class ResponseTypeSupport : public ServiceTypeSupport +class ResponseTypeSupport : public TypeSupport { public: - explicit ResponseTypeSupport(const ServiceMembersType * members); + ResponseTypeSupport(const ServiceMembersType * members, const void * ros_type_support); }; } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp index 218cbb91e..61237fdb9 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp @@ -29,14 +29,10 @@ namespace rmw_fastrtps_dynamic_cpp { -template -ServiceTypeSupport::ServiceTypeSupport() -{ -} - template RequestTypeSupport::RequestTypeSupport( - const ServiceMembersType * members) + const ServiceMembersType * members, const void * ros_type_support) +: TypeSupport(ros_type_support) { assert(members); this->members_ = members->request_members_; @@ -65,7 +61,8 @@ RequestTypeSupport::RequestTypeSupport( template ResponseTypeSupport::ResponseTypeSupport( - const ServiceMembersType * members) + const ServiceMembersType * members, const void * ros_type_support) +: TypeSupport(ros_type_support) { assert(members); this->members_ = members->response_members_; 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 e0bd7400c..f16fff136 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 @@ -130,20 +130,52 @@ struct StringHelper } }; +class TypeSupportProxy : public rmw_fastrtps_shared_cpp::TypeSupport +{ +public: + explicit TypeSupportProxy(rmw_fastrtps_shared_cpp::TypeSupport * inner_type); + + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const override; + + bool serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const override; + + bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; +}; + +class BaseTypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport +{ +public: + const void * ros_type_support() const + { + return ros_type_support_; + } + +protected: + explicit BaseTypeSupport(const void * ros_type_support) + { + ros_type_support_ = ros_type_support; + } + +private: + const void * ros_type_support_; +}; + template -class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport +class TypeSupport : public BaseTypeSupport { public: - size_t getEstimatedSerializedSize(const void * ros_message, const void * impl); + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const override; bool serializeROSmessage( - const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl); + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const override; bool deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl); + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; protected: - TypeSupport(); + explicit TypeSupport(const void * ros_type_support); size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment); @@ -151,14 +183,20 @@ class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport private: size_t getEstimatedSerializedSize( - const MembersType * members, const void * ros_message, size_t current_alignment); + const MembersType * members, + const void * ros_message, + size_t current_alignment) const; bool serializeROSmessage( - eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message); + eprosima::fastcdr::Cdr & ser, + const MembersType * members, + const void * ros_message) const; bool deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, const MembersType * members, void * ros_message, - bool call_new); + eprosima::fastcdr::Cdr & deser, + const MembersType * members, + void * ros_message, + bool call_new) const; }; } // namespace rmw_fastrtps_dynamic_cpp 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 7d27aa578..20e4d1f2a 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 @@ -108,7 +108,8 @@ rosidl_generator_c__void__Sequence__fini(rosidl_generator_c__void__Sequence * se } template -TypeSupport::TypeSupport() +TypeSupport::TypeSupport(const void * ros_type_support) +: BaseTypeSupport(ros_type_support) { m_isGetKeyDefined = false; max_size_bound_ = false; @@ -372,7 +373,9 @@ size_t get_array_size_and_assign_field( template bool TypeSupport::serializeROSmessage( - eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message) + eprosima::fastcdr::Cdr & ser, + const MembersType * members, + const void * ros_message) const { assert(members); assert(ros_message); @@ -630,7 +633,9 @@ size_t next_field_align_string( template size_t TypeSupport::getEstimatedSerializedSize( - const MembersType * members, const void * ros_message, size_t current_alignment) + const MembersType * members, + const void * ros_message, + size_t current_alignment) const { assert(members); assert(ros_message); @@ -948,7 +953,10 @@ inline size_t get_submessage_array_deserialize( template bool TypeSupport::deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, const MembersType * members, void * ros_message, bool call_new) + eprosima::fastcdr::Cdr & deser, + const MembersType * members, + void * ros_message, + bool call_new) const { assert(members); assert(ros_message); @@ -1116,21 +1124,21 @@ size_t TypeSupport::calculateMaxSerializedSize( template size_t TypeSupport::getEstimatedSerializedSize( - const void * ros_message, const void * impl) + const void * ros_message, const void * impl) const { if (max_size_bound_) { return m_typeSize; } assert(ros_message); - assert(impl); + assert(members_); // Encapsulation size size_t ret_val = 4; - auto members = static_cast(impl); - if (members->member_count_ != 0) { - ret_val += TypeSupport::getEstimatedSerializedSize(members, ros_message, 0); + (void)impl; + if (members_->member_count_ != 0) { + ret_val += TypeSupport::getEstimatedSerializedSize(members_, ros_message, 0); } else { ret_val += 1; } @@ -1140,17 +1148,17 @@ size_t TypeSupport::getEstimatedSerializedSize( template bool TypeSupport::serializeROSmessage( - const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const { assert(ros_message); - assert(impl); + assert(members_); // Serialize encapsulation ser.serialize_encapsulation(); - auto members = static_cast(impl); - if (members->member_count_ != 0) { - TypeSupport::serializeROSmessage(ser, members, ros_message); + (void)impl; + if (members_->member_count_ != 0) { + TypeSupport::serializeROSmessage(ser, members_, ros_message); } else { ser << (uint8_t)0; } @@ -1160,17 +1168,17 @@ bool TypeSupport::serializeROSmessage( template bool TypeSupport::deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const { assert(ros_message); - assert(impl); + assert(members_); // Deserialize encapsulation. deser.read_encapsulation(); - auto members = static_cast(impl); - if (members->member_count_ != 0) { - TypeSupport::deserializeROSmessage(deser, members, ros_message, false); + (void)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 f2bf46dc2..ccbe346df 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -34,10 +34,13 @@ #include "client_service_common.hpp" #include "type_support_common.hpp" +#include "type_support_registry.hpp" +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; using TopicDataType = eprosima::fastrtps::TopicDataType; +using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; extern "C" { @@ -101,6 +104,25 @@ rmw_create_client( info->request_publisher_matched_count_ = 0; info->response_subscriber_matched_count_ = 0; + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto request_type_impl = type_registry.get_request_type_support(type_support); + if (!request_type_impl) { + delete info; + RMW_SET_ERROR_MSG("failed to allocate request type support"); + return nullptr; + } + + auto response_type_impl = type_registry.get_response_type_support(type_support); + if (!response_type_impl) { + type_registry.return_request_type_support(type_support); + delete info; + RMW_SET_ERROR_MSG("failed to allocate response type support"); + return nullptr; + } + + info->request_type_support_impl_ = request_type_impl; + info->response_type_support_impl_ = response_type_impl; + const void * untyped_request_members; const void * untyped_response_members; @@ -109,27 +131,30 @@ 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, - info->typesupport_identifier_); + 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, info->typesupport_identifier_); if (!Domain::getRegisteredType(participant, request_type_name.c_str(), reinterpret_cast(&info->request_type_support_))) { - info->request_type_support_ = _create_request_type_support(type_support->data, - info->typesupport_identifier_); + info->request_type_support_ = new (std::nothrow) TypeSupportProxy(request_type_impl); + if (!info->request_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate request TypeSupportProxy"); + goto fail; + } _register_type(participant, info->request_type_support_); } if (!Domain::getRegisteredType(participant, response_type_name.c_str(), reinterpret_cast(&info->response_type_support_))) { - info->response_type_support_ = _create_response_type_support(type_support->data, - info->typesupport_identifier_); + info->response_type_support_ = new (std::nothrow) TypeSupportProxy(response_type_impl); + if (!info->response_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate response TypeSupportProxy"); + goto fail; + } _register_type(participant, info->response_type_support_); } @@ -243,6 +268,8 @@ rmw_create_client( "leaking type support objects because node impl is null"); } + type_registry.return_request_type_support(type_support); + type_registry.return_response_type_support(type_support); delete info; info = nullptr; } @@ -261,6 +288,29 @@ rmw_create_client( rmw_ret_t rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) { + auto info = static_cast(client->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "client info pointer is null", return RMW_RET_ERROR); + + auto impl = static_cast(const_cast(info->request_type_support_impl_)); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + impl, "client's request type support is null", + return RMW_RET_ERROR); + + auto ros_type_support = static_cast( + impl->ros_type_support()); + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + type_registry.return_request_type_support(ros_type_support); + + impl = static_cast(const_cast(info->response_type_support_impl_)); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + impl, "client's response type support is null", + return RMW_RET_ERROR); + + ros_type_support = static_cast( + impl->ros_type_support()); + type_registry.return_response_type_support(ros_type_support); + return rmw_fastrtps_shared_cpp::__rmw_destroy_client( eprosima_fastrtps_identifier, node, client); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index d7c11a56b..41c15a744 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -28,10 +28,13 @@ #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" #include "type_support_common.hpp" +#include "type_support_registry.hpp" +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; using TopicDataType = eprosima::fastrtps::TopicDataType; +using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; extern "C" { @@ -125,16 +128,28 @@ rmw_create_publisher( RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); return nullptr; } + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_impl = type_registry.get_message_type_support(type_support); + if (!type_impl) { + delete info; + RMW_SET_ERROR_MSG("failed to allocate type support"); + return nullptr; + } + info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; + info->type_support_impl_ = type_impl; std::string type_name = _create_type_name( type_support->data, info->typesupport_identifier_); if (!Domain::getRegisteredType(participant, type_name.c_str(), reinterpret_cast(&info->type_support_))) { - info->type_support_ = _create_message_type_support(type_support->data, - info->typesupport_identifier_); + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); + goto fail; + } _register_type(participant, info->type_support_); } @@ -215,6 +230,8 @@ rmw_create_publisher( delete info; } + type_registry.return_message_type_support(type_support); + if (rmw_publisher) { rmw_publisher_free(rmw_publisher); } @@ -250,6 +267,18 @@ rmw_publisher_get_actual_qos( rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) { + auto info = static_cast(publisher->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "publisher info pointer is null", return RMW_RET_ERROR); + + auto impl = static_cast(info->type_support_impl_); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(impl, "publisher type support is null", return RMW_RET_ERROR); + + auto ros_type_support = static_cast( + impl->ros_type_support()); + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + type_registry.return_message_type_support(ros_type_support); + return rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( eprosima_fastrtps_identifier, node, publisher); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp index ec0cb8378..b1124edcf 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp @@ -19,6 +19,7 @@ #include "rmw/rmw.h" #include "./type_support_common.hpp" +#include "./type_support_registry.hpp" extern "C" { @@ -39,11 +40,13 @@ rmw_serialize( } } - auto tss = _create_message_type_support(ts->data, ts->typesupport_identifier); + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto tss = type_registry.get_message_type_support(ts); 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"); + type_registry.return_message_type_support(ts); return RMW_RET_ERROR; } } @@ -57,7 +60,7 @@ rmw_serialize( auto ret = tss->serializeROSmessage(ros_message, ser, ts->data); serialized_message->buffer_length = data_length; serialized_message->buffer_capacity = data_length; - delete tss; + type_registry.return_message_type_support(ts); return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } @@ -78,14 +81,15 @@ rmw_deserialize( } } - auto tss = _create_message_type_support(ts->data, ts->typesupport_identifier); + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto tss = type_registry.get_message_type_support(ts); eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); auto ret = tss->deserializeROSmessage(deser, ros_message, ts->data); - delete tss; + type_registry.return_message_type_support(ts); 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 8a04615e7..3fec1524b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -43,13 +43,15 @@ #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" -#include "type_support_common.hpp" #include "client_service_common.hpp" +#include "type_support_common.hpp" +#include "type_support_registry.hpp" +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; using TopicDataType = eprosima::fastrtps::TopicDataType; -using CustomParticipantInfo = CustomParticipantInfo; +using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; extern "C" { @@ -111,6 +113,25 @@ rmw_create_service( info->participant_ = participant; info->typesupport_identifier_ = type_support->typesupport_identifier; + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto request_type_impl = type_registry.get_request_type_support(type_support); + if (!request_type_impl) { + delete info; + RMW_SET_ERROR_MSG("failed to allocate request type support"); + return nullptr; + } + + auto response_type_impl = type_registry.get_response_type_support(type_support); + if (!response_type_impl) { + type_registry.return_request_type_support(type_support); + delete info; + RMW_SET_ERROR_MSG("failed to allocate response type support"); + return nullptr; + } + + info->request_type_support_impl_ = request_type_impl; + info->response_type_support_impl_ = response_type_impl; + const void * untyped_request_members; const void * untyped_response_members; @@ -119,27 +140,30 @@ 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, - info->typesupport_identifier_); + 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, info->typesupport_identifier_); if (!Domain::getRegisteredType(participant, request_type_name.c_str(), reinterpret_cast(&info->request_type_support_))) { - info->request_type_support_ = _create_request_type_support(type_support->data, - info->typesupport_identifier_); + info->request_type_support_ = new (std::nothrow) TypeSupportProxy(request_type_impl); + if (!info->request_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate request TypeSupportProxy"); + goto fail; + } _register_type(participant, info->request_type_support_); } if (!Domain::getRegisteredType(participant, response_type_name.c_str(), reinterpret_cast(&info->response_type_support_))) { - info->response_type_support_ = _create_response_type_support(type_support->data, - info->typesupport_identifier_); + info->response_type_support_ = new (std::nothrow) TypeSupportProxy(response_type_impl); + if (!info->response_type_support_) { + RMW_SET_ERROR_MSG("failed to allocate response TypeSupportProxy"); + goto fail; + } _register_type(participant, info->response_type_support_); } @@ -240,6 +264,8 @@ rmw_create_service( rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); } + type_registry.return_request_type_support(type_support); + type_registry.return_response_type_support(type_support); delete info; } @@ -255,6 +281,29 @@ rmw_create_service( rmw_ret_t rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) { + auto info = static_cast(service->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "service info pointer is null", return RMW_RET_ERROR); + + auto impl = static_cast(const_cast(info->request_type_support_impl_)); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + impl, "client's request type support is null", + return RMW_RET_ERROR); + + auto ros_type_support = static_cast( + impl->ros_type_support()); + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + type_registry.return_request_type_support(ros_type_support); + + impl = static_cast(const_cast(info->response_type_support_impl_)); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + impl, "client's response type support is null", + return RMW_RET_ERROR); + + ros_type_support = static_cast( + impl->ros_type_support()); + type_registry.return_response_type_support(ros_type_support); + return rmw_fastrtps_shared_cpp::__rmw_destroy_service( eprosima_fastrtps_identifier, node, service); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index a9a3e3de3..1191f3d03 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -32,10 +32,13 @@ #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" #include "type_support_common.hpp" +#include "type_support_registry.hpp" +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; using TopicDataType = eprosima::fastrtps::TopicDataType; +using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; extern "C" { @@ -128,16 +131,28 @@ rmw_create_subscription( RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); return nullptr; } + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_impl = type_registry.get_message_type_support(type_support); + if (!type_impl) { + delete info; + RMW_SET_ERROR_MSG("failed to allocate type support"); + return nullptr; + } + info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; + info->type_support_impl_ = type_impl; std::string type_name = _create_type_name( type_support->data, info->typesupport_identifier_); if (!Domain::getRegisteredType(participant, type_name.c_str(), reinterpret_cast(&info->type_support_))) { - info->type_support_ = _create_message_type_support(type_support->data, - info->typesupport_identifier_); + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); + goto fail; + } _register_type(participant, info->type_support_); } @@ -197,6 +212,8 @@ rmw_create_subscription( delete info; } + type_registry.return_message_type_support(type_support); + if (rmw_subscription) { rmw_subscription_free(rmw_subscription); } @@ -216,6 +233,18 @@ rmw_subscription_count_matched_publishers( rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "subscription info pointer is null", return RMW_RET_ERROR); + + auto impl = static_cast(info->type_support_impl_); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(impl, "publisher type support is null", return RMW_RET_ERROR); + + auto ros_type_support = static_cast( + impl->ros_type_support()); + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + type_registry.return_message_type_support(ros_type_support); + return rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( eprosima_fastrtps_identifier, node, subscription); } diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp index 3d546a067..fc5e3a23b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp @@ -14,6 +14,8 @@ #include "rmw/error_handling.h" +#include "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" + #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" @@ -33,54 +35,6 @@ using_introspection_cpp_typesupport(const char * typesupport_identifier) rosidl_typesupport_introspection_cpp::typesupport_identifier; } -rmw_fastrtps_shared_cpp::TypeSupport * -_create_message_type_support(const void * untyped_members, const char * typesupport_identifier) -{ - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto members = static_cast( - untyped_members); - return new MessageTypeSupport_c(members); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto members = static_cast( - untyped_members); - return new MessageTypeSupport_cpp(members); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; -} - -rmw_fastrtps_shared_cpp::TypeSupport * -_create_request_type_support(const void * untyped_members, const char * typesupport_identifier) -{ - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto members = static_cast( - untyped_members); - return new RequestTypeSupport_c(members); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto members = static_cast( - untyped_members); - return new RequestTypeSupport_cpp(members); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; -} - -rmw_fastrtps_shared_cpp::TypeSupport * -_create_response_type_support(const void * untyped_members, const char * typesupport_identifier) -{ - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto members = static_cast( - untyped_members); - return new ResponseTypeSupport_c(members); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto members = static_cast( - untyped_members); - return new ResponseTypeSupport_cpp(members); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; -} - void _register_type( eprosima::fastrtps::Participant * participant, diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp index bd9e22d1d..a396d68f3 100644 --- a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp @@ -112,15 +112,6 @@ _create_type_name( return ""; } -rmw_fastrtps_shared_cpp::TypeSupport * -_create_message_type_support(const void * untyped_members, const char * typesupport_identifier); - -rmw_fastrtps_shared_cpp::TypeSupport * -_create_request_type_support(const void * untyped_members, const char * typesupport_identifier); - -rmw_fastrtps_shared_cpp::TypeSupport * -_create_response_type_support(const void * untyped_members, const char * typesupport_identifier); - void _register_type( eprosima::fastrtps::Participant * participant, diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp new file mode 100644 index 000000000..cc9eead5d --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_proxy.cpp @@ -0,0 +1,47 @@ +// 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 "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +TypeSupportProxy::TypeSupportProxy(rmw_fastrtps_shared_cpp::TypeSupport * inner_type) +{ + setName(inner_type->getName()); + m_typeSize = inner_type->m_typeSize; +} + +size_t TypeSupportProxy::getEstimatedSerializedSize( + const void * ros_message, const void * impl) const +{ + auto type_impl = static_cast(impl); + return type_impl->getEstimatedSerializedSize(ros_message, impl); +} + +bool TypeSupportProxy::serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const +{ + auto type_impl = static_cast(impl); + return type_impl->serializeROSmessage(ros_message, ser, impl); +} + +bool TypeSupportProxy::deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const +{ + auto type_impl = static_cast(impl); + return type_impl->deserializeROSmessage(deser, ros_message, impl); +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp new file mode 100644 index 000000000..d7f635b84 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_registry.cpp @@ -0,0 +1,158 @@ +// Copyright 2020 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 "rcutils/logging_macros.h" + +#include "rmw/error_handling.h" + +#include "type_support_common.hpp" +#include "type_support_registry.hpp" + +template +type_support_ptr get_type_support( + const key_type & ros_type_support, map_type & map, creator fun) +{ + std::lock_guard guard(map.getMutex()); + RefCountedTypeSupport & item = map()[ros_type_support]; + if (0 == item.ref_count++) { + item.type_support = fun(); + if (!item.type_support) { + map().erase(ros_type_support); + return nullptr; + } + } + return item.type_support; +} + +template +void return_type_support( + const key_type & ros_type_support, map_type & map) +{ + std::lock_guard guard(map.getMutex()); + auto it = map().find(ros_type_support); + assert(it != map().end()); + if (0 == --it->second.ref_count) { + delete it->second.type_support; + map().erase(it); + } +} + +template +void cleanup(map_type & map, const char * msg) +{ + std::lock_guard guard(map.getMutex()); + if (!map().empty()) { + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "TypeSupportRegistry %s is not empty. Cleaning it up...", msg); + for (auto it : map() ) { + delete it.second.type_support; + } + map().clear(); + } +} + +TypeSupportRegistry::~TypeSupportRegistry() +{ + cleanup(message_types_, "message_types_"); + cleanup(request_types_, "request_types_"); + cleanup(response_types_, "response_types_"); +} + +TypeSupportRegistry & TypeSupportRegistry::get_instance() +{ + static TypeSupportRegistry type_registry_instance; + return type_registry_instance; +} + +type_support_ptr TypeSupportRegistry::get_message_type_support( + const rosidl_message_type_support_t * ros_type_support) +{ + auto creator_fun = [&ros_type_support]() -> type_support_ptr + { + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new MessageTypeSupport_c(members, ros_type_support); + } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new MessageTypeSupport_cpp(members, ros_type_support); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; + }; + + return get_type_support(ros_type_support, message_types_, creator_fun); +} + +type_support_ptr TypeSupportRegistry::get_request_type_support( + const rosidl_service_type_support_t * ros_type_support) +{ + auto creator_fun = [&ros_type_support]() -> type_support_ptr + { + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new RequestTypeSupport_c(members, ros_type_support); + } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new RequestTypeSupport_cpp(members, ros_type_support); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; + }; + + return get_type_support(ros_type_support, request_types_, creator_fun); +} + +type_support_ptr TypeSupportRegistry::get_response_type_support( + const rosidl_service_type_support_t * ros_type_support) +{ + auto creator_fun = [&ros_type_support]() -> type_support_ptr + { + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new ResponseTypeSupport_c(members, ros_type_support); + } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new ResponseTypeSupport_cpp(members, ros_type_support); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; + }; + + return get_type_support(ros_type_support, response_types_, creator_fun); +} + +void TypeSupportRegistry::return_message_type_support( + const rosidl_message_type_support_t * ros_type_support) +{ + return_type_support(ros_type_support, message_types_); +} + +void TypeSupportRegistry::return_request_type_support( + const rosidl_service_type_support_t * ros_type_support) +{ + return_type_support(ros_type_support, request_types_); +} + +void TypeSupportRegistry::return_response_type_support( + const rosidl_service_type_support_t * ros_type_support) +{ + return_type_support(ros_type_support, response_types_); +} diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_registry.hpp b/rmw_fastrtps_dynamic_cpp/src/type_support_registry.hpp new file mode 100644 index 000000000..a3c66520a --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_registry.hpp @@ -0,0 +1,74 @@ +// Copyright 2020 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 TYPE_SUPPORT_REGISTRY_HPP_ +#define TYPE_SUPPORT_REGISTRY_HPP_ + +#include + +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/locked_object.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +#include "type_support_common.hpp" + +using type_support_ptr = rmw_fastrtps_dynamic_cpp::BaseTypeSupport *; + +/** + * A data structure to use as value type for the type registry. + */ +struct RefCountedTypeSupport +{ + type_support_ptr type_support = nullptr; + uint32_t ref_count = 0; +}; + +using msg_map_t = std::unordered_map; +using srv_map_t = std::unordered_map; + +class TypeSupportRegistry +{ +private: + LockedObject message_types_; + LockedObject request_types_; + LockedObject response_types_; + + TypeSupportRegistry() = default; + +public: + ~TypeSupportRegistry(); + + static TypeSupportRegistry & get_instance(); + + type_support_ptr get_message_type_support( + const rosidl_message_type_support_t * ros_type_support); + + type_support_ptr get_request_type_support( + const rosidl_service_type_support_t * ros_type_support); + + type_support_ptr get_response_type_support( + const rosidl_service_type_support_t * ros_type_support); + + void return_message_type_support( + const rosidl_message_type_support_t * ros_type_support); + + void return_request_type_support( + const rosidl_service_type_support_t * ros_type_support); + + void return_response_type_support( + const rosidl_service_type_support_t * ros_type_support); +}; + +#endif // TYPE_SUPPORT_REGISTRY_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 c670be90c..4515ba465 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 @@ -41,13 +41,13 @@ struct SerializedData class TypeSupport : public eprosima::fastrtps::TopicDataType { public: - virtual size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) = 0; + virtual size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const = 0; virtual bool serializeROSmessage( - const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) = 0; + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const = 0; virtual bool deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) = 0; + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const = 0; RMW_FASTRTPS_SHARED_CPP_PUBLIC bool getKey( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/locked_object.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/locked_object.hpp new file mode 100644 index 000000000..b880d04ab --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/locked_object.hpp @@ -0,0 +1,49 @@ +// Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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 RMW_FASTRTPS_SHARED_CPP__LOCKED_OBJECT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__LOCKED_OBJECT_HPP_ + +#include + +#include "rcpputils/thread_safety_annotations.hpp" + +template +class LockedObject +{ +private: + mutable std::mutex mutex_; + T object_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + +public: + /** + * \return a reference to this object to lock. + */ + std::mutex & getMutex() const RCPPUTILS_TSA_RETURN_CAPABILITY(mutex_) + { + return mutex_; + } + + T & operator()() + { + return object_; + } + + const T & operator()() const + { + return object_; + } +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__LOCKED_OBJECT_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp index c5604ee93..2fba5816c 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp @@ -28,9 +28,11 @@ #include "fastrtps/participant/Participant.h" #include "fastrtps/rtps/common/Guid.h" #include "fastrtps/rtps/common/InstanceHandle.h" -#include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" +#include "locked_object.hpp" +#include "qos.hpp" + typedef eprosima::fastrtps::rtps::GUID_t GUID_t; /** @@ -214,31 +216,4 @@ inline std::ostream & operator<<( return ostream; } -template -class LockedObject -{ -private: - mutable std::mutex mutex_; - T object_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - -public: - /** - * @return a reference to this object to lock. - */ - std::mutex & getMutex() const RCPPUTILS_TSA_RETURN_CAPABILITY(mutex_) - { - return mutex_; - } - - T & operator()() - { - return object_; - } - - const T & operator()() const - { - return object_; - } -}; - #endif // RMW_FASTRTPS_SHARED_CPP__TOPIC_CACHE_HPP_