diff --git a/fastrtps_cmake_module/CMakeLists.txt b/fastrtps_cmake_module/CMakeLists.txt index 8d868ae12..c2a62dce0 100644 --- a/fastrtps_cmake_module/CMakeLists.txt +++ b/fastrtps_cmake_module/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# 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. diff --git a/fastrtps_cmake_module/cmake/Modules/FindFastRTPS.cmake b/fastrtps_cmake_module/cmake/Modules/FindFastRTPS.cmake index 7d442bd9c..4b9c8ab70 100644 --- a/fastrtps_cmake_module/cmake/Modules/FindFastRTPS.cmake +++ b/fastrtps_cmake_module/cmake/Modules/FindFastRTPS.cmake @@ -1,4 +1,4 @@ -# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# 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. diff --git a/fastrtps_cmake_module/fastrtps_cmake_module-extras.cmake b/fastrtps_cmake_module/fastrtps_cmake_module-extras.cmake index b0c1047b6..ccb2d82ce 100644 --- a/fastrtps_cmake_module/fastrtps_cmake_module-extras.cmake +++ b/fastrtps_cmake_module/fastrtps_cmake_module-extras.cmake @@ -1,4 +1,4 @@ -# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# 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. diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index f3349e1a9..b70d179d3 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# 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. @@ -28,6 +28,7 @@ endif() find_package(ament_cmake_ros REQUIRED) find_package(rcutils REQUIRED) +find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) @@ -36,14 +37,12 @@ find_package(FastRTPS REQUIRED MODULE) find_package(rmw REQUIRED) find_package(rosidl_generator_c REQUIRED) -find_package(rosidl_typesupport_introspection_c REQUIRED) -find_package(rosidl_typesupport_introspection_cpp REQUIRED) +find_package(rosidl_typesupport_fastrtps_c REQUIRED) +find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) include_directories(include) add_library(rmw_fastrtps_cpp - src/client_service_common.cpp - src/demangle.cpp src/get_client.cpp src/get_participant.cpp src/get_publisher.cpp @@ -76,7 +75,6 @@ add_library(rmw_fastrtps_cpp src/rmw_trigger_guard_condition.cpp src/rmw_wait.cpp src/rmw_wait_set.cpp - src/ros_message_serialization.cpp src/type_support_common.cpp ) target_link_libraries(rmw_fastrtps_cpp @@ -85,8 +83,9 @@ target_link_libraries(rmw_fastrtps_cpp # specific order: dependents before dependencies ament_target_dependencies(rmw_fastrtps_cpp "rcutils" - "rosidl_typesupport_introspection_c" - "rosidl_typesupport_introspection_cpp" + "rosidl_typesupport_fastrtps_c" + "rosidl_typesupport_fastrtps_cpp" + "rmw_fastrtps_shared_cpp" "rmw" "rosidl_generator_c" ) @@ -102,15 +101,16 @@ PRIVATE "RMW_FASTRTPS_CPP_BUILDING_LIBRARY") ament_export_include_directories(include) ament_export_libraries(rmw_fastrtps_cpp) -ament_export_dependencies(rosidl_typesupport_introspection_cpp) -ament_export_dependencies(rosidl_typesupport_introspection_c) +ament_export_dependencies(rosidl_typesupport_fastrtps_cpp) +ament_export_dependencies(rosidl_typesupport_fastrtps_c) ament_export_dependencies(rosidl_generator_c) ament_export_dependencies(rcutils) +ament_export_dependencies(rmw_fastrtps_shared_cpp) ament_export_dependencies(rmw) register_rmw_implementation( - "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" - "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") + "c:rosidl_typesupport_c:rosidl_typesupport_fastrtps_c" + "cpp:rosidl_typesupport_cpp:rosidl_typesupport_fastrtps_cpp") if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp index 284f808c4..5f431bc46 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -15,28 +15,19 @@ #ifndef RMW_FASTRTPS_CPP__MESSAGETYPESUPPORT_HPP_ #define RMW_FASTRTPS_CPP__MESSAGETYPESUPPORT_HPP_ -#include -#include - -#include -#include +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "TypeSupport.hpp" -#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" -#include "rosidl_typesupport_introspection_cpp/field_types.hpp" namespace rmw_fastrtps_cpp { -template -class MessageTypeSupport : public TypeSupport +class MessageTypeSupport : public TypeSupport { public: - explicit MessageTypeSupport(const MembersType * members); + explicit MessageTypeSupport(const message_type_support_callbacks_t * members); }; } // namespace rmw_fastrtps_cpp -#include "MessageTypeSupport_impl.hpp" - #endif // RMW_FASTRTPS_CPP__MESSAGETYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp index dd374ee8f..49ec62900 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -19,37 +19,32 @@ #include #include -#include "TypeSupport.hpp" -#include "rosidl_typesupport_introspection_cpp/field_types.hpp" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" -struct CustomServiceInfo; +#include "TypeSupport.hpp" namespace rmw_fastrtps_cpp { -template -class ServiceTypeSupport : public TypeSupport +class ServiceTypeSupport : public TypeSupport { protected: ServiceTypeSupport(); }; -template -class RequestTypeSupport : public ServiceTypeSupport +class RequestTypeSupport : public ServiceTypeSupport { public: - explicit RequestTypeSupport(const ServiceMembersType * members); + explicit RequestTypeSupport(const service_type_support_callbacks_t * members); }; -template -class ResponseTypeSupport : public ServiceTypeSupport +class ResponseTypeSupport : public ServiceTypeSupport { public: - explicit ResponseTypeSupport(const ServiceMembersType * members); + explicit ResponseTypeSupport(const service_type_support_callbacks_t * members); }; } // namespace rmw_fastrtps_cpp -#include "ServiceTypeSupport_impl.hpp" - #endif // RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp index 30ec97fe2..f406bc650 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -25,118 +25,14 @@ #include #include -#include "rcutils/logging_macros.h" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" -#include "rosidl_typesupport_introspection_cpp/field_types.hpp" -#include "rosidl_typesupport_introspection_cpp/identifier.hpp" -#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" -#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" -#include "rosidl_typesupport_introspection_cpp/visibility_control.h" - -#include "rosidl_typesupport_introspection_c/field_types.h" -#include "rosidl_typesupport_introspection_c/identifier.h" -#include "rosidl_typesupport_introspection_c/message_introspection.h" -#include "rosidl_typesupport_introspection_c/service_introspection.h" -#include "rosidl_typesupport_introspection_c/visibility_control.h" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" namespace rmw_fastrtps_cpp { -// Publishers write method will receive a pointer to this struct -struct SerializedData -{ - bool is_cdr_buffer; // Whether next field is a pointer to a Cdr or to a plain ros message - void * data; -}; - -// Helper class that uses template specialization to read/write string types to/from a -// eprosima::fastcdr::Cdr -template -struct StringHelper; - -// For C introspection typesupport we create intermediate instances of std::string so that -// eprosima::fastcdr::Cdr can handle the string properly. -template<> -struct StringHelper -{ - using type = rosidl_generator_c__String; - - static size_t next_field_align(void * data, size_t current_alignment) - { - auto c_string = static_cast(data); - if (!c_string) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "Failed to cast data as rosidl_generator_c__String") - return current_alignment; - } - if (!c_string->data) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "rosidl_generator_c_String had invalid data") - return current_alignment; - } - - current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, 4); - current_alignment += 4; - return current_alignment + strlen(c_string->data) + 1; - } - - static std::string convert_to_std_string(void * data) - { - auto c_string = static_cast(data); - if (!c_string) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "Failed to cast data as rosidl_generator_c__String") - return ""; - } - if (!c_string->data) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "rosidl_generator_c_String had invalid data") - return ""; - } - return std::string(c_string->data); - } - - static std::string convert_to_std_string(rosidl_generator_c__String & data) - { - return std::string(data.data); - } - - static void assign(eprosima::fastcdr::Cdr & deser, void * field, bool) - { - std::string str; - deser >> str; - rosidl_generator_c__String * c_str = static_cast(field); - rosidl_generator_c__String__assign(c_str, str.c_str()); - } -}; - -// For C++ introspection typesupport we just reuse the same std::string transparently. -template<> -struct StringHelper -{ - using type = std::string; - - static std::string & convert_to_std_string(void * data) - { - return *(static_cast(data)); - } - - static void assign(eprosima::fastcdr::Cdr & deser, void * field, bool call_new) - { - std::string & str = *(std::string *)field; - if (call_new) { - new(&str) std::string; - } - deser >> str; - } -}; - -template -class TypeSupport : public eprosima::fastrtps::TopicDataType +class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport { public: size_t getEstimatedSerializedSize(const void * ros_message); @@ -145,38 +41,16 @@ class TypeSupport : public eprosima::fastrtps::TopicDataType bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message); - bool serialize(void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload); - - bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t * payload, void * data); - - std::function getSerializedSizeProvider(void * data); - - void * createData(); - - void deleteData(void * data); - protected: TypeSupport(); - size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment); - - const MembersType * members_; - bool max_size_bound_; + void set_members(const message_type_support_callbacks_t * members); private: - size_t getEstimatedSerializedSize( - const MembersType * members, const void * ros_message, size_t current_alignment); - - bool serializeROSmessage( - eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message); - - bool deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, const MembersType * members, void * ros_message, - bool call_new); + const message_type_support_callbacks_t * members_; + bool has_data_; }; } // namespace rmw_fastrtps_cpp -#include "TypeSupport_impl.hpp" - #endif // RMW_FASTRTPS_CPP__TYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/identifier.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/identifier.hpp index 18ac6be98..aa1ac3947 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/identifier.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/identifier.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/package.xml b/rmw_fastrtps_cpp/package.xml index cda2c03d5..1852c629d 100644 --- a/rmw_fastrtps_cpp/package.xml +++ b/rmw_fastrtps_cpp/package.xml @@ -3,33 +3,43 @@ rmw_fastrtps_cpp 0.5.1 - Implement the ROS middleware interface using eProsima FastRTPS static code generation in C++. + Implement the ROS middleware interface using eProsima FastRTPS static code generation in C++. Dirk Thomas Apache License 2.0 Ricardo González ament_cmake_ros + rosidl_cmake fastrtps_cmake_module ament_cmake + rosidl_cmake fastcdr fastrtps fastrtps_cmake_module rcutils rmw + rmw_fastrtps_shared_cpp rosidl_generator_c - rosidl_typesupport_introspection_c - rosidl_typesupport_introspection_cpp + rosidl_generator_cpp + rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp fastcdr fastrtps fastrtps_cmake_module rcutils + rmw_fastrtps_shared_cpp rmw rosidl_generator_c - rosidl_typesupport_introspection_c - rosidl_typesupport_introspection_cpp + rosidl_generator_cpp + rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp + + rcutils + rmw + rmw_fastrtps_shared_cpp ament_lint_auto ament_lint_common diff --git a/rmw_fastrtps_cpp/src/get_client.cpp b/rmw_fastrtps_cpp/src/get_client.cpp index 175c9e301..b76cd9f02 100644 --- a/rmw_fastrtps_cpp/src/get_client.cpp +++ b/rmw_fastrtps_cpp/src/get_client.cpp @@ -14,7 +14,7 @@ #include "rmw_fastrtps_cpp/get_client.hpp" -#include "rmw_fastrtps_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/get_participant.cpp b/rmw_fastrtps_cpp/src/get_participant.cpp index f20d46932..c489f3d4c 100644 --- a/rmw_fastrtps_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_cpp/src/get_participant.cpp @@ -14,7 +14,7 @@ #include "rmw_fastrtps_cpp/get_participant.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/get_publisher.cpp b/rmw_fastrtps_cpp/src/get_publisher.cpp index 3a2cf0ea3..bac263c6c 100644 --- a/rmw_fastrtps_cpp/src/get_publisher.cpp +++ b/rmw_fastrtps_cpp/src/get_publisher.cpp @@ -14,7 +14,7 @@ #include "rmw_fastrtps_cpp/get_publisher.hpp" -#include "rmw_fastrtps_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/get_service.cpp b/rmw_fastrtps_cpp/src/get_service.cpp index 1af63baff..a7245a83b 100644 --- a/rmw_fastrtps_cpp/src/get_service.cpp +++ b/rmw_fastrtps_cpp/src/get_service.cpp @@ -14,7 +14,7 @@ #include "rmw_fastrtps_cpp/get_service.hpp" -#include "rmw_fastrtps_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/get_subscriber.cpp b/rmw_fastrtps_cpp/src/get_subscriber.cpp index 8bec35177..9eb7eae8b 100644 --- a/rmw_fastrtps_cpp/src/get_subscriber.cpp +++ b/rmw_fastrtps_cpp/src/get_subscriber.cpp @@ -14,7 +14,7 @@ #include "rmw_fastrtps_cpp/get_subscriber.hpp" -#include "rmw_fastrtps_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/identifier.cpp b/rmw_fastrtps_cpp/src/identifier.cpp index 97bd9ed84..e25ab9b5d 100644 --- a/rmw_fastrtps_cpp/src/identifier.cpp +++ b/rmw_fastrtps_cpp/src/identifier.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/src/namespace_prefix.cpp b/rmw_fastrtps_cpp/src/namespace_prefix.cpp index 569054daf..179c06c13 100644 --- a/rmw_fastrtps_cpp/src/namespace_prefix.cpp +++ b/rmw_fastrtps_cpp/src/namespace_prefix.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/src/namespace_prefix.hpp b/rmw_fastrtps_cpp/src/namespace_prefix.hpp index 191a35a89..a230f3cc9 100644 --- a/rmw_fastrtps_cpp/src/namespace_prefix.hpp +++ b/rmw_fastrtps_cpp/src/namespace_prefix.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/src/qos.cpp b/rmw_fastrtps_cpp/src/qos.cpp index fafd73bf9..53b2b6262 100644 --- a/rmw_fastrtps_cpp/src/qos.cpp +++ b/rmw_fastrtps_cpp/src/qos.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/src/qos.hpp b/rmw_fastrtps_cpp/src/qos.hpp index 9d78bcd01..c490929b6 100644 --- a/rmw_fastrtps_cpp/src/qos.hpp +++ b/rmw_fastrtps_cpp/src/qos.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index e7f111285..3d9493f06 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -19,17 +19,15 @@ #include "rmw/allocators.h" #include "rmw/rmw.h" -#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "rosidl_typesupport_introspection_c/identifier.h" - -#include "client_service_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "namespace_prefix.hpp" -#include "qos.hpp" -#include "type_support_common.hpp" -#include "rmw_fastrtps_cpp/custom_client_info.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" + +#include "./namespace_prefix.hpp" +#include "./qos.hpp" +#include "./type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; @@ -76,10 +74,10 @@ rmw_create_client( } const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { type_support = get_service_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); if (!type_support) { RMW_SET_ERROR_MSG("type support not from this implementation"); return nullptr; @@ -95,33 +93,31 @@ rmw_create_client( info->participant_ = participant; info->typesupport_identifier_ = type_support->typesupport_identifier; - const void * untyped_request_members; - const void * untyped_response_members; + const service_type_support_callbacks_t * service_members; + const message_type_support_callbacks_t * request_members; + const message_type_support_callbacks_t * response_members; - untyped_request_members = - get_request_ptr(type_support->data, info->typesupport_identifier_); - untyped_response_members = get_response_ptr(type_support->data, - info->typesupport_identifier_); + service_members = static_cast(type_support->data); + request_members = static_cast( + service_members->request_members_->data); + response_members = static_cast( + service_members->response_members_->data); - std::string request_type_name = _create_type_name(untyped_request_members, "srv", - info->typesupport_identifier_); - std::string response_type_name = _create_type_name(untyped_response_members, "srv", - info->typesupport_identifier_); + std::string request_type_name = _create_type_name(request_members, "srv"); + std::string response_type_name = _create_type_name(response_members, "srv"); 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_); - _register_type(participant, info->request_type_support_, info->typesupport_identifier_); + info->request_type_support_ = new RequestTypeSupport_cpp(service_members); + _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_); - _register_type(participant, info->response_type_support_, info->typesupport_identifier_); + info->response_type_support_ = new ResponseTypeSupport_cpp(service_members); + _register_type(participant, info->response_type_support_); } subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; @@ -219,11 +215,11 @@ rmw_create_client( if (impl) { if (info->request_type_support_ != nullptr) { - _unregister_type(participant, info->request_type_support_, info->typesupport_identifier_); + rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); } if (info->response_type_support_ != nullptr) { - _unregister_type(participant, info->response_type_support_, info->typesupport_identifier_); + rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); } } else { RCUTILS_LOG_ERROR_NAMED( @@ -249,43 +245,7 @@ rmw_create_client( rmw_ret_t rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) { - (void)node; - if (!client) { - RMW_SET_ERROR_MSG("client handle is null"); - return RMW_RET_ERROR; - } - if (client->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(client->data); - if (info != nullptr) { - if (info->response_subscriber_ != nullptr) { - Domain::removeSubscriber(info->response_subscriber_); - } - if (info->request_publisher_ != nullptr) { - Domain::removePublisher(info->request_publisher_); - } - if (info->listener_ != nullptr) { - delete info->listener_; - } - if (info->request_type_support_ != nullptr) { - _unregister_type(info->participant_, info->request_type_support_, - info->typesupport_identifier_); - } - if (info->response_type_support_ != nullptr) { - _unregister_type(info->participant_, info->response_type_support_, - info->typesupport_identifier_); - } - delete info; - } - if (client->service_name != nullptr) { - rmw_free(const_cast(client->service_name)); - client->service_name = nullptr; - } - rmw_client_free(client); - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_destroy_client( + eprosima_fastrtps_identifier, node, client); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp b/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp index 6815146b4..8cf195c6b 100644 --- a/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp +++ b/rmw_fastrtps_cpp/src/rmw_compare_gids_equal.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -18,6 +18,8 @@ #include "rmw/error_handling.h" #include "rmw/types.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" extern "C" @@ -25,34 +27,7 @@ extern "C" rmw_ret_t rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * result) { - if (!gid1) { - RMW_SET_ERROR_MSG("gid1 is null"); - return RMW_RET_ERROR; - } - - if (gid1->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("guid1 handle not from this implementation"); - return RMW_RET_ERROR; - } - - if (!gid2) { - RMW_SET_ERROR_MSG("gid2 is null"); - return RMW_RET_ERROR; - } - - if (gid2->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("gid1 handle not from this implementation"); - return RMW_RET_ERROR; - } - - if (!result) { - RMW_SET_ERROR_MSG("result is null"); - return RMW_RET_ERROR; - } - - *result = - memcmp(gid1->data, gid2->data, sizeof(eprosima::fastrtps::rtps::GUID_t)) == 0; - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_compare_gids_equal( + eprosima_fastrtps_identifier, gid1, gid2, result); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_count.cpp b/rmw_fastrtps_cpp/src/rmw_count.cpp index 0ed66c4e6..cf61bd1cc 100644 --- a/rmw_fastrtps_cpp/src/rmw_count.cpp +++ b/rmw_fastrtps_cpp/src/rmw_count.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -22,9 +22,9 @@ #include "rmw/rmw.h" #include "rmw/types.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" -#include "demangle.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" extern "C" { @@ -34,37 +34,8 @@ rmw_count_publishers( const char * topic_name, size_t * count) { - // safechecks - - if (!node) { - RMW_SET_ERROR_MSG("null node handle"); - return RMW_RET_ERROR; - } - // Get participant pointer from node - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto impl = static_cast(node->data); - - WriterInfo * slave_target = impl->secondaryPubListener; - slave_target->mapmutex.lock(); - *count = 0; - for (const auto & it : slave_target->topicNtypes) { - const auto topic_fqdn = _demangle_if_ros_topic(it.first); - if (topic_fqdn == topic_name) { - *count += it.second.size(); - } - } - slave_target->mapmutex.unlock(); - - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", - "looking for subscriber topic: %s, number of matches: %zu", - topic_name, *count) - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_count_publishers( + eprosima_fastrtps_identifier, node, topic_name, count); } rmw_ret_t @@ -73,36 +44,7 @@ rmw_count_subscribers( const char * topic_name, size_t * count) { - // safechecks - - if (!node) { - RMW_SET_ERROR_MSG("null node handle"); - return RMW_RET_ERROR; - } - // Get participant pointer from node - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - CustomParticipantInfo * impl = static_cast(node->data); - - ReaderInfo * slave_target = impl->secondarySubListener; - *count = 0; - slave_target->mapmutex.lock(); - for (const auto & it : slave_target->topicNtypes) { - const auto topic_fqdn = _demangle_if_ros_topic(it.first); - if (topic_fqdn == topic_name) { - *count += it.second.size(); - } - } - slave_target->mapmutex.unlock(); - - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", - "looking for subscriber topic: %s, number of matches: %zu", - topic_name, *count) - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_count_subscribers( + eprosima_fastrtps_identifier, node, topic_name, count); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_get_gid_for_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_get_gid_for_publisher.cpp index 7bf074082..4df396720 100644 --- a/rmw_fastrtps_cpp/src/rmw_get_gid_for_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_get_gid_for_publisher.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -16,7 +16,8 @@ #include "rmw/rmw.h" #include "rmw/types.h" -#include "rmw_fastrtps_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" extern "C" @@ -24,29 +25,7 @@ extern "C" rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) { - if (!publisher) { - RMW_SET_ERROR_MSG("publisher is null"); - return RMW_RET_ERROR; - } - - if (publisher->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - if (!gid) { - RMW_SET_ERROR_MSG("gid is null"); - return RMW_RET_ERROR; - } - - auto info = static_cast(publisher->data); - - if (!info) { - RMW_SET_ERROR_MSG("publisher info handle is null"); - return RMW_RET_ERROR; - } - - *gid = info->publisher_gid; - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_get_gid_for_publisher( + eprosima_fastrtps_identifier, publisher, gid); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_get_implementation_identifier.cpp b/rmw_fastrtps_cpp/src/rmw_get_implementation_identifier.cpp index c12e1354c..89e1caa48 100644 --- a/rmw_fastrtps_cpp/src/rmw_get_implementation_identifier.cpp +++ b/rmw_fastrtps_cpp/src/rmw_get_implementation_identifier.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/src/rmw_guard_condition.cpp b/rmw_fastrtps_cpp/src/rmw_guard_condition.cpp index be6a4a1a5..0344c15d3 100644 --- a/rmw_fastrtps_cpp/src/rmw_guard_condition.cpp +++ b/rmw_fastrtps_cpp/src/rmw_guard_condition.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -15,29 +15,23 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" -#include "types/guard_condition.hpp" extern "C" { rmw_guard_condition_t * rmw_create_guard_condition() { - rmw_guard_condition_t * guard_condition_handle = new rmw_guard_condition_t; - guard_condition_handle->implementation_identifier = eprosima_fastrtps_identifier; - guard_condition_handle->data = new GuardCondition(); - return guard_condition_handle; + return rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( + eprosima_fastrtps_identifier); } rmw_ret_t rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) { - if (guard_condition) { - delete static_cast(guard_condition->data); - delete guard_condition; - return RMW_RET_OK; - } - - return RMW_RET_ERROR; + return rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + guard_condition); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 3c50f5c2e..fe9b50da0 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/src/rmw_logging.cpp b/rmw_fastrtps_cpp/src/rmw_logging.cpp index 065818a22..6f793ffdc 100644 --- a/rmw_fastrtps_cpp/src/rmw_logging.cpp +++ b/rmw_fastrtps_cpp/src/rmw_logging.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -17,38 +17,13 @@ #include "rcutils/logging_macros.h" -#include "fastrtps/log/Log.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" extern "C" { -using eprosima::fastrtps::Log; - rmw_ret_t rmw_set_log_severity(rmw_log_severity_t severity) { - Log::Kind log_kind; - - switch (severity) { - case RMW_LOG_SEVERITY_DEBUG: -// fall through - case RMW_LOG_SEVERITY_INFO: - log_kind = Log::Kind::Info; - break; - case RMW_LOG_SEVERITY_WARN: - log_kind = Log::Kind::Warning; - break; - case RMW_LOG_SEVERITY_ERROR: -// fall through - case RMW_LOG_SEVERITY_FATAL: - log_kind = Log::Kind::Error; - break; - default: - RCUTILS_LOG_ERROR("Unknown logging severity type %d", severity); - return RMW_RET_ERROR; - } - - eprosima::fastrtps::Log::SetVerbosity(log_kind); - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_set_log_severity(severity); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index a4915afa0..022db9a9a 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -24,198 +24,12 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "fastrtps/config.h" -#include "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/attributes/ParticipantAttributes.h" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/publisher/PublisherListener.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SubscriberListener.h" -#include "fastrtps/subscriber/SampleInfo.h" -#include "fastrtps/attributes/SubscriberAttributes.h" - -#include "fastrtps/rtps/RTPSDomain.h" - -#include "fastrtps/rtps/reader/RTPSReader.h" -#include "fastrtps/rtps/reader/StatefulReader.h" -#include "fastrtps/rtps/reader/ReaderListener.h" -#include "fastrtps/rtps/builtin/discovery/endpoint/EDPSimple.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using ParticipantAttributes = eprosima::fastrtps::ParticipantAttributes; -using StatefulReader = eprosima::fastrtps::rtps::StatefulReader; extern "C" { -rmw_node_t * -create_node( - const char * name, - const char * namespace_, - ParticipantAttributes participantAttrs) -{ - if (!name) { - RMW_SET_ERROR_MSG("name is null"); - return nullptr; - } - - if (!namespace_) { - RMW_SET_ERROR_MSG("namespace_ is null"); - return nullptr; - } - - // Declare everything before beginning to create things. - ::ParticipantListener * listener = nullptr; - Participant * participant = nullptr; - rmw_guard_condition_t * graph_guard_condition = nullptr; - CustomParticipantInfo * node_impl = nullptr; - rmw_node_t * node_handle = nullptr; - ReaderInfo * tnat_1 = nullptr; - WriterInfo * tnat_2 = nullptr; - std::pair edp_readers; - - try { - listener = new ::ParticipantListener(); - } catch (std::bad_alloc &) { - RMW_SET_ERROR_MSG("failed to allocate participant listener"); - goto fail; - } - - participant = Domain::createParticipant(participantAttrs, listener); - if (!participant) { - RMW_SET_ERROR_MSG("create_node() could not create participant"); - return nullptr; - } - - graph_guard_condition = rmw_create_guard_condition(); - if (!graph_guard_condition) { - // error already set - goto fail; - } - - try { - node_impl = new CustomParticipantInfo(); - } catch (std::bad_alloc &) { - RMW_SET_ERROR_MSG("failed to allocate node impl struct"); - goto fail; - } - - node_handle = rmw_node_allocate(); - if (!node_handle) { - RMW_SET_ERROR_MSG("failed to allocate rmw_node_t"); - goto fail; - } - node_handle->implementation_identifier = eprosima_fastrtps_identifier; - node_impl->participant = participant; - node_impl->listener = listener; - node_impl->graph_guard_condition = graph_guard_condition; - node_handle->data = node_impl; - - node_handle->name = - static_cast(rmw_allocate(sizeof(char) * strlen(name) + 1)); - if (!node_handle->name) { - RMW_SET_ERROR_MSG("failed to allocate memory"); - node_handle->namespace_ = nullptr; // to avoid free on uninitialized memory - goto fail; - } - memcpy(const_cast(node_handle->name), name, strlen(name) + 1); - - node_handle->namespace_ = - static_cast(rmw_allocate(sizeof(char) * strlen(namespace_) + 1)); - if (!node_handle->namespace_) { - RMW_SET_ERROR_MSG("failed to allocate memory"); - goto fail; - } - memcpy(const_cast(node_handle->namespace_), namespace_, strlen(namespace_) + 1); - - tnat_1 = new ReaderInfo(participant, graph_guard_condition); - tnat_2 = new WriterInfo(participant, graph_guard_condition); - - node_impl->secondarySubListener = tnat_1; - node_impl->secondaryPubListener = tnat_2; - - edp_readers = participant->getEDPReaders(); - if (!edp_readers.first) { - RMW_SET_ERROR_MSG("edp_readers.first is null"); - goto fail; - } - - if (!edp_readers.second) { - RMW_SET_ERROR_MSG("edp_readers.second is null"); - goto fail; - } - - if (!(edp_readers.first->setListener(tnat_1) & edp_readers.second->setListener(tnat_2))) { - RMW_SET_ERROR_MSG("Failed to attach ROS related logic to the Participant"); - goto fail; - } - - return node_handle; -fail: - delete tnat_2; - delete tnat_1; - if (node_handle) { - rmw_free(const_cast(node_handle->namespace_)); - node_handle->namespace_ = nullptr; - rmw_free(const_cast(node_handle->name)); - node_handle->name = nullptr; - } - rmw_node_free(node_handle); - delete node_impl; - if (graph_guard_condition) { - rmw_ret_t ret = rmw_destroy_guard_condition(graph_guard_condition); - if (ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "failed to destroy guard condition during error handling") - } - } - rmw_free(listener); - if (participant) { - Domain::removeParticipant(participant); - } - return nullptr; -} - -bool -get_security_file_paths( - std::array & security_files_paths, const char * node_secure_root) -{ - // here assume only 6 files for security - const char * file_names[6] = { - "ca.cert.pem", "cert.pem", "key.pem", - "ca.cert.pem", "governance.p7s", "permissions.p7s" - }; - size_t num_files = sizeof(file_names) / sizeof(char *); - - std::string file_prefix("file://"); - - for (size_t i = 0; i < num_files; i++) { - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - char * file_path = rcutils_join_path(node_secure_root, file_names[i], allocator); - - if (!file_path) { - return false; - } - - if (rcutils_is_readable(file_path)) { - security_files_paths[i] = file_prefix + std::string(file_path); - } else { - allocator.deallocate(file_path, allocator.state); - return false; - } - - allocator.deallocate(file_path, allocator.state); - } - - return true; -} - rmw_node_t * rmw_create_node( const char * name, @@ -223,147 +37,21 @@ rmw_create_node( size_t domain_id, const rmw_node_security_options_t * security_options) { - if (!name) { - RMW_SET_ERROR_MSG("name is null"); - return nullptr; - } - if (!security_options) { - RMW_SET_ERROR_MSG("security_options is null"); - return nullptr; - } - - ParticipantAttributes participantAttrs; - - // Load default XML profile. - Domain::getDefaultParticipantAttributes(participantAttrs); - - participantAttrs.rtps.builtin.domainId = static_cast(domain_id); - // since the participant name is not part of the DDS spec - participantAttrs.rtps.setName(name); - // the node name is also set in the user_data - size_t name_length = strlen(name); - const char prefix[6] = "name="; - participantAttrs.rtps.userData.resize(name_length + sizeof(prefix)); - memcpy(participantAttrs.rtps.userData.data(), prefix, sizeof(prefix) - 1); - for (size_t i = 0; i < name_length; ++i) { - participantAttrs.rtps.userData[sizeof(prefix) - 1 + i] = name[i]; - } - participantAttrs.rtps.userData[sizeof(prefix) - 1 + name_length] = ';'; - - if (security_options->security_root_path) { - // if security_root_path provided, try to find the key and certificate files -#if HAVE_SECURITY - std::array security_files_paths; - - if (get_security_file_paths(security_files_paths, security_options->security_root_path)) { - eprosima::fastrtps::rtps::PropertyPolicy property_policy; - using Property = eprosima::fastrtps::rtps::Property; - property_policy.properties().emplace_back( - Property("dds.sec.auth.plugin", "builtin.PKI-DH")); - property_policy.properties().emplace_back( - Property("dds.sec.auth.builtin.PKI-DH.identity_ca", - security_files_paths[0])); - property_policy.properties().emplace_back( - Property("dds.sec.auth.builtin.PKI-DH.identity_certificate", - security_files_paths[1])); - property_policy.properties().emplace_back( - Property("dds.sec.auth.builtin.PKI-DH.private_key", - security_files_paths[2])); - property_policy.properties().emplace_back( - Property("dds.sec.crypto.plugin", "builtin.AES-GCM-GMAC")); - - property_policy.properties().emplace_back(Property( - "dds.sec.access.plugin", "builtin.Access-Permissions")); - property_policy.properties().emplace_back(Property( - "dds.sec.access.builtin.Access-Permissions.permissions_ca", security_files_paths[3])); - property_policy.properties().emplace_back(Property( - "dds.sec.access.builtin.Access-Permissions.governance", security_files_paths[4])); - property_policy.properties().emplace_back(Property( - "dds.sec.access.builtin.Access-Permissions.permissions", security_files_paths[5])); - - participantAttrs.rtps.properties = property_policy; - } else if (security_options->enforce_security) { - RMW_SET_ERROR_MSG("couldn't find all security files!"); - return nullptr; - } -#else - RMW_SET_ERROR_MSG( - "This Fast-RTPS version doesn't have the security libraries\n" - "Please compile Fast-RTPS using the -DSECURITY=ON CMake option"); - return nullptr; -#endif - } - return create_node(name, namespace_, participantAttrs); + return rmw_fastrtps_shared_cpp::__rmw_create_node( + eprosima_fastrtps_identifier, name, namespace_, domain_id, security_options); } rmw_ret_t rmw_destroy_node(rmw_node_t * node) { - rmw_ret_t result_ret = RMW_RET_OK; - if (!node) { - RMW_SET_ERROR_MSG("node handle is null"); - return RMW_RET_ERROR; - } - - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return RMW_RET_ERROR; - } - - Participant * participant = impl->participant; - - // Begin deleting things in the same order they were created in rmw_create_node(). - std::pair edp_readers = participant->getEDPReaders(); - if (!edp_readers.first || !edp_readers.second) { - RMW_SET_ERROR_MSG("failed to get EDPReader listener"); - result_ret = RMW_RET_ERROR; - } - - if (edp_readers.first && !edp_readers.first->setListener(nullptr)) { - RMW_SET_ERROR_MSG("failed to unset EDPReader listener"); - result_ret = RMW_RET_ERROR; - } - delete impl->secondarySubListener; - if (edp_readers.second && !edp_readers.second->setListener(nullptr)) { - RMW_SET_ERROR_MSG("failed to unset EDPReader listener"); - result_ret = RMW_RET_ERROR; - } - delete impl->secondaryPubListener; - - rmw_free(const_cast(node->name)); - node->name = nullptr; - rmw_free(const_cast(node->namespace_)); - node->namespace_ = nullptr; - rmw_node_free(node); - - if (RMW_RET_OK != rmw_destroy_guard_condition(impl->graph_guard_condition)) { - RMW_SET_ERROR_MSG("failed to destroy graph guard condition"); - result_ret = RMW_RET_ERROR; - } - - Domain::removeParticipant(participant); - - delete impl->listener; - impl->listener = nullptr; - delete impl; - - return result_ret; + return rmw_fastrtps_shared_cpp::__rmw_destroy_node( + eprosima_fastrtps_identifier, node); } const rmw_guard_condition_t * rmw_node_get_graph_guard_condition(const rmw_node_t * node) { - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - return impl->graph_guard_condition; + return rmw_fastrtps_shared_cpp::__rmw_node_get_graph_guard_condition( + node); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_cpp/src/rmw_node_names.cpp index 6cc2d614e..9db14f9e5 100644 --- a/rmw_fastrtps_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node_names.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -24,12 +24,9 @@ #include "rmw/rmw.h" #include "rmw/sanity_checks.h" -#include "fastrtps/Domain.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" - -using Participant = eprosima::fastrtps::Participant; extern "C" { @@ -38,47 +35,7 @@ rmw_get_node_names( const rmw_node_t * node, rcutils_string_array_t * node_names) { - if (!node) { - RMW_SET_ERROR_MSG("null node handle"); - return RMW_RET_ERROR; - } - if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) { - return RMW_RET_ERROR; - } - - // Get participant pointer from node - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto impl = static_cast(node->data); - auto participant_names = impl->listener->get_discovered_names(); - - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rcutils_ret_t rcutils_ret = - rcutils_string_array_init(node_names, participant_names.size() + 1, &allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string_safe()) - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - for (size_t i = 0; i < participant_names.size() + 1; ++i) { - if (0 == i) { - node_names->data[i] = rcutils_strdup(node->name, allocator); - } else { - node_names->data[i] = rcutils_strdup(participant_names[i - 1].c_str(), allocator); - } - if (!node_names->data[i]) { - RMW_SET_ERROR_MSG("failed to allocate memory for node name") - rcutils_ret = rcutils_string_array_fini(node_names); - if (rcutils_ret != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "failed to cleanup during error handling: %s", rcutils_get_error_string_safe()) - } - return RMW_RET_BAD_ALLOC; - } - } - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_get_node_names( + eprosima_fastrtps_identifier, node, node_names); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_publish.cpp b/rmw_fastrtps_cpp/src/rmw_publish.cpp index 9990a395b..26e8cbc9e 100644 --- a/rmw_fastrtps_cpp/src/rmw_publish.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publish.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -19,81 +19,24 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_cpp/custom_publisher_info.hpp" -#include "rmw_fastrtps_cpp/identifier.hpp" -#include "rmw_fastrtps_cpp/macros.hpp" -#include "rmw_fastrtps_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "./ros_message_serialization.hpp" +#include "rmw_fastrtps_cpp/identifier.hpp" extern "C" { rmw_ret_t rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) { - auto error_allocator = rcutils_get_default_allocator(); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - publisher, "publisher pointer is null", return RMW_RET_ERROR, error_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - ros_message, "ros_message pointer is null", return RMW_RET_ERROR, error_allocator); - - if (publisher->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(publisher->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - info, "publisher info pointer is null", return RMW_RET_ERROR, error_allocator); - - rmw_fastrtps_cpp::SerializedData data; - data.is_cdr_buffer = false; - data.data = const_cast(ros_message); - if (!info->publisher_->write(&data)) { - RMW_SET_ERROR_MSG("cannot publish data"); - return RMW_RET_ERROR; - } - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, publisher, ros_message); } rmw_ret_t rmw_publish_serialized_message( const rmw_publisher_t * publisher, const rmw_serialized_message_t * serialized_message) { - auto error_allocator = rcutils_get_default_allocator(); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - publisher, "publisher pointer is null", return RMW_RET_ERROR, error_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - serialized_message, "serialized_message pointer is null", - return RMW_RET_ERROR, error_allocator); - - if (publisher->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(publisher->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - info, "publisher info pointer is null", return RMW_RET_ERROR, error_allocator); - - eprosima::fastcdr::FastBuffer buffer( - serialized_message->buffer, serialized_message->buffer_length); - eprosima::fastcdr::Cdr ser( - buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - if (!ser.jump(serialized_message->buffer_length)) { - RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); - return RMW_RET_ERROR; - } - - rmw_fastrtps_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = &ser; - if (!info->publisher_->write(&data)) { - RMW_SET_ERROR_MSG("cannot publish data"); - return RMW_RET_ERROR; - } - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_publish_serialized_message( + eprosima_fastrtps_identifier, publisher, serialized_message); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 490569ec2..1d101719d 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -18,12 +18,15 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" -#include "namespace_prefix.hpp" -#include "qos.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_cpp/custom_publisher_info.hpp" -#include "type_support_common.hpp" + +#include "./namespace_prefix.hpp" +#include "./qos.hpp" +#include "./type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; @@ -70,10 +73,10 @@ rmw_create_publisher( } const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); if (!type_support) { RMW_SET_ERROR_MSG("type support not from this implementation"); return nullptr; @@ -92,14 +95,13 @@ rmw_create_publisher( info = new CustomPublisherInfo(); info->typesupport_identifier_ = type_support->typesupport_identifier; - std::string type_name = _create_type_name( - type_support->data, "msg", info->typesupport_identifier_); + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks, "msg"); 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_); - _register_type(participant, info->type_support_, info->typesupport_identifier_); + info->type_support_ = new MessageTypeSupport_cpp(callbacks); + _register_type(participant, info->type_support_); } publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; @@ -166,7 +168,9 @@ rmw_create_publisher( fail: if (info) { - _delete_typesupport(info->type_support_, info->typesupport_identifier_); + if (info->type_support_ != nullptr) { + delete info->type_support_; + } delete info; } @@ -180,47 +184,7 @@ rmw_create_publisher( rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) { - if (!node) { - RMW_SET_ERROR_MSG("node handle is null"); - return RMW_RET_ERROR; - } - - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - if (!publisher) { - RMW_SET_ERROR_MSG("publisher handle is null"); - return RMW_RET_ERROR; - } - - if (publisher->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(publisher->data); - if (info != nullptr) { - if (info->publisher_ != nullptr) { - Domain::removePublisher(info->publisher_); - } - if (info->type_support_ != nullptr) { - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return RMW_RET_ERROR; - } - - Participant * participant = impl->participant; - _unregister_type(participant, info->type_support_, info->typesupport_identifier_); - } - delete info; - } - rmw_free(const_cast(publisher->topic_name)); - publisher->topic_name = nullptr; - rmw_publisher_free(publisher); - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + eprosima_fastrtps_identifier, node, publisher); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_request.cpp b/rmw_fastrtps_cpp/src/rmw_request.cpp index faef0cf62..44a6f44b7 100644 --- a/rmw_fastrtps_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_cpp/src/rmw_request.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,22 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include "fastcdr/Cdr.h" -#include "fastcdr/FastBuffer.h" - -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "rmw/types.h" -#include "rmw_fastrtps_cpp/custom_client_info.hpp" -#include "rmw_fastrtps_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" -#include "rmw_fastrtps_cpp/TypeSupport.hpp" -#include "ros_message_serialization.hpp" extern "C" { @@ -37,33 +28,8 @@ rmw_send_request( const void * ros_request, int64_t * sequence_id) { - assert(client); - assert(ros_request); - assert(sequence_id); - - rmw_ret_t returnedValue = RMW_RET_ERROR; - - if (client->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(client->data); - assert(info); - - eprosima::fastrtps::rtps::WriteParams wparams; - rmw_fastrtps_cpp::SerializedData data; - data.is_cdr_buffer = false; - data.data = const_cast(ros_request); - if (info->request_publisher_->write(&data, wparams)) { - returnedValue = RMW_RET_OK; - *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | - wparams.sample_identity().sequence_number().low; - } else { - RMW_SET_ERROR_MSG("cannot publish data"); - } - - return returnedValue; + return rmw_fastrtps_shared_cpp::__rmw_send_request( + eprosima_fastrtps_identifier, client, ros_request, sequence_id); } rmw_ret_t @@ -73,40 +39,7 @@ rmw_take_request( void * ros_request, bool * taken) { - assert(service); - assert(request_header); - assert(ros_request); - assert(taken); - - *taken = false; - - if (service->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("service handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(service->data); - assert(info); - - CustomServiceRequest request = info->listener_->getRequest(); - - if (request.buffer_ != nullptr) { - eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - _deserialize_ros_message(deser, ros_request, info->request_type_support_, - info->typesupport_identifier_); - - // Get header - memcpy(request_header->writer_guid, &request.sample_identity_.writer_guid(), - sizeof(eprosima::fastrtps::rtps::GUID_t)); - request_header->sequence_number = ((int64_t)request.sample_identity_.sequence_number().high) << - 32 | request.sample_identity_.sequence_number().low; - - delete request.buffer_; - - *taken = true; - } - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_take_request( + eprosima_fastrtps_identifier, service, request_header, ros_request, taken); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_response.cpp b/rmw_fastrtps_cpp/src/rmw_response.cpp index c84f0c401..47c283abc 100644 --- a/rmw_fastrtps_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_cpp/src/rmw_response.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,20 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include "fastcdr/Cdr.h" - -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_cpp/custom_client_info.hpp" -#include "rmw_fastrtps_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" -#include "rmw_fastrtps_cpp/TypeSupport.hpp" -#include "ros_message_serialization.hpp" extern "C" { @@ -36,38 +28,8 @@ rmw_take_response( void * ros_response, bool * taken) { - assert(client); - assert(request_header); - assert(ros_response); - assert(taken); - - *taken = false; - - if (client->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("service handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(client->data); - assert(info); - - CustomClientResponse response; - - if (info->listener_->getResponse(response)) { - eprosima::fastcdr::Cdr deser( - *response.buffer_, - eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - _deserialize_ros_message( - deser, ros_response, info->response_type_support_, info->typesupport_identifier_); - - request_header->sequence_number = ((int64_t)response.sample_identity_.sequence_number().high) << - 32 | response.sample_identity_.sequence_number().low; - - *taken = true; - } - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_take_response( + eprosima_fastrtps_identifier, client, request_header, ros_response, taken); } rmw_ret_t @@ -76,38 +38,7 @@ rmw_send_response( rmw_request_id_t * request_header, void * ros_response) { - assert(service); - assert(request_header); - assert(ros_response); - - rmw_ret_t returnedValue = RMW_RET_ERROR; - - if (service->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("service handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(service->data); - assert(info); - - eprosima::fastrtps::rtps::WriteParams wparams; - memcpy(&wparams.related_sample_identity().writer_guid(), request_header->writer_guid, - sizeof(eprosima::fastrtps::rtps::GUID_t)); - wparams.related_sample_identity().sequence_number().high = - (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); - wparams.related_sample_identity().sequence_number().low = - (int32_t)(request_header->sequence_number & 0xFFFFFFFF); - - rmw_fastrtps_cpp::SerializedData data; - data.is_cdr_buffer = false; - data.data = const_cast(ros_response); - - if (info->response_publisher_->write(&data, wparams)) { - returnedValue = RMW_RET_OK; - } else { - RMW_SET_ERROR_MSG("cannot publish data"); - } - - return returnedValue; + return rmw_fastrtps_shared_cpp::__rmw_send_response( + eprosima_fastrtps_identifier, service, request_header, ros_response); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_serialize.cpp b/rmw_fastrtps_cpp/src/rmw_serialize.cpp index 6e9896cda..e0915d3d5 100644 --- a/rmw_fastrtps_cpp/src/rmw_serialize.cpp +++ b/rmw_fastrtps_cpp/src/rmw_serialize.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -19,7 +19,6 @@ #include "rmw/rmw.h" #include "./type_support_common.hpp" -#include "./ros_message_serialization.hpp" extern "C" { @@ -30,18 +29,19 @@ rmw_serialize( rmw_serialized_message_t * serialized_message) { const rosidl_message_type_support_t * ts = get_message_typesupport_handle( - type_support, rosidl_typesupport_introspection_c__identifier); + type_support, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!ts) { ts = get_message_typesupport_handle( - type_support, rosidl_typesupport_introspection_cpp::typesupport_identifier); + type_support, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); if (!ts) { RMW_SET_ERROR_MSG("type support not from this implementation"); return RMW_RET_ERROR; } } - auto tss = _create_message_type_support(ts->data, ts->typesupport_identifier); - auto data_length = _get_serialized_size(ros_message, tss, ts->typesupport_identifier); + auto callbacks = static_cast(ts->data); + auto tss = new MessageTypeSupport_cpp(callbacks); + auto data_length = tss->getEstimatedSerializedSize(ros_message); 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"); @@ -53,10 +53,10 @@ rmw_serialize( eprosima::fastcdr::Cdr ser( buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - auto ret = _serialize_ros_message(ros_message, buffer, ser, tss, ts->typesupport_identifier); + auto ret = tss->serializeROSmessage(ros_message, ser); serialized_message->buffer_length = data_length; serialized_message->buffer_capacity = data_length; - _delete_typesupport(tss, ts->typesupport_identifier); + delete tss; return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } @@ -67,24 +67,25 @@ rmw_deserialize( void * ros_message) { const rosidl_message_type_support_t * ts = get_message_typesupport_handle( - type_support, rosidl_typesupport_introspection_c__identifier); + type_support, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!ts) { ts = get_message_typesupport_handle( - type_support, rosidl_typesupport_introspection_cpp::typesupport_identifier); + type_support, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); if (!ts) { RMW_SET_ERROR_MSG("type support not from this implementation"); return RMW_RET_ERROR; } } - auto tss = _create_message_type_support(ts->data, ts->typesupport_identifier); + auto callbacks = static_cast(ts->data); + auto tss = new MessageTypeSupport_cpp(callbacks); eprosima::fastcdr::FastBuffer buffer( serialized_message->buffer, serialized_message->buffer_length); eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - auto ret = _deserialize_ros_message(deser, ros_message, tss, ts->typesupport_identifier); - _delete_typesupport(tss, ts->typesupport_identifier); + auto ret = tss->deserializeROSmessage(deser, ros_message); + delete tss; return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 161a0300c..028d2be25 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -30,21 +30,20 @@ #include "rmw/allocators.h" #include "rmw/rmw.h" -#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" -#include "rosidl_typesupport_introspection_c/identifier.h" - -#include "client_service_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "namespace_prefix.hpp" -#include "qos.hpp" -#include "type_support_common.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_cpp/custom_service_info.hpp" + +#include "./type_support_common.hpp" +#include "./namespace_prefix.hpp" +#include "./qos.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; using TopicDataType = eprosima::fastrtps::TopicDataType; +using CustomParticipantInfo = CustomParticipantInfo; extern "C" { @@ -87,10 +86,10 @@ rmw_create_service( } const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { type_support = get_service_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); if (!type_support) { RMW_SET_ERROR_MSG("type support not from this implementation"); return nullptr; @@ -106,33 +105,31 @@ rmw_create_service( info->participant_ = participant; info->typesupport_identifier_ = type_support->typesupport_identifier; - const void * untyped_request_members; - const void * untyped_response_members; + const service_type_support_callbacks_t * service_members; + const message_type_support_callbacks_t * request_members; + const message_type_support_callbacks_t * response_members; - untyped_request_members = - get_request_ptr(type_support->data, info->typesupport_identifier_); - untyped_response_members = get_response_ptr(type_support->data, - info->typesupport_identifier_); + service_members = static_cast(type_support->data); + request_members = static_cast( + service_members->request_members_->data); + response_members = static_cast( + service_members->response_members_->data); - std::string request_type_name = _create_type_name(untyped_request_members, "srv", - info->typesupport_identifier_); - std::string response_type_name = _create_type_name(untyped_response_members, "srv", - info->typesupport_identifier_); + std::string request_type_name = _create_type_name(request_members, "srv"); + std::string response_type_name = _create_type_name(response_members, "srv"); 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_); - _register_type(participant, info->request_type_support_, info->typesupport_identifier_); + info->request_type_support_ = new RequestTypeSupport_cpp(service_members); + _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_); - _register_type(participant, info->response_type_support_, info->typesupport_identifier_); + info->response_type_support_ = new ResponseTypeSupport_cpp(service_members); + _register_type(participant, info->response_type_support_); } subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; @@ -227,11 +224,11 @@ rmw_create_service( } if (info->request_type_support_) { - _unregister_type(participant, info->request_type_support_, info->typesupport_identifier_); + rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); } if (info->response_type_support_) { - _unregister_type(participant, info->response_type_support_, info->typesupport_identifier_); + rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); } delete info; @@ -249,44 +246,7 @@ rmw_create_service( rmw_ret_t rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) { - (void)node; - if (!service) { - RMW_SET_ERROR_MSG("service handle is null"); - return RMW_RET_ERROR; - } - if (service->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - CustomServiceInfo * info = static_cast(service->data); - if (info != nullptr) { - if (info->request_subscriber_ != nullptr) { - Domain::removeSubscriber(info->request_subscriber_); - } - if (info->response_publisher_ != nullptr) { - Domain::removePublisher(info->response_publisher_); - } - if (info->listener_ != nullptr) { - delete info->listener_; - } - - if (info->request_type_support_ != nullptr) { - _unregister_type(info->participant_, info->request_type_support_, - info->typesupport_identifier_); - } - if (info->response_type_support_ != nullptr) { - _unregister_type(info->participant_, info->response_type_support_, - info->typesupport_identifier_); - } - delete info; - } - if (service->service_name != nullptr) { - rmw_free(const_cast(service->service_name)); - service->service_name = nullptr; - } - rmw_service_free(service); - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_destroy_service( + eprosima_fastrtps_identifier, node, service); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_service_names_and_types.cpp b/rmw_fastrtps_cpp/src/rmw_service_names_and_types.cpp index 2994a183d..28b3edd33 100644 --- a/rmw_fastrtps_cpp/src/rmw_service_names_and_types.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service_names_and_types.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,12 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include - -#include "rcutils/strdup.h" - #include "rmw/allocators.h" #include "rmw/convert_rcutils_ret_to_rmw_ret.h" #include "rmw/error_handling.h" @@ -25,9 +19,9 @@ #include "rmw/names_and_types.h" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" -#include "demangle.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" extern "C" { @@ -37,123 +31,7 @@ rmw_get_service_names_and_types( rcutils_allocator_t * allocator, rmw_names_and_types_t * service_names_and_types) { - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null") - return RMW_RET_INVALID_ARGUMENT; - } - if (!node) { - RMW_SET_ERROR_MSG_ALLOC("null node handle", *allocator) - return RMW_RET_INVALID_ARGUMENT; - } - rmw_ret_t ret = rmw_names_and_types_check_zero(service_names_and_types); - if (ret != RMW_RET_OK) { - return ret; - } - - // Get participant pointer from node - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG_ALLOC("node handle not from this implementation", *allocator); - return RMW_RET_ERROR; - } - - auto impl = static_cast(node->data); - - // Access the slave Listeners, which are the ones that have the topicnamesandtypes member - // Get info from publisher and subscriber - // Combined results from the two lists - std::map> services; - { - ReaderInfo * slave_target = impl->secondarySubListener; - slave_target->mapmutex.lock(); - for (auto it : slave_target->topicNtypes) { - std::string service_name = _demangle_service_from_topic(it.first); - if (!service_name.length()) { - // not a service - continue; - } - for (auto & itt : it.second) { - std::string service_type = _demangle_service_type_only(itt); - if (service_type.length()) { - services[service_name].insert(service_type); - } - } - } - slave_target->mapmutex.unlock(); - } - { - WriterInfo * slave_target = impl->secondaryPubListener; - slave_target->mapmutex.lock(); - for (auto it : slave_target->topicNtypes) { - std::string service_name = _demangle_service_from_topic(it.first); - if (!service_name.length()) { - // not a service - continue; - } - for (auto & itt : it.second) { - std::string service_type = _demangle_service_type_only(itt); - if (service_type.length()) { - services[service_name].insert(service_type); - } - } - } - slave_target->mapmutex.unlock(); - } - - // Fill out service_names_and_types - if (services.size()) { - // Setup string array to store names - rmw_ret_t rmw_ret = - rmw_names_and_types_init(service_names_and_types, services.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&service_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(service_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "error during report of error: %s", rmw_get_error_string_safe()) - } - }; - // For each service, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & service_n_types : services) { - // Duplicate and store the service_name - char * service_name = rcutils_strdup(service_n_types.first.c_str(), *allocator); - if (!service_name) { - RMW_SET_ERROR_MSG_ALLOC("failed to allocate memory for service name", *allocator); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - service_names_and_types->names.data[index] = service_name; - // Setup storage for types - { - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &service_names_and_types->types[index], - service_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string_safe()) - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - } - // Duplicate and store each type for the service - size_t type_index = 0; - for (const auto & type : service_n_types.second) { - char * type_name = rcutils_strdup(type.c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG_ALLOC("failed to allocate memory for type name", *allocator) - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - service_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each service - } - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_get_service_names_and_types( + eprosima_fastrtps_identifier, node, allocator, service_names_and_types); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_service_server_is_available.cpp b/rmw_fastrtps_cpp/src/rmw_service_server_is_available.cpp index 734f55551..35c119d91 100644 --- a/rmw_fastrtps_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service_server_is_available.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,20 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include "fastrtps/subscriber/Subscriber.h" - -#include "rcutils/logging_macros.h" - #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" #include "rmw/types.h" -#include "demangle.hpp" -#include "rmw_fastrtps_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" extern "C" @@ -36,73 +30,7 @@ rmw_service_server_is_available( const rmw_client_t * client, bool * is_available) { - if (!node) { - RMW_SET_ERROR_MSG("node handle is null"); - return RMW_RET_ERROR; - } - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node handle, - node->implementation_identifier, eprosima_fastrtps_identifier, - return RMW_RET_ERROR); - - if (!client) { - RMW_SET_ERROR_MSG("client handle is null"); - return RMW_RET_ERROR; - } - - if (!is_available) { - RMW_SET_ERROR_MSG("is_available is null"); - return RMW_RET_ERROR; - } - - auto client_info = static_cast(client->data); - if (!client_info) { - RMW_SET_ERROR_MSG("client info handle is null"); - return RMW_RET_ERROR; - } - - auto pub_topic_name = - client_info->request_publisher_->getAttributes().topic.getTopicName(); - - auto pub_fqdn = _demangle_if_ros_topic(pub_topic_name); - - auto sub_topic_name = - client_info->response_subscriber_->getAttributes().topic.getTopicName(); - - auto sub_fqdn = _demangle_if_ros_topic(sub_topic_name); - - *is_available = false; - size_t number_of_request_subscribers = 0; - rmw_ret_t ret = rmw_count_subscribers( - node, - pub_fqdn.c_str(), - &number_of_request_subscribers); - if (ret != RMW_RET_OK) { - // error string already set - return ret; - } - if (0 == number_of_request_subscribers) { - // not ready - return RMW_RET_OK; - } - - size_t number_of_response_publishers = 0; - ret = rmw_count_publishers( - node, - sub_fqdn.c_str(), - &number_of_response_publishers); - if (ret != RMW_RET_OK) { - // error string already set - return ret; - } - if (0 == number_of_response_publishers) { - // not ready - return RMW_RET_OK; - } - - // all conditions met, there is a service server available - *is_available = true; - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_service_server_is_available( + eprosima_fastrtps_identifier, node, client, is_available); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 087ff2897..887d0ba22 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -19,16 +19,18 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" + #include "fastrtps/participant/Participant.h" #include "fastrtps/subscriber/Subscriber.h" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "namespace_prefix.hpp" -#include "qos.hpp" -#include "type_support_common.hpp" +#include "./namespace_prefix.hpp" +#include "./qos.hpp" +#include "./type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; @@ -75,10 +77,10 @@ rmw_create_subscription( } const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); if (!type_support) { RMW_SET_ERROR_MSG("type support not from this implementation"); return nullptr; @@ -96,14 +98,13 @@ rmw_create_subscription( info = new CustomSubscriberInfo(); info->typesupport_identifier_ = type_support->typesupport_identifier; - std::string type_name = _create_type_name( - type_support->data, "msg", info->typesupport_identifier_); + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks, "msg"); 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_); - _register_type(participant, info->type_support_, info->typesupport_identifier_); + info->type_support_ = new MessageTypeSupport_cpp(callbacks); + _register_type(participant, info->type_support_); } subscriberParam.historyMemoryPolicy = @@ -151,7 +152,7 @@ rmw_create_subscription( if (info != nullptr) { if (info->type_support_ != nullptr) { - _delete_typesupport(info->type_support_, info->typesupport_identifier_); + delete info->type_support_; } delete info; } @@ -166,51 +167,7 @@ rmw_create_subscription( rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { - if (!node) { - RMW_SET_ERROR_MSG("node handle is null"); - return RMW_RET_ERROR; - } - - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - if (!subscription) { - RMW_SET_ERROR_MSG("subscription handle is null"); - return RMW_RET_ERROR; - } - - if (subscription->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(subscription->data); - - if (info != nullptr) { - if (info->subscriber_ != nullptr) { - Domain::removeSubscriber(info->subscriber_); - } - if (info->listener_ != nullptr) { - delete info->listener_; - } - if (info->type_support_ != nullptr) { - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return RMW_RET_ERROR; - } - - Participant * participant = impl->participant; - _unregister_type(participant, info->type_support_, info->typesupport_identifier_); - } - delete info; - } - rmw_free(const_cast(subscription->topic_name)); - subscription->topic_name = nullptr; - rmw_subscription_free(subscription); - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, node, subscription); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_take.cpp b/rmw_fastrtps_cpp/src/rmw_take.cpp index 5eb49b901..849c19825 100644 --- a/rmw_fastrtps_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_cpp/src/rmw_take.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -17,84 +17,17 @@ #include "rmw/serialized_message.h" #include "rmw/rmw.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SampleInfo.h" -#include "fastrtps/attributes/SubscriberAttributes.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "fastcdr/Cdr.h" -#include "fastcdr/FastBuffer.h" - -#include "rmw_fastrtps_cpp/custom_subscriber_info.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "rmw_fastrtps_cpp/macros.hpp" -#include "rmw_fastrtps_cpp/TypeSupport.hpp" - -#include "./ros_message_serialization.hpp" extern "C" { -void -_assign_message_info( - rmw_message_info_t * message_info, - const eprosima::fastrtps::SampleInfo_t * sinfo) -{ - rmw_gid_t * sender_gid = &message_info->publisher_gid; - sender_gid->implementation_identifier = eprosima_fastrtps_identifier; - memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); - memcpy(sender_gid->data, &sinfo->sample_identity.writer_guid(), - sizeof(eprosima::fastrtps::rtps::GUID_t)); -} - -rmw_ret_t -_take( - const rmw_subscription_t * subscription, - void * ros_message, - bool * taken, - rmw_message_info_t * message_info) -{ - *taken = false; - - if (subscription->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - CustomSubscriberInfo * info = static_cast(subscription->data); - auto error_msg_allocator = rcutils_get_default_allocator(); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - info, "custom subscriber info is null", return RMW_RET_ERROR, error_msg_allocator); - - eprosima::fastrtps::SampleInfo_t sinfo; - - rmw_fastrtps_cpp::SerializedData data; - data.is_cdr_buffer = false; - data.data = ros_message; - if (info->subscriber_->takeNextData(&data, &sinfo)) { - info->listener_->data_taken(); - - if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { - if (message_info) { - _assign_message_info(message_info, &sinfo); - } - *taken = true; - } - } - - return RMW_RET_OK; -} - rmw_ret_t rmw_take(const rmw_subscription_t * subscription, void * ros_message, bool * taken) { - auto error_msg_allocator = rcutils_get_default_allocator(); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - subscription, "subscription pointer is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - ros_message, "ros_message pointer is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - taken, "boolean flag for taken is null", return RMW_RET_ERROR, error_msg_allocator); - - return _take(subscription, ros_message, taken, nullptr); + return rmw_fastrtps_shared_cpp::__rmw_take( + eprosima_fastrtps_identifier, subscription, ros_message, taken); } rmw_ret_t @@ -104,66 +37,8 @@ rmw_take_with_info( bool * taken, rmw_message_info_t * message_info) { - auto error_msg_allocator = rcutils_get_default_allocator(); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - subscription, "subscription pointer is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - ros_message, "ros_message pointer is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - taken, "boolean flag for taken is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - message_info, "message info pointer is null", return RMW_RET_ERROR, error_msg_allocator); - - return _take(subscription, ros_message, taken, message_info); -} - -rmw_ret_t -_take_serialized_message( - const rmw_subscription_t * subscription, - rmw_serialized_message_t * serialized_message, - bool * taken, - rmw_message_info_t * message_info) -{ - *taken = false; - - if (subscription->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - CustomSubscriberInfo * info = static_cast(subscription->data); - auto error_msg_allocator = rcutils_get_default_allocator(); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - info, "custom subscriber info is null", return RMW_RET_ERROR, error_msg_allocator); - - eprosima::fastcdr::FastBuffer buffer; - eprosima::fastrtps::SampleInfo_t sinfo; - - rmw_fastrtps_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = &buffer; - if (info->subscriber_->takeNextData(&data, &sinfo)) { - info->listener_->data_taken(); - - if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { - auto buffer_size = static_cast(buffer.getBufferSize()); - if (serialized_message->buffer_capacity < buffer_size) { - auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); - if (ret != RMW_RET_OK) { - return ret; // Error message already set - } - } - serialized_message->buffer_length = buffer_size; - memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); - - if (message_info) { - _assign_message_info(message_info, &sinfo); - } - *taken = true; - } - } - - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_take_with_info( + eprosima_fastrtps_identifier, subscription, ros_message, taken, message_info); } rmw_ret_t @@ -172,15 +47,8 @@ rmw_take_serialized_message( rmw_serialized_message_t * serialized_message, bool * taken) { - auto error_msg_allocator = rcutils_get_default_allocator(); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - subscription, "subscription pointer is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - serialized_message, "ros_message pointer is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - taken, "boolean flag for taken is null", return RMW_RET_ERROR, error_msg_allocator); - - return _take_serialized_message(subscription, serialized_message, taken, nullptr); + return rmw_fastrtps_shared_cpp::__rmw_take_serialized_message( + eprosima_fastrtps_identifier, subscription, serialized_message, taken); } rmw_ret_t @@ -190,16 +58,7 @@ rmw_take_serialized_message_with_info( bool * taken, rmw_message_info_t * message_info) { - auto error_msg_allocator = rcutils_get_default_allocator(); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - subscription, "subscription pointer is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - serialized_message, "ros_message pointer is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - taken, "boolean flag for taken is null", return RMW_RET_ERROR, error_msg_allocator); - RCUTILS_CHECK_FOR_NULL_WITH_MSG( - message_info, "message info pointer is null", return RMW_RET_ERROR, error_msg_allocator); - - return _take_serialized_message(subscription, serialized_message, taken, message_info); + return rmw_fastrtps_shared_cpp::__rmw_take_serialized_message_with_info( + eprosima_fastrtps_identifier, subscription, serialized_message, taken, message_info); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_topic_names_and_types.cpp b/rmw_fastrtps_cpp/src/rmw_topic_names_and_types.cpp index 3d8befec7..14146ac9a 100644 --- a/rmw_fastrtps_cpp/src/rmw_topic_names_and_types.cpp +++ b/rmw_fastrtps_cpp/src/rmw_topic_names_and_types.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -30,10 +30,9 @@ #include "rmw/names_and_types.h" #include "rmw/rmw.h" -#include "demangle.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" -#include "namespace_prefix.hpp" -#include "rmw_fastrtps_cpp/custom_participant_info.hpp" extern "C" { @@ -44,125 +43,7 @@ rmw_get_topic_names_and_types( bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null") - return RMW_RET_INVALID_ARGUMENT; - } - if (!node) { - RMW_SET_ERROR_MSG_ALLOC("null node handle", *allocator) - return RMW_RET_INVALID_ARGUMENT; - } - - rmw_ret_t ret = rmw_names_and_types_check_zero(topic_names_and_types); - if (ret != RMW_RET_OK) { - return ret; - } - - // Get participant pointer from node - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG_ALLOC("node handle not from this implementation", *allocator); - return RMW_RET_ERROR; - } - - auto impl = static_cast(node->data); - - // Access the slave Listeners, which are the ones that have the topicnamesandtypes member - // Get info from publisher and subscriber - // Combined results from the two lists - std::map> topics; - { - ReaderInfo * slave_target = impl->secondarySubListener; - slave_target->mapmutex.lock(); - for (auto it : slave_target->topicNtypes) { - if (!no_demangle && _get_ros_prefix_if_exists(it.first) != ros_topic_prefix) { - // if we are demangling and this is not prefixed with rt/, skip it - continue; - } - for (auto & itt : it.second) { - topics[it.first].insert(itt); - } - } - slave_target->mapmutex.unlock(); - } - { - WriterInfo * slave_target = impl->secondaryPubListener; - slave_target->mapmutex.lock(); - for (auto it : slave_target->topicNtypes) { - if (!no_demangle && _get_ros_prefix_if_exists(it.first) != ros_topic_prefix) { - // if we are demangling and this is not prefixed with rt/, skip it - continue; - } - for (auto & itt : it.second) { - topics[it.first].insert(itt); - } - } - slave_target->mapmutex.unlock(); - } - - // Copy data to results handle - if (topics.size() > 0) { - // Setup string array to store names - rmw_ret_t rmw_ret = rmw_names_and_types_init(topic_names_and_types, topics.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&topic_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "error during report of error: %s", rmw_get_error_string_safe()) - } - }; - // Setup demangling functions based on no_demangle option - auto demangle_topic = _demangle_if_ros_topic; - auto demangle_type = _demangle_if_ros_type; - if (no_demangle) { - auto noop = [](const std::string & in) { - return in; - }; - demangle_topic = noop; - demangle_type = noop; - } - // For each topic, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & topic_n_types : topics) { - // Duplicate and store the topic_name - char * topic_name = rcutils_strdup(demangle_topic(topic_n_types.first).c_str(), *allocator); - if (!topic_name) { - RMW_SET_ERROR_MSG_ALLOC("failed to allocate memory for topic name", *allocator); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->names.data[index] = topic_name; - // Setup storage for types - { - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &topic_names_and_types->types[index], - topic_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string_safe()) - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - } - // Duplicate and store each type for the topic - size_t type_index = 0; - for (const auto & type : topic_n_types.second) { - char * type_name = rcutils_strdup(demangle_type(type).c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG_ALLOC("failed to allocate memory for type name", *allocator) - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each topic - } - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_get_topic_names_and_types( + eprosima_fastrtps_identifier, node, allocator, no_demangle, topic_names_and_types); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_trigger_guard_condition.cpp b/rmw_fastrtps_cpp/src/rmw_trigger_guard_condition.cpp index e842bb855..ec07e4329 100644 --- a/rmw_fastrtps_cpp/src/rmw_trigger_guard_condition.cpp +++ b/rmw_fastrtps_cpp/src/rmw_trigger_guard_condition.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,28 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" -#include "types/guard_condition.hpp" extern "C" { rmw_ret_t rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition_handle) { - assert(guard_condition_handle); - - if (guard_condition_handle->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("guard condition handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto guard_condition = static_cast(guard_condition_handle->data); - guard_condition->trigger(); - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, guard_condition_handle); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_wait.cpp b/rmw_fastrtps_cpp/src/rmw_wait.cpp index 30f53250a..5ec82be64 100644 --- a/rmw_fastrtps_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_cpp/src/rmw_wait.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,67 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fastrtps/subscriber/Subscriber.h" - #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_cpp/custom_client_info.hpp" -#include "rmw_fastrtps_cpp/custom_service_info.hpp" -#include "rmw_fastrtps_cpp/custom_subscriber_info.hpp" -#include "types/custom_wait_set_info.hpp" -#include "types/guard_condition.hpp" - -// helper function for wait -bool -check_wait_set_for_data( - const rmw_subscriptions_t * subscriptions, - const rmw_guard_conditions_t * guard_conditions, - const rmw_services_t * services, - const rmw_clients_t * clients) -{ - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - void * data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - // Short circuiting out of this function is possible - if (custom_subscriber_info && custom_subscriber_info->listener_->hasData()) { - return true; - } - } - } - - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - void * data = clients->clients[i]; - CustomClientInfo * custom_client_info = static_cast(data); - if (custom_client_info && custom_client_info->listener_->hasData()) { - return true; - } - } - } - - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - void * data = services->services[i]; - CustomServiceInfo * custom_service_info = static_cast(data); - if (custom_service_info && custom_service_info->listener_->hasData()) { - return true; - } - } - } - - if (guard_conditions) { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - if (guard_condition && guard_condition->hasTriggered()) { - return true; - } - } - } - return false; -} +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" extern "C" { @@ -85,134 +28,7 @@ rmw_wait( rmw_wait_set_t * wait_set, const rmw_time_t * wait_timeout) { - if (!wait_set) { - RMW_SET_ERROR_MSG("wait set handle is null"); - return RMW_RET_ERROR; - } - CustomWaitsetInfo * wait_set_info = static_cast(wait_set->data); - if (!wait_set_info) { - RMW_SET_ERROR_MSG("Waitset info struct is null"); - return RMW_RET_ERROR; - } - std::mutex * conditionMutex = &wait_set_info->condition_mutex; - std::condition_variable * conditionVariable = &wait_set_info->condition; - if (!conditionMutex) { - RMW_SET_ERROR_MSG("Mutex for wait set was null"); - return RMW_RET_ERROR; - } - if (!conditionVariable) { - RMW_SET_ERROR_MSG("Condition variable for wait set was null"); - return RMW_RET_ERROR; - } - - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - void * data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - custom_subscriber_info->listener_->attachCondition(conditionMutex, conditionVariable); - } - } - - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - void * data = clients->clients[i]; - CustomClientInfo * custom_client_info = static_cast(data); - custom_client_info->listener_->attachCondition(conditionMutex, conditionVariable); - } - } - - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - void * data = services->services[i]; - auto custom_service_info = static_cast(data); - custom_service_info->listener_->attachCondition(conditionMutex, conditionVariable); - } - } - - if (guard_conditions) { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - guard_condition->attachCondition(conditionMutex, conditionVariable); - } - } - - // This mutex prevents any of the listeners - // to change the internal state and notify the condition - // between the call to hasData() / hasTriggered() and wait() - // otherwise the decision to wait might be incorrect - std::unique_lock lock(*conditionMutex); - - bool hasData = check_wait_set_for_data(subscriptions, guard_conditions, services, clients); - auto predicate = [subscriptions, guard_conditions, services, clients]() { - return check_wait_set_for_data(subscriptions, guard_conditions, services, clients); - }; - - bool timeout = false; - if (!hasData) { - if (!wait_timeout) { - conditionVariable->wait(lock, predicate); - } else if (wait_timeout->sec > 0 || wait_timeout->nsec > 0) { - auto n = std::chrono::duration_cast( - std::chrono::seconds(wait_timeout->sec)); - n += std::chrono::nanoseconds(wait_timeout->nsec); - timeout = !conditionVariable->wait_for(lock, n, predicate); - } else { - timeout = true; - } - } - - // Unlock the condition variable mutex to prevent deadlocks that can occur if - // a listener triggers while the condition variable is being detached. - // Listeners will no longer be prevented from changing their internal state, - // but that should not cause issues (if a listener has data / has triggered - // after we check, it will be caught on the next call to this function). - lock.unlock(); - - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - void * data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - custom_subscriber_info->listener_->detachCondition(); - if (!custom_subscriber_info->listener_->hasData()) { - subscriptions->subscribers[i] = 0; - } - } - } - - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - void * data = clients->clients[i]; - CustomClientInfo * custom_client_info = static_cast(data); - custom_client_info->listener_->detachCondition(); - if (!custom_client_info->listener_->hasData()) { - clients->clients[i] = 0; - } - } - } - - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - void * data = services->services[i]; - auto custom_service_info = static_cast(data); - custom_service_info->listener_->detachCondition(); - if (!custom_service_info->listener_->hasData()) { - services->services[i] = 0; - } - } - } - - if (guard_conditions) { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - void * data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - guard_condition->detachCondition(); - if (!guard_condition->getHasTriggered()) { - guard_conditions->guard_conditions[i] = 0; - } - } - } - - return timeout ? RMW_RET_TIMEOUT : RMW_RET_OK; + return rmw_fastrtps_shared_cpp::__rmw_wait( + subscriptions, guard_conditions, services, clients, wait_set, wait_timeout); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_wait_set.cpp b/rmw_fastrtps_cpp/src/rmw_wait_set.cpp index e9bf61e57..f18a04864 100644 --- a/rmw_fastrtps_cpp/src/rmw_wait_set.cpp +++ b/rmw_fastrtps_cpp/src/rmw_wait_set.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -15,81 +15,24 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw/impl/cpp/macros.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "types/custom_wait_set_info.hpp" extern "C" { rmw_wait_set_t * rmw_create_wait_set(size_t max_conditions) { - (void)max_conditions; - rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); - CustomWaitsetInfo * wait_set_info = nullptr; - - // From here onward, error results in unrolling in the goto fail block. - if (!wait_set) { - RMW_SET_ERROR_MSG("failed to allocate wait set"); - goto fail; - } - wait_set->implementation_identifier = eprosima_fastrtps_identifier; - wait_set->data = rmw_allocate(sizeof(CustomWaitsetInfo)); - // This should default-construct the fields of CustomWaitsetInfo - wait_set_info = static_cast(wait_set->data); - RMW_TRY_PLACEMENT_NEW(wait_set_info, wait_set_info, goto fail, CustomWaitsetInfo, ) - if (!wait_set_info) { - RMW_SET_ERROR_MSG("failed to construct wait set info struct"); - goto fail; - } - - return wait_set; - -fail: - if (wait_set) { - if (wait_set->data) { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - wait_set_info->~CustomWaitsetInfo(), wait_set_info) - rmw_free(wait_set->data); - } - rmw_wait_set_free(wait_set); - } - return nullptr; + return rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + eprosima_fastrtps_identifier, max_conditions); } rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t * wait_set) { - if (!wait_set) { - RMW_SET_ERROR_MSG("wait set handle is null"); - return RMW_RET_ERROR; - } - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - wait set handle, - wait_set->implementation_identifier, eprosima_fastrtps_identifier, - return RMW_RET_ERROR) - - auto result = RMW_RET_OK; - auto wait_set_info = static_cast(wait_set->data); - if (!wait_set_info) { - RMW_SET_ERROR_MSG("wait set info is null"); - return RMW_RET_ERROR; - } - std::mutex * conditionMutex = &wait_set_info->condition_mutex; - if (!conditionMutex) { - RMW_SET_ERROR_MSG("wait set mutex is null"); - return RMW_RET_ERROR; - } - - if (wait_set->data) { - if (wait_set_info) { - RMW_TRY_DESTRUCTOR( - wait_set_info->~CustomWaitsetInfo(), wait_set_info, result = RMW_RET_ERROR) - } - rmw_free(wait_set->data); - } - rmw_wait_set_free(wait_set); - return result; + return rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + eprosima_fastrtps_identifier, wait_set); } } // extern "C" diff --git a/rmw_fastrtps_cpp/src/ros_message_serialization.cpp b/rmw_fastrtps_cpp/src/ros_message_serialization.cpp index 74c5d023d..9e1994806 100644 --- a/rmw_fastrtps_cpp/src/ros_message_serialization.cpp +++ b/rmw_fastrtps_cpp/src/ros_message_serialization.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -17,46 +17,6 @@ #include "./type_support_common.hpp" -size_t -_get_serialized_size( - const 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->getEstimatedSerializedSize(ros_message); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - return typed_typesupport->getEstimatedSerializedSize(ros_message); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return 0; -} - -bool -_serialize_ros_message( - const void * ros_message, - eprosima::fastcdr::FastBuffer & buffer, - eprosima::fastcdr::Cdr & ser, - void * untyped_typesupport, - const char * typesupport_identifier) -{ - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - size_t estimated_size = typed_typesupport->getEstimatedSerializedSize(ros_message); - buffer.reserve(estimated_size); - return typed_typesupport->serializeROSmessage(ros_message, ser); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - size_t estimated_size = typed_typesupport->getEstimatedSerializedSize(ros_message); - buffer.reserve(estimated_size); - return typed_typesupport->serializeROSmessage(ros_message, ser); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return false; -} - bool _deserialize_ros_message( eprosima::fastcdr::Cdr & deser, diff --git a/rmw_fastrtps_cpp/src/ros_message_serialization.hpp b/rmw_fastrtps_cpp/src/ros_message_serialization.hpp index f39692863..73006fafc 100644 --- a/rmw_fastrtps_cpp/src/ros_message_serialization.hpp +++ b/rmw_fastrtps_cpp/src/ros_message_serialization.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -24,12 +24,6 @@ class FastBuffer; } // namespace fastcdr } // namespace eprosima -size_t -_get_serialized_size( - const void * ros_message, - void * untyped_typesupport, - const char * typesupport_identifier); - bool _serialize_ros_message( const void * ros_message, diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp index b93247684..5869535d1 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.cpp +++ b/rmw_fastrtps_cpp/src/type_support_common.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,127 +12,129 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw/error_handling.h" - -#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include -#include "rosidl_typesupport_introspection_c/identifier.h" +#include "rmw/error_handling.h" #include "type_support_common.hpp" -bool -using_introspection_c_typesupport(const char * typesupport_identifier) +namespace rmw_fastrtps_cpp { - return typesupport_identifier == rosidl_typesupport_introspection_c__identifier; -} -bool -using_introspection_cpp_typesupport(const char * typesupport_identifier) +TypeSupport::TypeSupport() { - return typesupport_identifier == - rosidl_typesupport_introspection_cpp::typesupport_identifier; + m_isGetKeyDefined = false; + max_size_bound_ = false; } -void * -_create_message_type_support(const void * untyped_members, const char * typesupport_identifier) +void TypeSupport::set_members(const message_type_support_callbacks_t * members) { - 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); + members_ = members; + + // Fully bound by default + max_size_bound_ = true; + auto data_size = static_cast(members->max_serialized_size(max_size_bound_)); + + // A fully bound message of size 0 is an empty message + if (max_size_bound_ && (data_size == 0) ) { + has_data_ = false; + ++data_size; // Dummy byte + } else { + has_data_ = true; } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; + + // Total size is encapsulation size + data size + m_typeSize = 4 + data_size; } -void * -_create_request_type_support(const void * untyped_members, const char * typesupport_identifier) +size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message) { - 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); + if (max_size_bound_) { + return m_typeSize; } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; + + assert(ros_message); + + // Encapsulation size + message size + return 4 + members_->get_serialized_size(ros_message); } -void * -_create_response_type_support(const void * untyped_members, const char * typesupport_identifier) +bool TypeSupport::serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser) { - 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); + assert(ros_message); + + // Serialize encapsulation + ser.serialize_encapsulation(); + + // If type is not empty, serialize message + if (has_data_) { + return members_->cdr_serialize(ros_message, ser); } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; + + // Otherwise, add a dummy byte + ser << (uint8_t)0; + return true; } -void -_register_type( - eprosima::fastrtps::Participant * participant, - void * untyped_typesupport, - const char * typesupport_identifier) +bool TypeSupport::deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message) { - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); - } else { - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + assert(ros_message); + + // Deserialize encapsulation. + deser.read_encapsulation(); + + // If type is not empty, deserialize message + if (has_data_) { + return members_->cdr_deserialize(deser, ros_message); } + + // Otherwise, consume dummy byte + uint8_t dump = 0; + deser >> dump; + (void)dump; + + return true; } -void -_unregister_type( - eprosima::fastrtps::Participant * participant, - void * untyped_typesupport, - const char * typesupport_identifier) +MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members) { - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - if (eprosima::fastrtps::Domain::unregisterType(participant, typed_typesupport->getName())) { - delete typed_typesupport; - } - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - if (eprosima::fastrtps::Domain::unregisterType(participant, typed_typesupport->getName())) { - delete typed_typesupport; - } - } else { - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - } + assert(members); + + std::string name = std::string(members->package_name_) + "::msg::dds_::" + + members->message_name_ + "_"; + this->setName(name.c_str()); + + set_members(members); } -void -_delete_typesupport(void * untyped_typesupport, const char * typesupport_identifier) +ServiceTypeSupport::ServiceTypeSupport() { - if (using_introspection_c_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - if (typed_typesupport != nullptr) { - delete typed_typesupport; - } - } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { - auto typed_typesupport = static_cast(untyped_typesupport); - if (typed_typesupport != nullptr) { - delete typed_typesupport; - } - } else { - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - } } + +RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * members) +{ + assert(members); + + std::string name = std::string(members->package_name_) + "::srv::dds_::" + + members->service_name_ + "_Request_"; + this->setName(name.c_str()); + + auto msg = static_cast( + members->request_members_->data); + set_members(msg); +} + +ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members) +{ + assert(members); + + std::string name = std::string(members->package_name_) + "::srv::dds_::" + + members->service_name_ + "_Response_"; + this->setName(name.c_str()); + + auto msg = static_cast( + members->response_members_->data); + set_members(msg); +} + +} // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/type_support_common.hpp b/rmw_fastrtps_cpp/src/type_support_common.hpp index a2d9906e0..75af57fd6 100644 --- a/rmw_fastrtps_cpp/src/type_support_common.hpp +++ b/rmw_fastrtps_cpp/src/type_support_common.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -22,54 +22,30 @@ #include "rmw/error_handling.h" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + #include "rmw_fastrtps_cpp/MessageTypeSupport.hpp" #include "rmw_fastrtps_cpp/ServiceTypeSupport.hpp" -#include "rosidl_typesupport_introspection_c/visibility_control.h" - #include "rmw_fastrtps_cpp/identifier.hpp" -using MessageTypeSupport_c = - rmw_fastrtps_cpp::MessageTypeSupport; -using MessageTypeSupport_cpp = - rmw_fastrtps_cpp::MessageTypeSupport; -using TypeSupport_c = - rmw_fastrtps_cpp::TypeSupport; -using TypeSupport_cpp = - rmw_fastrtps_cpp::TypeSupport; - -using RequestTypeSupport_c = rmw_fastrtps_cpp::RequestTypeSupport< - rosidl_typesupport_introspection_c__ServiceMembers, - rosidl_typesupport_introspection_c__MessageMembers ->; -using RequestTypeSupport_cpp = rmw_fastrtps_cpp::RequestTypeSupport< - rosidl_typesupport_introspection_cpp::ServiceMembers, - rosidl_typesupport_introspection_cpp::MessageMembers ->; - -using ResponseTypeSupport_c = rmw_fastrtps_cpp::ResponseTypeSupport< - rosidl_typesupport_introspection_c__ServiceMembers, - rosidl_typesupport_introspection_c__MessageMembers ->; -using ResponseTypeSupport_cpp = rmw_fastrtps_cpp::ResponseTypeSupport< - rosidl_typesupport_introspection_cpp::ServiceMembers, - rosidl_typesupport_introspection_cpp::MessageMembers ->; - -bool -using_introspection_c_typesupport(const char * typesupport_identifier); +#include "rosidl_typesupport_fastrtps_c/identifier.h" +#include "rosidl_typesupport_fastrtps_cpp/identifier.hpp" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" +#define RMW_FASTRTPS_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier +#define RMW_FASTRTPS_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier -bool -using_introspection_cpp_typesupport(const char * typesupport_identifier); +using MessageTypeSupport_cpp = rmw_fastrtps_cpp::MessageTypeSupport; +using TypeSupport_cpp = rmw_fastrtps_cpp::TypeSupport; +using RequestTypeSupport_cpp = rmw_fastrtps_cpp::RequestTypeSupport; +using ResponseTypeSupport_cpp = rmw_fastrtps_cpp::ResponseTypeSupport; -template -ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL inline std::string _create_type_name( - const void * untyped_members, + const message_type_support_callbacks_t * members, const std::string & sep) { - auto members = static_cast(untyped_members); if (!members) { RMW_SET_ERROR_MSG("members handle is null"); return ""; @@ -78,46 +54,12 @@ _create_type_name( std::string(members->package_name_) + "::" + sep + "::dds_::" + members->message_name_ + "_"; } -ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL -inline std::string -_create_type_name( - const void * untyped_members, - const std::string & sep, - const char * typesupport) -{ - if (using_introspection_c_typesupport(typesupport)) { - return _create_type_name( - untyped_members, sep); - } else if (using_introspection_cpp_typesupport(typesupport)) { - return _create_type_name( - untyped_members, sep); - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return ""; -} - -void * -_create_message_type_support(const void * untyped_members, const char * typesupport_identifier); - -void * -_create_request_type_support(const void * untyped_members, const char * typesupport_identifier); - -void * -_create_response_type_support(const void * untyped_members, const char * typesupport_identifier); - -void +inline void _register_type( eprosima::fastrtps::Participant * participant, - void * untyped_typesupport, - const char * typesupport_identifier); - -void -_unregister_type( - eprosima::fastrtps::Participant * participant, - void * untyped_typesupport, - const char * typesupport_identifier); - -void -_delete_typesupport(void * untyped_typesupport, const char * typesupport_identifier); + rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) +{ + eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); +} #endif // TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt new file mode 100644 index 000000000..33c8e9ddb --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -0,0 +1,135 @@ +# 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. + +cmake_minimum_required(VERSION 3.5) + +project(rmw_fastrtps_dynamic_cpp) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_ros REQUIRED) + +find_package(rcutils REQUIRED) +find_package(rmw_fastrtps_shared_cpp REQUIRED) + +find_package(fastrtps_cmake_module REQUIRED) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +find_package(rmw REQUIRED) +find_package(rosidl_generator_c REQUIRED) +find_package(rosidl_typesupport_introspection_c REQUIRED) +find_package(rosidl_typesupport_introspection_cpp REQUIRED) + +include_directories(include) + +add_library(rmw_fastrtps_dynamic_cpp + src/client_service_common.cpp + src/get_client.cpp + src/get_participant.cpp + src/get_publisher.cpp + src/get_service.cpp + src/get_subscriber.cpp + src/identifier.cpp + src/namespace_prefix.cpp + src/qos.cpp + src/rmw_logging.cpp + src/rmw_client.cpp + src/rmw_compare_gids_equal.cpp + src/rmw_count.cpp + src/rmw_get_gid_for_publisher.cpp + src/rmw_get_implementation_identifier.cpp + src/rmw_guard_condition.cpp + src/rmw_init.cpp + src/rmw_node.cpp + src/rmw_node_names.cpp + src/rmw_publish.cpp + src/rmw_publisher.cpp + src/rmw_request.cpp + src/rmw_response.cpp + src/rmw_serialize.cpp + src/rmw_service.cpp + src/rmw_service_names_and_types.cpp + src/rmw_service_server_is_available.cpp + src/rmw_subscription.cpp + src/rmw_take.cpp + src/rmw_topic_names_and_types.cpp + src/rmw_trigger_guard_condition.cpp + src/rmw_wait.cpp + src/rmw_wait_set.cpp + src/type_support_common.cpp +) +target_link_libraries(rmw_fastrtps_dynamic_cpp + fastcdr fastrtps) + +# specific order: dependents before dependencies +ament_target_dependencies(rmw_fastrtps_dynamic_cpp + "rcutils" + "rosidl_typesupport_introspection_c" + "rosidl_typesupport_introspection_cpp" + "rmw_fastrtps_shared_cpp" + "rmw" + "rosidl_generator_c" +) + +configure_rmw_library(rmw_fastrtps_dynamic_cpp) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} +PRIVATE "RMW_FASTRTPS_DYNAMIC_CPP_BUILDING_LIBRARY") + +# specific order: dependents before dependencies +ament_export_include_directories(include) +ament_export_libraries(rmw_fastrtps_dynamic_cpp) + +ament_export_dependencies(rosidl_typesupport_introspection_cpp) +ament_export_dependencies(rosidl_typesupport_introspection_c) +ament_export_dependencies(rosidl_generator_c) +ament_export_dependencies(rcutils) +ament_export_dependencies(rmw_fastrtps_shared_cpp) +ament_export_dependencies(rmw) + +register_rmw_implementation( + "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" + "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package( + CONFIG_EXTRAS_POST "rmw_fastrtps_dynamic_cpp-extras.cmake" +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS rmw_fastrtps_dynamic_cpp + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) 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 new file mode 100644 index 000000000..db9158643 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp @@ -0,0 +1,42 @@ +// 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 RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_HPP_ + +#include +#include + +#include +#include + +#include "TypeSupport.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +template +class MessageTypeSupport : public TypeSupport +{ +public: + explicit MessageTypeSupport(const MembersType * members); +}; + +} // namespace rmw_fastrtps_dynamic_cpp + +#include "MessageTypeSupport_impl.hpp" + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp similarity index 76% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport_impl.hpp rename to rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp index b629ab872..782645b0c 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/MessageTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ -#define RMW_FASTRTPS_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ #include #include @@ -22,10 +22,10 @@ #include #include -#include "rmw_fastrtps_cpp/MessageTypeSupport.hpp" +#include "rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" -namespace rmw_fastrtps_cpp +namespace rmw_fastrtps_dynamic_cpp { template @@ -49,6 +49,6 @@ MessageTypeSupport::MessageTypeSupport(const MembersType * members) } } -} // namespace rmw_fastrtps_cpp +} // namespace rmw_fastrtps_dynamic_cpp -#endif // RMW_FASTRTPS_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ +#endif // RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ 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 new file mode 100644 index 000000000..3b908f1e9 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp @@ -0,0 +1,53 @@ +// 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 RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ + +#include +#include +#include + +#include "TypeSupport.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +template +class ServiceTypeSupport : public TypeSupport +{ +protected: + ServiceTypeSupport(); +}; + +template +class RequestTypeSupport : public ServiceTypeSupport +{ +public: + explicit RequestTypeSupport(const ServiceMembersType * members); +}; + +template +class ResponseTypeSupport : public ServiceTypeSupport +{ +public: + explicit ResponseTypeSupport(const ServiceMembersType * members); +}; + +} // namespace rmw_fastrtps_dynamic_cpp + +#include "ServiceTypeSupport_impl.hpp" + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp similarity index 84% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport_impl.hpp rename to rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp index bac9db48c..3451577f8 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/ServiceTypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_IMPL_HPP_ -#define RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_IMPL_HPP_ +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ #include #include #include #include -#include "rmw_fastrtps_cpp/ServiceTypeSupport.hpp" +#include "rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" -namespace rmw_fastrtps_cpp +namespace rmw_fastrtps_dynamic_cpp { template @@ -75,6 +75,6 @@ ResponseTypeSupport::ResponseTypeSupport } } -} // namespace rmw_fastrtps_cpp +} // namespace rmw_fastrtps_dynamic_cpp -#endif // RMW_FASTRTPS_CPP__SERVICETYPESUPPORT_IMPL_HPP_ +#endif // RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ 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 new file mode 100644 index 000000000..6f54b8458 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp @@ -0,0 +1,166 @@ +// 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 RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/visibility_control.h" + +#include "rosidl_typesupport_introspection_c/field_types.h" +#include "rosidl_typesupport_introspection_c/identifier.h" +#include "rosidl_typesupport_introspection_c/message_introspection.h" +#include "rosidl_typesupport_introspection_c/service_introspection.h" +#include "rosidl_typesupport_introspection_c/visibility_control.h" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +// Helper class that uses template specialization to read/write string types to/from a +// eprosima::fastcdr::Cdr +template +struct StringHelper; + +// For C introspection typesupport we create intermediate instances of std::string so that +// eprosima::fastcdr::Cdr can handle the string properly. +template<> +struct StringHelper +{ + using type = rosidl_generator_c__String; + + static size_t next_field_align(void * data, size_t current_alignment) + { + auto c_string = static_cast(data); + if (!c_string) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Failed to cast data as rosidl_generator_c__String") + return current_alignment; + } + if (!c_string->data) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "rosidl_generator_c_String had invalid data") + return current_alignment; + } + + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + current_alignment += 4; + return current_alignment + strlen(c_string->data) + 1; + } + + static std::string convert_to_std_string(void * data) + { + auto c_string = static_cast(data); + if (!c_string) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Failed to cast data as rosidl_generator_c__String") + return ""; + } + if (!c_string->data) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "rosidl_generator_c_String had invalid data") + return ""; + } + return std::string(c_string->data); + } + + static std::string convert_to_std_string(rosidl_generator_c__String & data) + { + return std::string(data.data); + } + + static void assign(eprosima::fastcdr::Cdr & deser, void * field, bool) + { + std::string str; + deser >> str; + rosidl_generator_c__String * c_str = static_cast(field); + rosidl_generator_c__String__assign(c_str, str.c_str()); + } +}; + +// For C++ introspection typesupport we just reuse the same std::string transparently. +template<> +struct StringHelper +{ + using type = std::string; + + static std::string & convert_to_std_string(void * data) + { + return *(static_cast(data)); + } + + static void assign(eprosima::fastcdr::Cdr & deser, void * field, bool call_new) + { + std::string & str = *(std::string *)field; + if (call_new) { + new(&str) std::string; + } + deser >> str; + } +}; + +template +class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport +{ +public: + size_t getEstimatedSerializedSize(const void * ros_message); + + bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser); + + bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message); + +protected: + TypeSupport(); + + size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment); + + const MembersType * members_; + +private: + size_t getEstimatedSerializedSize( + const MembersType * members, const void * ros_message, size_t current_alignment); + + bool serializeROSmessage( + eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message); + + bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, const MembersType * members, void * ros_message, + bool call_new); +}; + +} // namespace rmw_fastrtps_dynamic_cpp + +#include "TypeSupport_impl.hpp" + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp similarity index 91% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp rename to rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp index 83f746e6a..632b946d2 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__TYPESUPPORT_IMPL_HPP_ -#define RMW_FASTRTPS_CPP__TYPESUPPORT_IMPL_HPP_ +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ #include #include @@ -21,8 +21,8 @@ #include #include -#include "rmw_fastrtps_cpp/TypeSupport.hpp" -#include "rmw_fastrtps_cpp/macros.hpp" +#include "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_dynamic_cpp/macros.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" #include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" @@ -32,7 +32,7 @@ #include "rosidl_generator_c/primitives_array_functions.h" -namespace rmw_fastrtps_cpp +namespace rmw_fastrtps_dynamic_cpp { template @@ -111,13 +111,6 @@ TypeSupport::TypeSupport() max_size_bound_ = false; } -template -void TypeSupport::deleteData(void * data) -{ - assert(data); - delete static_cast(data); -} - static inline void * align_(size_t __align, void * & __ptr) noexcept { @@ -984,91 +977,6 @@ bool TypeSupport::deserializeROSmessage( return true; } -template -void * TypeSupport::createData() -{ - return new eprosima::fastcdr::FastBuffer(); -} - -template -bool TypeSupport::serialize( - void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) -{ - assert(data); - assert(payload); - - auto ser_data = static_cast(data); - if (ser_data->is_cdr_buffer) { - auto ser = static_cast(ser_data->data); - if (payload->max_size >= ser->getSerializedDataLength()) { - payload->length = static_cast(ser->getSerializedDataLength()); - payload->encapsulation = ser->endianness() == - eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; - memcpy(payload->data, ser->getBufferPointer(), ser->getSerializedDataLength()); - return true; - } - } else { - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(payload->data), - 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)) { - payload->encapsulation = ser.endianness() == - eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; - payload->length = (uint32_t)ser.getSerializedDataLength(); - return true; - } - } - - return false; -} - -template -bool TypeSupport::deserialize( - eprosima::fastrtps::rtps::SerializedPayload_t * payload, - void * data) -{ - assert(data); - assert(payload); - - auto ser_data = static_cast(data); - if (ser_data->is_cdr_buffer) { - auto buffer = static_cast(ser_data->data); - if (!buffer->reserve(payload->length)) { - return false; - } - memcpy(buffer->getBuffer(), payload->data, payload->length); - return true; - } - - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(payload->data), - payload->length); - eprosima::fastcdr::Cdr deser( - fastbuffer, - eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - return deserializeROSmessage(deser, ser_data->data); -} - -template -std::function TypeSupport::getSerializedSizeProvider(void * data) -{ - assert(data); - - auto ser_data = static_cast(data); - auto ser_size = [this, ser_data]() -> uint32_t - { - if (ser_data->is_cdr_buffer) { - auto ser = static_cast(ser_data->data); - return static_cast(ser->getSerializedDataLength()); - } - return static_cast(this->getEstimatedSerializedSize(ser_data->data)); - }; - return ser_size; -} - -} // namespace rmw_fastrtps_cpp +} // namespace rmw_fastrtps_dynamic_cpp -#endif // RMW_FASTRTPS_CPP__TYPESUPPORT_IMPL_HPP_ +#endif // RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp new file mode 100644 index 000000000..9bf08a1f7 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_client.hpp @@ -0,0 +1,50 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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_DYNAMIC_CPP__GET_CLIENT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_CLIENT_HPP_ + +#include "fastrtps/publisher/Publisher.h" +#include "fastrtps/subscriber/Subscriber.h" +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native FastRTPS publisher handle for the request. +/** + * The function returns `NULL` when either the client handle is `NULL` or + * when the client handle is from a different rmw implementation. + * + * \return native FastRTPS publisher handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_request_publisher(rmw_client_t * client); + +/// Return a native FastRTPS subscriber handle for the response. +/** + * The function returns `NULL` when either the client handle is `NULL` or + * when the client handle is from a different rmw implementation. + * + * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_response_subscriber(rmw_client_t * client); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_CLIENT_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp new file mode 100644 index 000000000..d32e07fe1 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp @@ -0,0 +1,38 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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_DYNAMIC_CPP__GET_PARTICIPANT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_PARTICIPANT_HPP_ + +#include "fastrtps/participant/Participant.h" +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native FastRTPS participant handle. +/** + * The function returns `NULL` when either the node handle is `NULL` or when the + * node handle is from a different rmw implementation. + * + * \return native FastRTPS participant handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Participant * +get_participant(rmw_node_t * node); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_PARTICIPANT_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp new file mode 100644 index 000000000..727bb4846 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp @@ -0,0 +1,38 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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_DYNAMIC_CPP__GET_PUBLISHER_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_PUBLISHER_HPP_ + +#include "fastrtps/publisher/Publisher.h" +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native FastRTPS publisher handle. +/** + * The function returns `NULL` when either the publisher handle is `NULL` or + * when the publisher handle is from a different rmw implementation. + * + * \return native FastRTPS publisher handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_publisher(rmw_publisher_t * publisher); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_PUBLISHER_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp new file mode 100644 index 000000000..6d9e3d74a --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_service.hpp @@ -0,0 +1,50 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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_DYNAMIC_CPP__GET_SERVICE_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_SERVICE_HPP_ + +#include "fastrtps/publisher/Publisher.h" +#include "fastrtps/subscriber/Subscriber.h" +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native FastRTPS subscriber handle for the request. +/** + * The function returns `NULL` when either the service handle is `NULL` or + * when the service handle is from a different rmw implementation. + * + * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_request_subscriber(rmw_service_t * service); + +/// Return a native FastRTPS publisher handle for the response. +/** + * The function returns `NULL` when either the service handle is `NULL` or + * when the service handle is from a different rmw implementation. + * + * \return native FastRTPS publisher handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Publisher * +get_response_publisher(rmw_service_t * service); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_SERVICE_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp new file mode 100644 index 000000000..f6614e7bf --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp @@ -0,0 +1,38 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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_DYNAMIC_CPP__GET_SUBSCRIBER_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_SUBSCRIBER_HPP_ + +#include "fastrtps/subscriber/Subscriber.h" +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native FastRTPS subscriber handle. +/** + * The function returns `NULL` when either the subscription handle is `NULL` or + * when the subscription handle is from a different rmw implementation. + * + * \return native FastRTPS subscriber handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastrtps::Subscriber * +get_subscriber(rmw_subscription_t * subscription); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_SUBSCRIBER_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/identifier.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/identifier.hpp new file mode 100644 index 000000000..3983ff99d --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/identifier.hpp @@ -0,0 +1,20 @@ +// 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 RMW_FASTRTPS_DYNAMIC_CPP__IDENTIFIER_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__IDENTIFIER_HPP_ + +extern const char * const eprosima_fastrtps_identifier; + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__IDENTIFIER_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/macros.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/macros.hpp similarity index 88% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/macros.hpp rename to rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/macros.hpp index c6a23a04a..1334aa47c 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/macros.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/macros.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__MACROS_HPP_ -#define RMW_FASTRTPS_CPP__MACROS_HPP_ +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__MACROS_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__MACROS_HPP_ #include #include @@ -33,4 +33,4 @@ } \ }; -#endif // RMW_FASTRTPS_CPP__MACROS_HPP_ +#endif // RMW_FASTRTPS_DYNAMIC_CPP__MACROS_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/visibility_control.h b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/visibility_control.h new file mode 100644 index 000000000..39a0eb6c7 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__VISIBILITY_CONTROL_H_ +#define RMW_FASTRTPS_DYNAMIC_CPP__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RMW_FASTRTPS_DYNAMIC_CPP_EXPORT __attribute__ ((dllexport)) + #define RMW_FASTRTPS_DYNAMIC_CPP_IMPORT __attribute__ ((dllimport)) + #else + #define RMW_FASTRTPS_DYNAMIC_CPP_EXPORT __declspec(dllexport) + #define RMW_FASTRTPS_DYNAMIC_CPP_IMPORT __declspec(dllimport) + #endif + #ifdef RMW_FASTRTPS_DYNAMIC_CPP_BUILDING_LIBRARY + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC RMW_FASTRTPS_DYNAMIC_CPP_EXPORT + #else + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC RMW_FASTRTPS_DYNAMIC_CPP_IMPORT + #endif + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC_TYPE RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC + #define RMW_FASTRTPS_DYNAMIC_CPP_LOCAL +#else + #define RMW_FASTRTPS_DYNAMIC_CPP_EXPORT __attribute__ ((visibility("default"))) + #define RMW_FASTRTPS_DYNAMIC_CPP_IMPORT + #if __GNUC__ >= 4 + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC __attribute__ ((visibility("default"))) + #define RMW_FASTRTPS_DYNAMIC_CPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC + #define RMW_FASTRTPS_DYNAMIC_CPP_LOCAL + #endif + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC_TYPE +#endif + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__VISIBILITY_CONTROL_H_ diff --git a/rmw_fastrtps_dynamic_cpp/package.xml b/rmw_fastrtps_dynamic_cpp/package.xml new file mode 100644 index 000000000..3c0e88ede --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/package.xml @@ -0,0 +1,44 @@ + + + + rmw_fastrtps_dynamic_cpp + 0.5.1 + Implement the ROS middleware interface using eProsima FastRTPS static code generation in C++. + Dirk Thomas + Apache License 2.0 + Ricardo González + + ament_cmake_ros + fastrtps_cmake_module + + ament_cmake + + fastcdr + fastrtps + fastrtps_cmake_module + rcutils + rmw + rmw_fastrtps_shared_cpp + rosidl_generator_c + rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp + + fastcdr + fastrtps + fastrtps_cmake_module + rcutils + rmw + rmw_fastrtps_shared_cpp + rosidl_generator_c + rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp + + ament_lint_auto + ament_lint_common + + rmw_implementation_packages + + + ament_cmake + + diff --git a/rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake b/rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake new file mode 100644 index 000000000..2db8a7b31 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake @@ -0,0 +1,24 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +# copied from rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake + +find_package(fastrtps_cmake_module REQUIRED) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +list(APPEND rmw_fastrtps_dynamic_cpp_INCLUDE_DIRS ${FastRTPS_INCLUDE_DIR}) +# specific order: dependents before dependencies +list(APPEND rmw_fastrtps_dynamic_cpp_LIBRARIES fastrtps fastcdr) diff --git a/rmw_fastrtps_cpp/src/client_service_common.cpp b/rmw_fastrtps_dynamic_cpp/src/client_service_common.cpp similarity index 94% rename from rmw_fastrtps_cpp/src/client_service_common.cpp rename to rmw_fastrtps_dynamic_cpp/src/client_service_common.cpp index 6535f7fde..7677079d3 100644 --- a/rmw_fastrtps_cpp/src/client_service_common.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/client_service_common.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "client_service_common.hpp" +#include +#include "client_service_common.hpp" #include "type_support_common.hpp" const void * diff --git a/rmw_fastrtps_cpp/src/client_service_common.hpp b/rmw_fastrtps_dynamic_cpp/src/client_service_common.hpp similarity index 95% rename from rmw_fastrtps_cpp/src/client_service_common.hpp rename to rmw_fastrtps_dynamic_cpp/src/client_service_common.hpp index f7878b2a4..2020ff589 100644 --- a/rmw_fastrtps_cpp/src/client_service_common.hpp +++ b/rmw_fastrtps_dynamic_cpp/src/client_service_common.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_dynamic_cpp/src/get_client.cpp b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp new file mode 100644 index 000000000..017fa3a94 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/get_client.cpp @@ -0,0 +1,49 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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/get_client.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastrtps::Publisher * +get_request_publisher(rmw_client_t * client) +{ + if (!client) { + return nullptr; + } + if (client->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(client->data); + return impl->request_publisher_; +} + +eprosima::fastrtps::Subscriber * +get_response_subscriber(rmw_client_t * client) +{ + if (!client) { + return nullptr; + } + if (client->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(client->data); + return impl->response_subscriber_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp new file mode 100644 index 000000000..ba7aa5c5e --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp @@ -0,0 +1,36 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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/get_participant.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastrtps::Participant * +get_participant(rmw_node_t * node) +{ + if (!node) { + return nullptr; + } + if (node->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(node->data); + return impl->participant; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp new file mode 100644 index 000000000..a60638c99 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/get_publisher.cpp @@ -0,0 +1,36 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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/get_publisher.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastrtps::Publisher * +get_publisher(rmw_publisher_t * publisher) +{ + if (!publisher) { + return nullptr; + } + if (publisher->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(publisher->data); + return impl->publisher_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_service.cpp b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp new file mode 100644 index 000000000..4eb691bc5 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/get_service.cpp @@ -0,0 +1,49 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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/get_service.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastrtps::Subscriber * +get_request_subscriber(rmw_service_t * service) +{ + if (!service) { + return nullptr; + } + if (service->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(service->data); + return impl->request_subscriber_; +} + +eprosima::fastrtps::Publisher * +get_response_publisher(rmw_service_t * service) +{ + if (!service) { + return nullptr; + } + if (service->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(service->data); + return impl->response_publisher_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp new file mode 100644 index 000000000..001249f2e --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/get_subscriber.cpp @@ -0,0 +1,36 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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/get_subscriber.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastrtps::Subscriber * +get_subscriber(rmw_subscription_t * subscription) +{ + if (!subscription) { + return nullptr; + } + if (subscription->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(subscription->data); + return impl->subscriber_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/identifier.cpp b/rmw_fastrtps_dynamic_cpp/src/identifier.cpp new file mode 100644 index 000000000..1c47288bc --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/identifier.cpp @@ -0,0 +1,17 @@ +// 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/identifier.hpp" + +const char * const eprosima_fastrtps_identifier = "rmw_fastrtps_dynamic_cpp"; diff --git a/rmw_fastrtps_dynamic_cpp/src/namespace_prefix.cpp b/rmw_fastrtps_dynamic_cpp/src/namespace_prefix.cpp new file mode 100644 index 000000000..179c06c13 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/namespace_prefix.cpp @@ -0,0 +1,53 @@ +// 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 +#include + +#include "namespace_prefix.hpp" + +extern "C" +{ +// static for internal linkage +const char * const ros_topic_prefix = "rt"; +const char * const ros_service_requester_prefix = "rq"; +const char * const ros_service_response_prefix = "rr"; + +const std::vector _ros_prefixes = +{ros_topic_prefix, ros_service_requester_prefix, ros_service_response_prefix}; +} // extern "C" + +/// Return the ROS specific prefix if it exists, otherwise "". +std::string +_get_ros_prefix_if_exists(const std::string & topic_name) +{ + for (const auto & prefix : _ros_prefixes) { + if (topic_name.rfind(prefix, 0) == 0 && topic_name.at(prefix.length()) == '/') { + return prefix; + } + } + return ""; +} + +/// Strip the ROS specific prefix if it exists from the topic name. +std::string +_strip_ros_prefix_if_exists(const std::string & topic_name) +{ + for (const auto & prefix : _ros_prefixes) { + if (topic_name.rfind(prefix, 0) == 0 && topic_name.at(prefix.length()) == '/') { + return topic_name.substr(prefix.length()); + } + } + return topic_name; +} diff --git a/rmw_fastrtps_dynamic_cpp/src/namespace_prefix.hpp b/rmw_fastrtps_dynamic_cpp/src/namespace_prefix.hpp new file mode 100644 index 000000000..a230f3cc9 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/namespace_prefix.hpp @@ -0,0 +1,37 @@ +// 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 NAMESPACE_PREFIX_HPP_ +#define NAMESPACE_PREFIX_HPP_ + +#include +#include + +extern "C" +{ +extern const char * const ros_topic_prefix; +extern const char * const ros_service_requester_prefix; +extern const char * const ros_service_response_prefix; + +extern const std::vector _ros_prefixes; +} // extern "C" + +/// Return the ROS specific prefix if it exists, otherwise "". +std::string +_get_ros_prefix_if_exists(const std::string & topic_name); + +/// Returns the topic name stripped of and ROS specific prefix if exists. +std::string +_strip_ros_prefix_if_exists(const std::string & topic_name); +#endif // NAMESPACE_PREFIX_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/src/qos.cpp b/rmw_fastrtps_dynamic_cpp/src/qos.cpp new file mode 100644 index 000000000..53b2b6262 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/qos.cpp @@ -0,0 +1,160 @@ +// 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 + +#include "qos.hpp" + +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/attributes/SubscriberAttributes.h" + +#include "rmw/error_handling.h" + +extern "C" +{ +bool +get_datareader_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastrtps::SubscriberAttributes & sattr) +{ + switch (qos_policies.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + sattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + sattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS history policy"); + return false; + } + + switch (qos_policies.reliability) { + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + sattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + sattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); + return false; + } + + switch (qos_policies.durability) { + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + sattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + sattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS durability policy"); + return false; + } + + if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) { + sattr.topic.historyQos.depth = static_cast(qos_policies.depth); + } + + // ensure the history depth is at least the requested queue size + assert(sattr.topic.historyQos.depth >= 0); + if ( + eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == sattr.topic.historyQos.kind && + static_cast(sattr.topic.historyQos.depth) < qos_policies.depth) + { + if (qos_policies.depth > (std::numeric_limits::max)()) { + RMW_SET_ERROR_MSG( + "failed to set history depth since the requested queue size exceeds the DDS type"); + return false; + } + sattr.topic.historyQos.depth = static_cast(qos_policies.depth); + } + + return true; +} + +bool +get_datawriter_qos( + const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr) +{ + switch (qos_policies.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS history policy"); + return false; + } + + switch (qos_policies.durability) { + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + pattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + pattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS durability policy"); + return false; + } + + switch (qos_policies.reliability) { + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + pattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + pattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); + return false; + } + + if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) { + pattr.topic.historyQos.depth = static_cast(qos_policies.depth); + } + + // ensure the history depth is at least the requested queue size + assert(pattr.topic.historyQos.depth >= 0); + if ( + eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == pattr.topic.historyQos.kind && + static_cast(pattr.topic.historyQos.depth) < qos_policies.depth) + { + if (qos_policies.depth > (std::numeric_limits::max)()) { + RMW_SET_ERROR_MSG( + "failed to set history depth since the requested queue size exceeds the DDS type"); + return false; + } + pattr.topic.historyQos.depth = static_cast(qos_policies.depth); + } + + return true; +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/qos.hpp b/rmw_fastrtps_dynamic_cpp/src/qos.hpp new file mode 100644 index 000000000..c490929b6 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/qos.hpp @@ -0,0 +1,45 @@ +// 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 QOS_HPP_ +#define QOS_HPP_ + +#include "rmw/rmw.h" + +namespace eprosima +{ +namespace fastrtps +{ +class SubscriberAttributes; +class PublisherAttributes; +} // namespace fastrtps +} // namespace eprosima + +extern "C" +{ +RMW_LOCAL +bool +get_datareader_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastrtps::SubscriberAttributes & sattr); + +RMW_LOCAL +bool +get_datawriter_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastrtps::PublisherAttributes & pattr); +} +// extern "C" + +#endif // QOS_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp new file mode 100644 index 000000000..518109e20 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -0,0 +1,256 @@ +// 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 + +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/rmw.h" + +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" + +#include "rosidl_typesupport_introspection_c/identifier.h" + +#include "client_service_common.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "namespace_prefix.hpp" +#include "qos.hpp" +#include "type_support_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +extern "C" +{ +rmw_client_t * +rmw_create_client( + const rmw_node_t * node, + const rosidl_service_type_support_t * type_supports, + const char * service_name, const rmw_qos_profile_t * qos_policies) +{ + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return nullptr; + } + + if (node->implementation_identifier != eprosima_fastrtps_identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return nullptr; + } + + if (!service_name || strlen(service_name) == 0) { + RMW_SET_ERROR_MSG("client topic is null or empty string"); + return nullptr; + } + + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_profile is null"); + return nullptr; + } + + auto impl = static_cast(node->data); + if (!impl) { + RMW_SET_ERROR_MSG("node impl is null"); + return nullptr; + } + + Participant * participant = impl->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + type_support = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + CustomClientInfo * info = nullptr; + eprosima::fastrtps::SubscriberAttributes subscriberParam; + eprosima::fastrtps::PublisherAttributes publisherParam; + rmw_client_t * rmw_client = nullptr; + + info = new CustomClientInfo(); + info->participant_ = participant; + info->typesupport_identifier_ = type_support->typesupport_identifier; + + const void * untyped_request_members; + const void * untyped_response_members; + + untyped_request_members = + get_request_ptr(type_support->data, info->typesupport_identifier_); + untyped_response_members = get_response_ptr(type_support->data, + info->typesupport_identifier_); + + std::string request_type_name = _create_type_name(untyped_request_members, "srv", + info->typesupport_identifier_); + std::string response_type_name = _create_type_name(untyped_response_members, "srv", + 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_); + _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_); + _register_type(participant, info->response_type_support_); + } + + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + subscriberParam.topic.topicDataType = response_type_name; + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!qos_policies->avoid_ros_namespace_conventions) { + subscriberParam.topic.topicName = std::string(ros_service_response_prefix) + service_name; + } else { + subscriberParam.topic.topicName = service_name; + } + subscriberParam.topic.topicName += "Reply"; + + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + publisherParam.topic.topicDataType = request_type_name; + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!qos_policies->avoid_ros_namespace_conventions) { + publisherParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name; + } else { + publisherParam.topic.topicName = service_name; + } + publisherParam.topic.topicName += "Request"; + + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "************ Client Details *********") + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Sub Topic %s", subscriberParam.topic.topicName.c_str()) + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Pub Topic %s", publisherParam.topic.topicName.c_str()) + RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "***********") + + // Create Client Subscriber and set QoS + if (!get_datareader_qos(*qos_policies, subscriberParam)) { + RMW_SET_ERROR_MSG("failed to get datareader qos"); + goto fail; + } + info->listener_ = new ClientListener(info); + info->response_subscriber_ = + Domain::createSubscriber(participant, subscriberParam, info->listener_); + if (!info->response_subscriber_) { + RMW_SET_ERROR_MSG("create_client() could not create subscriber"); + goto fail; + } + + // Create Client Subscriber and set QoS + if (!get_datawriter_qos(*qos_policies, publisherParam)) { + RMW_SET_ERROR_MSG("failed to get datawriter qos"); + goto fail; + } + info->request_publisher_ = + Domain::createPublisher(participant, publisherParam, nullptr); + if (!info->request_publisher_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + goto fail; + } + + info->writer_guid_ = info->request_publisher_->getGuid(); + + rmw_client = rmw_client_allocate(); + if (!rmw_client) { + RMW_SET_ERROR_MSG("failed to allocate memory for client"); + goto fail; + } + + rmw_client->implementation_identifier = eprosima_fastrtps_identifier; + rmw_client->data = info; + rmw_client->service_name = reinterpret_cast( + rmw_allocate(strlen(service_name) + 1)); + if (!rmw_client->service_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for client name"); + goto fail; + } + memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); + + return rmw_client; + +fail: + if (info != nullptr) { + if (info->request_publisher_ != nullptr) { + Domain::removePublisher(info->request_publisher_); + } + + if (info->response_subscriber_ != nullptr) { + Domain::removeSubscriber(info->response_subscriber_); + } + + if (info->listener_ != nullptr) { + delete info->listener_; + } + + if (impl) { + if (info->request_type_support_ != nullptr) { + rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); + } + + if (info->response_type_support_ != nullptr) { + rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); + } + } else { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "leaking type support objects because node impl is null") + } + + delete info; + info = nullptr; + } + + if (nullptr != rmw_client) { + if (rmw_client->service_name != nullptr) { + rmw_free(const_cast(rmw_client->service_name)); + rmw_client->service_name = nullptr; + } + rmw_client_free(rmw_client); + } + + return nullptr; +} + +rmw_ret_t +rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_client( + eprosima_fastrtps_identifier, node, client); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp new file mode 100644 index 000000000..3c5cab0f4 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_compare_gids_equal.cpp @@ -0,0 +1,33 @@ +// 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 "fastrtps/rtps/common/Guid.h" + +#include "rmw/rmw.h" +#include "rmw/error_handling.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * result) +{ + return rmw_fastrtps_shared_cpp::__rmw_compare_gids_equal( + eprosima_fastrtps_identifier, gid1, gid2, result); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_count.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_count.cpp new file mode 100644 index 000000000..c5dacef08 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_count.cpp @@ -0,0 +1,50 @@ +// 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 +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_count_publishers( + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + return rmw_fastrtps_shared_cpp::__rmw_count_publishers( + eprosima_fastrtps_identifier, node, topic_name, count); +} + +rmw_ret_t +rmw_count_subscribers( + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + return rmw_fastrtps_shared_cpp::__rmw_count_subscribers( + eprosima_fastrtps_identifier, node, topic_name, count); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_get_gid_for_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_get_gid_for_publisher.cpp new file mode 100644 index 000000000..0bade66ec --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_get_gid_for_publisher.cpp @@ -0,0 +1,31 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_gid_for_publisher( + eprosima_fastrtps_identifier, publisher, gid); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_get_implementation_identifier.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_get_implementation_identifier.cpp new file mode 100644 index 000000000..94434b372 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_get_implementation_identifier.cpp @@ -0,0 +1,26 @@ +// 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/rmw.h" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +const char * +rmw_get_implementation_identifier() +{ + return eprosima_fastrtps_identifier; +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_guard_condition.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_guard_condition.cpp new file mode 100644 index 000000000..202ecc618 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_guard_condition.cpp @@ -0,0 +1,37 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_guard_condition_t * +rmw_create_guard_condition() +{ + return rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( + eprosima_fastrtps_identifier); +} + +rmw_ret_t +rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + guard_condition); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp new file mode 100644 index 000000000..fe9b50da0 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -0,0 +1,24 @@ +// 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/rmw.h" + +extern "C" +{ +rmw_ret_t +rmw_init() +{ + return RMW_RET_OK; +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_logging.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_logging.cpp new file mode 100644 index 000000000..6f793ffdc --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_logging.cpp @@ -0,0 +1,29 @@ +// 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/rmw.h" +#include "rmw/error_handling.h" + +#include "rcutils/logging_macros.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_set_log_severity(rmw_log_severity_t severity) +{ + return rmw_fastrtps_shared_cpp::__rmw_set_log_severity(severity); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp new file mode 100644 index 000000000..77d73c191 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -0,0 +1,57 @@ +// 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 +#include +#include +#include + +#include "rcutils/filesystem.h" +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_node_t * +rmw_create_node( + const char * name, + const char * namespace_, + size_t domain_id, + const rmw_node_security_options_t * security_options) +{ + return rmw_fastrtps_shared_cpp::__rmw_create_node( + eprosima_fastrtps_identifier, name, namespace_, domain_id, security_options); +} + +rmw_ret_t +rmw_destroy_node(rmw_node_t * node) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_node( + eprosima_fastrtps_identifier, node); +} + +const rmw_guard_condition_t * +rmw_node_get_graph_guard_condition(const rmw_node_t * node) +{ + return rmw_fastrtps_shared_cpp::__rmw_node_get_graph_guard_condition( + node); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp new file mode 100644 index 000000000..f957ae4c6 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp @@ -0,0 +1,41 @@ +// 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 + +#include "rcutils/allocator.h" +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/allocators.h" +#include "rmw/convert_rcutils_ret_to_rmw_ret.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/sanity_checks.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_node_names( + const rmw_node_t * node, + rcutils_string_array_t * node_names) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_node_names( + eprosima_fastrtps_identifier, node, node_names); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publish.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publish.cpp new file mode 100644 index 000000000..6a5e414b1 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publish.cpp @@ -0,0 +1,42 @@ +// 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 "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) +{ + return rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, publisher, ros_message); +} + +rmw_ret_t +rmw_publish_serialized_message( + const rmw_publisher_t * publisher, const rmw_serialized_message_t * serialized_message) +{ + return rmw_fastrtps_shared_cpp::__rmw_publish_serialized_message( + eprosima_fastrtps_identifier, publisher, serialized_message); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp new file mode 100644 index 000000000..505d5f710 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -0,0 +1,190 @@ +// 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 + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "namespace_prefix.hpp" +#include "qos.hpp" +#include "type_support_common.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +extern "C" +{ +rmw_publisher_t * +rmw_create_publisher( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, const rmw_qos_profile_t * qos_policies) +{ + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return nullptr; + } + + if (node->implementation_identifier != eprosima_fastrtps_identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return nullptr; + } + + if (!topic_name || strlen(topic_name) == 0) { + RMW_SET_ERROR_MSG("publisher topic is null or empty string"); + return nullptr; + } + + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_profile is null"); + return nullptr; + } + + auto impl = static_cast(node->data); + if (!impl) { + RMW_SET_ERROR_MSG("node impl is null"); + return nullptr; + } + + Participant * participant = impl->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + CustomPublisherInfo * info = nullptr; + rmw_publisher_t * rmw_publisher = nullptr; + eprosima::fastrtps::PublisherAttributes publisherParam; + const eprosima::fastrtps::rtps::GUID_t * guid = nullptr; + + // Load default XML profile. + Domain::getDefaultPublisherAttributes(publisherParam); + + // TODO(karsten1987) Verify consequences for std::unique_ptr? + info = new CustomPublisherInfo(); + info->typesupport_identifier_ = type_support->typesupport_identifier; + + std::string type_name = _create_type_name( + type_support->data, "msg", 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_); + _register_type(participant, info->type_support_); + } + + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + publisherParam.topic.topicDataType = type_name; + if (!qos_policies->avoid_ros_namespace_conventions) { + publisherParam.topic.topicName = std::string(ros_topic_prefix) + topic_name; + } else { + publisherParam.topic.topicName = topic_name; + } + + // 1 Heartbeat every 10ms + // publisherParam.times.heartbeatPeriod.seconds = 0; + // publisherParam.times.heartbeatPeriod.fraction = 42949673; + + // 300000 bytes each 10ms + // ThroughputControllerDescriptor throughputController{3000000, 10}; + // publisherParam.throughputController = throughputController; + + if (!get_datawriter_qos(*qos_policies, publisherParam)) { + RMW_SET_ERROR_MSG("failed to get datawriter qos"); + goto fail; + } + + info->publisher_ = Domain::createPublisher(participant, publisherParam, nullptr); + + if (!info->publisher_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + goto fail; + } + + info->publisher_gid.implementation_identifier = eprosima_fastrtps_identifier; + static_assert( + sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, + "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_dynamic_cpp GID implementation." + ); + + memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); + guid = &info->publisher_->getGuid(); + if (!guid) { + RMW_SET_ERROR_MSG("no guid found for publisher"); + goto fail; + } + memcpy(info->publisher_gid.data, guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + + rmw_publisher = rmw_publisher_allocate(); + if (!rmw_publisher) { + RMW_SET_ERROR_MSG("failed to allocate publisher"); + goto fail; + } + rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; + rmw_publisher->data = info; + rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + + if (!rmw_publisher->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + goto fail; + } + + memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); + return rmw_publisher; + +fail: + if (info) { + if (info->type_support_ != nullptr) { + delete info->type_support_; + } + delete info; + } + + if (rmw_publisher) { + rmw_publisher_free(rmw_publisher); + } + + return nullptr; +} + +rmw_ret_t +rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + eprosima_fastrtps_identifier, node, publisher); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp new file mode 100644 index 000000000..9b3a12b91 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp @@ -0,0 +1,45 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_send_request( + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id) +{ + return rmw_fastrtps_shared_cpp::__rmw_send_request( + eprosima_fastrtps_identifier, client, ros_request, sequence_id); +} + +rmw_ret_t +rmw_take_request( + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_request, + bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_request( + eprosima_fastrtps_identifier, service, request_header, ros_request, taken); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp new file mode 100644 index 000000000..7aeba853e --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp @@ -0,0 +1,44 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_take_response( + const rmw_client_t * client, + rmw_request_id_t * request_header, + void * ros_response, + bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_response( + eprosima_fastrtps_identifier, client, request_header, ros_response, taken); +} + +rmw_ret_t +rmw_send_response( + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) +{ + return rmw_fastrtps_shared_cpp::__rmw_send_response( + eprosima_fastrtps_identifier, service, request_header, ros_response); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp new file mode 100644 index 000000000..b47cc4146 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_serialize.cpp @@ -0,0 +1,89 @@ +// 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/FastBuffer.h" + +#include "rmw/error_handling.h" +#include "rmw/serialized_message.h" +#include "rmw/rmw.h" + +#include "./type_support_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_serialize( + const void * ros_message, + const rosidl_message_type_support_t * type_support, + rmw_serialized_message_t * serialized_message) +{ + const rosidl_message_type_support_t * ts = get_message_typesupport_handle( + type_support, rosidl_typesupport_introspection_c__identifier); + if (!ts) { + ts = get_message_typesupport_handle( + type_support, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!ts) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return RMW_RET_ERROR; + } + } + + auto tss = _create_message_type_support(ts->data, ts->typesupport_identifier); + auto data_length = tss->getEstimatedSerializedSize(ros_message); + 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"); + return RMW_RET_ERROR; + } + } + + eprosima::fastcdr::FastBuffer buffer(serialized_message->buffer, data_length); + eprosima::fastcdr::Cdr ser( + buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + + auto ret = tss->serializeROSmessage(ros_message, ser); + serialized_message->buffer_length = data_length; + serialized_message->buffer_capacity = data_length; + delete tss; + return ret == true ? RMW_RET_OK : RMW_RET_ERROR; +} + +rmw_ret_t +rmw_deserialize( + const rmw_serialized_message_t * serialized_message, + const rosidl_message_type_support_t * type_support, + void * ros_message) +{ + const rosidl_message_type_support_t * ts = get_message_typesupport_handle( + type_support, rosidl_typesupport_introspection_c__identifier); + if (!ts) { + ts = get_message_typesupport_handle( + type_support, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!ts) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return RMW_RET_ERROR; + } + } + + auto tss = _create_message_type_support(ts->data, ts->typesupport_identifier); + eprosima::fastcdr::FastBuffer buffer( + 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); + delete tss; + return ret == true ? RMW_RET_OK : RMW_RET_ERROR; +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp new file mode 100644 index 000000000..342396033 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -0,0 +1,257 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_c/identifier.h" + +#include "type_support_common.hpp" +#include "client_service_common.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "namespace_prefix.hpp" +#include "qos.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; +using CustomParticipantInfo = CustomParticipantInfo; + +extern "C" +{ +rmw_service_t * +rmw_create_service( + const rmw_node_t * node, + const rosidl_service_type_support_t * type_supports, + const char * service_name, const rmw_qos_profile_t * qos_policies) +{ + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return nullptr; + } + + if (node->implementation_identifier != eprosima_fastrtps_identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return nullptr; + } + + if (!service_name || strlen(service_name) == 0) { + RMW_SET_ERROR_MSG("service topic is null or empty string"); + return nullptr; + } + + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_profile is null"); + return nullptr; + } + + const CustomParticipantInfo * impl = static_cast(node->data); + if (!impl) { + RMW_SET_ERROR_MSG("node impl is null"); + return nullptr; + } + + Participant * participant = impl->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + type_support = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + CustomServiceInfo * info = nullptr; + eprosima::fastrtps::SubscriberAttributes subscriberParam; + eprosima::fastrtps::PublisherAttributes publisherParam; + rmw_service_t * rmw_service = nullptr; + + info = new CustomServiceInfo(); + info->participant_ = participant; + info->typesupport_identifier_ = type_support->typesupport_identifier; + + const void * untyped_request_members; + const void * untyped_response_members; + + untyped_request_members = + get_request_ptr(type_support->data, info->typesupport_identifier_); + untyped_response_members = get_response_ptr(type_support->data, + info->typesupport_identifier_); + + std::string request_type_name = _create_type_name(untyped_request_members, "srv", + info->typesupport_identifier_); + std::string response_type_name = _create_type_name(untyped_response_members, "srv", + 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_); + _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_); + _register_type(participant, info->response_type_support_); + } + + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + subscriberParam.topic.topicDataType = request_type_name; + if (!qos_policies->avoid_ros_namespace_conventions) { + subscriberParam.topic.topicName = std::string(ros_service_requester_prefix) + service_name; + } else { + subscriberParam.topic.topicName = service_name; + } + subscriberParam.topic.topicName += "Request"; + + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + publisherParam.topic.topicDataType = response_type_name; + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + if (!qos_policies->avoid_ros_namespace_conventions) { + publisherParam.topic.topicName = std::string(ros_service_response_prefix) + service_name; + } else { + publisherParam.topic.topicName = service_name; + } + publisherParam.topic.topicName += "Reply"; + + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "************ Service Details *********") + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Sub Topic %s", subscriberParam.topic.topicName.c_str()) + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Pub Topic %s", publisherParam.topic.topicName.c_str()) + RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "***********") + + // Create Service Subscriber and set QoS + if (!get_datareader_qos(*qos_policies, subscriberParam)) { + RMW_SET_ERROR_MSG("failed to get datareader qos"); + goto fail; + } + info->listener_ = new ServiceListener(info); + info->request_subscriber_ = + Domain::createSubscriber(participant, subscriberParam, info->listener_); + if (!info->request_subscriber_) { + RMW_SET_ERROR_MSG("create_client() could not create subscriber"); + goto fail; + } + + // Create Service Publisher and set QoS + if (!get_datawriter_qos(*qos_policies, publisherParam)) { + RMW_SET_ERROR_MSG("failed to get datawriter qos"); + goto fail; + } + info->response_publisher_ = + Domain::createPublisher(participant, publisherParam, nullptr); + if (!info->response_publisher_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + goto fail; + } + + rmw_service = rmw_service_allocate(); + if (!rmw_service) { + RMW_SET_ERROR_MSG("failed to allocate memory for service"); + goto fail; + } + rmw_service->implementation_identifier = eprosima_fastrtps_identifier; + rmw_service->data = info; + rmw_service->service_name = reinterpret_cast( + rmw_allocate(strlen(service_name) + 1)); + if (!rmw_service->service_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for service name"); + goto fail; + } + memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); + + return rmw_service; + +fail: + + if (info) { + if (info->response_publisher_) { + Domain::removePublisher(info->response_publisher_); + } + + if (info->request_subscriber_) { + Domain::removeSubscriber(info->request_subscriber_); + } + + if (info->listener_) { + delete info->listener_; + } + + if (info->request_type_support_) { + rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); + } + + if (info->response_type_support_) { + rmw_fastrtps_shared_cpp::_unregister_type(participant, info->response_type_support_); + } + + delete info; + } + + if (rmw_service && rmw_service->service_name) { + rmw_free(const_cast(rmw_service->service_name)); + rmw_service->service_name = nullptr; + } + rmw_service_free(rmw_service); + + return nullptr; +} + +rmw_ret_t +rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_service( + eprosima_fastrtps_identifier, node, service); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service_names_and_types.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service_names_and_types.cpp new file mode 100644 index 000000000..d8e839fa9 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service_names_and_types.cpp @@ -0,0 +1,37 @@ +// 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/allocators.h" +#include "rmw/convert_rcutils_ret_to_rmw_ret.h" +#include "rmw/error_handling.h" +#include "rmw/get_service_names_and_types.h" +#include "rmw/names_and_types.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_service_names_and_types( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * service_names_and_types) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_service_names_and_types( + eprosima_fastrtps_identifier, node, allocator, service_names_and_types); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service_server_is_available.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service_server_is_available.cpp new file mode 100644 index 000000000..d4d739ac0 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service_server_is_available.cpp @@ -0,0 +1,36 @@ +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_service_server_is_available( + const rmw_node_t * node, + const rmw_client_t * client, + bool * is_available) +{ + return rmw_fastrtps_shared_cpp::__rmw_service_server_is_available( + eprosima_fastrtps_identifier, node, client, is_available); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp new file mode 100644 index 000000000..6253a4598 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -0,0 +1,174 @@ +// 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 +#include + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" + +#include "fastrtps/participant/Participant.h" +#include "fastrtps/subscriber/Subscriber.h" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +#include "namespace_prefix.hpp" +#include "qos.hpp" +#include "type_support_common.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +extern "C" +{ +rmw_subscription_t * +rmw_create_subscription( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, const rmw_qos_profile_t * qos_policies, bool ignore_local_publications) +{ + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return nullptr; + } + + if (node->implementation_identifier != eprosima_fastrtps_identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return nullptr; + } + + if (!topic_name || strlen(topic_name) == 0) { + RMW_SET_ERROR_MSG("subscription topic is null or empty string"); + return nullptr; + } + + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_profile is null"); + return nullptr; + } + + auto impl = static_cast(node->data); + if (!impl) { + RMW_SET_ERROR_MSG("node impl is null"); + return nullptr; + } + + Participant * participant = impl->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + (void)ignore_local_publications; + CustomSubscriberInfo * info = nullptr; + rmw_subscription_t * rmw_subscription = nullptr; + eprosima::fastrtps::SubscriberAttributes subscriberParam; + + // Load default XML profile. + Domain::getDefaultSubscriberAttributes(subscriberParam); + + info = new CustomSubscriberInfo(); + info->typesupport_identifier_ = type_support->typesupport_identifier; + + std::string type_name = _create_type_name( + type_support->data, "msg", 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_); + _register_type(participant, info->type_support_); + } + + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + subscriberParam.topic.topicDataType = type_name; + if (!qos_policies->avoid_ros_namespace_conventions) { + subscriberParam.topic.topicName = std::string(ros_topic_prefix) + topic_name; + } else { + subscriberParam.topic.topicName = topic_name; + } + + if (!get_datareader_qos(*qos_policies, subscriberParam)) { + RMW_SET_ERROR_MSG("failed to get datareader qos"); + goto fail; + } + + info->listener_ = new SubListener(info); + info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); + + if (!info->subscriber_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); + goto fail; + } + + rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("failed to allocate subscription"); + goto fail; + } + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; + rmw_subscription->data = info; + rmw_subscription->topic_name = + reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + + if (!rmw_subscription->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); + goto fail; + } + + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + return rmw_subscription; + +fail: + + if (info != nullptr) { + if (info->type_support_ != nullptr) { + delete info->type_support_; + } + delete info; + } + + if (rmw_subscription) { + rmw_subscription_free(rmw_subscription); + } + + return nullptr; +} + +rmw_ret_t +rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, node, subscription); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_take.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_take.cpp new file mode 100644 index 000000000..ce9192fa4 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_take.cpp @@ -0,0 +1,64 @@ +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/serialized_message.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_take(const rmw_subscription_t * subscription, void * ros_message, bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take( + eprosima_fastrtps_identifier, subscription, ros_message, taken); +} + +rmw_ret_t +rmw_take_with_info( + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_with_info( + eprosima_fastrtps_identifier, subscription, ros_message, taken, message_info); +} + +rmw_ret_t +rmw_take_serialized_message( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_serialized_message( + eprosima_fastrtps_identifier, subscription, serialized_message, taken); +} + +rmw_ret_t +rmw_take_serialized_message_with_info( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_serialized_message_with_info( + eprosima_fastrtps_identifier, subscription, serialized_message, taken, message_info); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_topic_names_and_types.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_topic_names_and_types.cpp new file mode 100644 index 000000000..1e104a46e --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_topic_names_and_types.cpp @@ -0,0 +1,49 @@ +// 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 +#include +#include + +#include "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/allocators.h" +#include "rmw/convert_rcutils_ret_to_rmw_ret.h" +#include "rmw/error_handling.h" +#include "rmw/get_topic_names_and_types.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/names_and_types.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_topic_names_and_types( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_topic_names_and_types( + eprosima_fastrtps_identifier, node, allocator, no_demangle, topic_names_and_types); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_trigger_guard_condition.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_trigger_guard_condition.cpp new file mode 100644 index 000000000..8e0a5c37c --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_trigger_guard_condition.cpp @@ -0,0 +1,30 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition_handle) +{ + return rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, guard_condition_handle); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_wait.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_wait.cpp new file mode 100644 index 000000000..5ec82be64 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_wait.cpp @@ -0,0 +1,34 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_wait( + rmw_subscriptions_t * subscriptions, + rmw_guard_conditions_t * guard_conditions, + rmw_services_t * services, + rmw_clients_t * clients, + rmw_wait_set_t * wait_set, + const rmw_time_t * wait_timeout) +{ + return rmw_fastrtps_shared_cpp::__rmw_wait( + subscriptions, guard_conditions, services, clients, wait_set, wait_timeout); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_wait_set.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_wait_set.cpp new file mode 100644 index 000000000..7af1f2398 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_wait_set.cpp @@ -0,0 +1,38 @@ +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_wait_set_t * +rmw_create_wait_set(size_t max_conditions) +{ + return rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + eprosima_fastrtps_identifier, max_conditions); +} + +rmw_ret_t +rmw_destroy_wait_set(rmw_wait_set_t * wait_set) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + eprosima_fastrtps_identifier, wait_set); +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.cpp b/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.cpp new file mode 100644 index 000000000..9e1994806 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.cpp @@ -0,0 +1,36 @@ +// 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 new file mode 100644 index 000000000..73006fafc --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/ros_message_serialization.hpp @@ -0,0 +1,42 @@ +// 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_dynamic_cpp/src/type_support_common.cpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp new file mode 100644 index 000000000..3d546a067 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.cpp @@ -0,0 +1,90 @@ +// 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/error_handling.h" + +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" + +#include "rosidl_typesupport_introspection_c/identifier.h" + +#include "type_support_common.hpp" + +bool +using_introspection_c_typesupport(const char * typesupport_identifier) +{ + return typesupport_identifier == rosidl_typesupport_introspection_c__identifier; +} + +bool +using_introspection_cpp_typesupport(const char * typesupport_identifier) +{ + return 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, + rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport) +{ + eprosima::fastrtps::Domain::registerType(participant, typed_typesupport); +} diff --git a/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp new file mode 100644 index 000000000..2469d92e9 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp @@ -0,0 +1,119 @@ +// 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 TYPE_SUPPORT_COMMON_HPP_ +#define TYPE_SUPPORT_COMMON_HPP_ + +#include + +#include "fastrtps/Domain.h" +#include "fastrtps/participant/Participant.h" + +#include "rmw/error_handling.h" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +#include "rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp" +#include "rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp" + +#include "rosidl_typesupport_introspection_c/visibility_control.h" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +using MessageTypeSupport_c = rmw_fastrtps_dynamic_cpp::MessageTypeSupport< + rosidl_typesupport_introspection_c__MessageMembers +>; +using MessageTypeSupport_cpp = rmw_fastrtps_dynamic_cpp::MessageTypeSupport< + rosidl_typesupport_introspection_cpp::MessageMembers +>; +using TypeSupport_c = rmw_fastrtps_dynamic_cpp::TypeSupport< + rosidl_typesupport_introspection_c__MessageMembers +>; +using TypeSupport_cpp = rmw_fastrtps_dynamic_cpp::TypeSupport< + rosidl_typesupport_introspection_cpp::MessageMembers +>; + +using RequestTypeSupport_c = rmw_fastrtps_dynamic_cpp::RequestTypeSupport< + rosidl_typesupport_introspection_c__ServiceMembers, + rosidl_typesupport_introspection_c__MessageMembers +>; +using RequestTypeSupport_cpp = rmw_fastrtps_dynamic_cpp::RequestTypeSupport< + rosidl_typesupport_introspection_cpp::ServiceMembers, + rosidl_typesupport_introspection_cpp::MessageMembers +>; + +using ResponseTypeSupport_c = rmw_fastrtps_dynamic_cpp::ResponseTypeSupport< + rosidl_typesupport_introspection_c__ServiceMembers, + rosidl_typesupport_introspection_c__MessageMembers +>; +using ResponseTypeSupport_cpp = rmw_fastrtps_dynamic_cpp::ResponseTypeSupport< + rosidl_typesupport_introspection_cpp::ServiceMembers, + rosidl_typesupport_introspection_cpp::MessageMembers +>; + +bool +using_introspection_c_typesupport(const char * typesupport_identifier); + +bool +using_introspection_cpp_typesupport(const char * typesupport_identifier); + +template +ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL +inline std::string +_create_type_name( + const void * untyped_members, + const std::string & sep) +{ + auto members = static_cast(untyped_members); + if (!members) { + RMW_SET_ERROR_MSG("members handle is null"); + return ""; + } + return + std::string(members->package_name_) + "::" + sep + "::dds_::" + members->message_name_ + "_"; +} + +ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL +inline std::string +_create_type_name( + const void * untyped_members, + const std::string & sep, + const char * typesupport) +{ + if (using_introspection_c_typesupport(typesupport)) { + return _create_type_name( + untyped_members, sep); + } else if (using_introspection_cpp_typesupport(typesupport)) { + return _create_type_name( + untyped_members, sep); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + 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, + rmw_fastrtps_shared_cpp::TypeSupport * typed_typesupport); + +#endif // TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt new file mode 100644 index 000000000..df358c6e4 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -0,0 +1,107 @@ +# 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. + +cmake_minimum_required(VERSION 3.5) + +project(rmw_fastrtps_shared_cpp) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_ros REQUIRED) + +find_package(rcutils REQUIRED) + +find_package(fastrtps_cmake_module REQUIRED) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +find_package(rmw REQUIRED) +include_directories(include) + +add_library(rmw_fastrtps_shared_cpp + src/demangle.cpp + src/namespace_prefix.cpp + src/qos.cpp + src/rmw_client.cpp + src/rmw_compare_gids_equal.cpp + src/rmw_count.cpp + src/rmw_get_gid_for_publisher.cpp + src/rmw_guard_condition.cpp + src/rmw_logging.cpp + src/rmw_node.cpp + src/rmw_node_names.cpp + src/rmw_publish.cpp + src/rmw_publisher.cpp + src/rmw_request.cpp + src/rmw_response.cpp + src/rmw_service.cpp + src/rmw_service_names_and_types.cpp + src/rmw_service_server_is_available.cpp + src/rmw_subscription.cpp + src/rmw_take.cpp + src/rmw_topic_names_and_types.cpp + src/rmw_trigger_guard_condition.cpp + src/rmw_wait.cpp + src/rmw_wait_set.cpp + src/TypeSupport_impl.cpp +) +target_link_libraries(rmw_fastrtps_shared_cpp + fastcdr fastrtps) + +# specific order: dependents before dependencies +ament_target_dependencies(rmw_fastrtps_shared_cpp + "rcutils" + "rmw" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} +PRIVATE "RMW_FASTRTPS_SHARED_CPP_BUILDING_LIBRARY") + +# specific order: dependents before dependencies +ament_export_include_directories(include) +ament_export_libraries(rmw_fastrtps_shared_cpp) + +ament_export_dependencies(rcutils) +ament_export_dependencies(rmw) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package( + CONFIG_EXTRAS_POST "rmw_fastrtps_shared_cpp-extras.cmake" +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS rmw_fastrtps_shared_cpp + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/MessageTypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/MessageTypeSupport.hpp new file mode 100644 index 000000000..04632e353 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/MessageTypeSupport.hpp @@ -0,0 +1,38 @@ +// 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 RMW_FASTRTPS_SHARED_CPP__MESSAGETYPESUPPORT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__MESSAGETYPESUPPORT_HPP_ + +#include +#include + +#include +#include + +#include "TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +template +class MessageTypeSupport : public TypeSupport +{ +public: + explicit MessageTypeSupport(const MembersType * members); +}; + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__MESSAGETYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/ServiceTypeSupport.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/ServiceTypeSupport.hpp new file mode 100644 index 000000000..ac62bf2e1 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/ServiceTypeSupport.hpp @@ -0,0 +1,52 @@ +// 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 RMW_FASTRTPS_SHARED_CPP__SERVICETYPESUPPORT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__SERVICETYPESUPPORT_HPP_ + +#include +#include +#include + +#include "TypeSupport.hpp" + +struct CustomServiceInfo; + +namespace rmw_fastrtps_shared_cpp +{ + +template +class ServiceTypeSupport : public TypeSupport +{ +protected: + ServiceTypeSupport(); +}; + +template +class RequestTypeSupport : public ServiceTypeSupport +{ +public: + explicit RequestTypeSupport(const ServiceMembersType * members); +}; + +template +class ResponseTypeSupport : public ServiceTypeSupport +{ +public: + explicit ResponseTypeSupport(const ServiceMembersType * members); +}; + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__SERVICETYPESUPPORT_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 new file mode 100644 index 000000000..212e6120c --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -0,0 +1,86 @@ +// 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 RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include "rcutils/logging_macros.h" + +#include "./visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +// Publishers write method will receive a pointer to this struct +struct SerializedData +{ + bool is_cdr_buffer; // Whether next field is a pointer to a Cdr or to a plain ros message + void * data; +}; + +class TypeSupport : public eprosima::fastrtps::TopicDataType +{ +public: + virtual size_t getEstimatedSerializedSize(const void * ros_message) = 0; + + virtual bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser) = 0; + + virtual bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message) = 0; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool serialize(void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload); + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t * payload, void * data); + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + std::function getSerializedSizeProvider(void * data); + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void * createData(); + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void deleteData(void * data); + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + virtual ~TypeSupport() {} + +protected: + RMW_FASTRTPS_SHARED_CPP_PUBLIC + TypeSupport(); + + bool max_size_bound_; +}; + +inline void +_unregister_type( + eprosima::fastrtps::Participant * participant, + TypeSupport * typed_typesupport) +{ + if (eprosima::fastrtps::Domain::unregisterType(participant, typed_typesupport->getName())) { + delete typed_typesupport; + } +} + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp similarity index 89% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_client_info.hpp rename to rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 1074bd058..babb2876e 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__CUSTOM_CLIENT_INFO_HPP_ -#define RMW_FASTRTPS_CPP__CUSTOM_CLIENT_INFO_HPP_ +#ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ #include #include @@ -27,14 +27,15 @@ #include "fastrtps/subscriber/SubscriberListener.h" #include "fastrtps/participant/Participant.h" #include "fastrtps/publisher/Publisher.h" -#include "rmw_fastrtps_cpp/TypeSupport.hpp" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" class ClientListener; typedef struct CustomClientInfo { - void * request_type_support_; - void * response_type_support_; + rmw_fastrtps_shared_cpp::TypeSupport * request_type_support_; + rmw_fastrtps_shared_cpp::TypeSupport * response_type_support_; eprosima::fastrtps::Subscriber * response_subscriber_; eprosima::fastrtps::Publisher * request_publisher_; ClientListener * listener_; @@ -67,7 +68,7 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener response.buffer_.reset(new eprosima::fastcdr::FastBuffer()); eprosima::fastrtps::SampleInfo_t sinfo; - rmw_fastrtps_cpp::SerializedData data; + rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; data.data = response.buffer_.get(); if (sub->takeNextData(&data, &sinfo)) { @@ -149,4 +150,4 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener std::condition_variable * conditionVariable_; }; -#endif // RMW_FASTRTPS_CPP__CUSTOM_CLIENT_INFO_HPP_ +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp similarity index 89% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_participant_info.hpp rename to rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 84e479d15..775739875 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ -#define RMW_FASTRTPS_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ +#ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ #include #include @@ -26,10 +26,9 @@ #include "rmw/impl/cpp/key_value.hpp" #include "rmw/rmw.h" -#include "rmw_fastrtps_cpp/reader_info.hpp" -#include "rmw_fastrtps_cpp/writer_info.hpp" - class ParticipantListener; +class ReaderInfo; +class WriterInfo; typedef struct CustomParticipantInfo { @@ -95,4 +94,4 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener std::map discovered_names; }; -#endif // RMW_FASTRTPS_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp similarity index 68% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_publisher_info.hpp rename to rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 96556963c..330977b9d 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,19 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__CUSTOM_PUBLISHER_INFO_HPP_ -#define RMW_FASTRTPS_CPP__CUSTOM_PUBLISHER_INFO_HPP_ +#ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ #include "fastrtps/publisher/Publisher.h" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + typedef struct CustomPublisherInfo { eprosima::fastrtps::Publisher * publisher_; - void * type_support_; + rmw_fastrtps_shared_cpp::TypeSupport * type_support_; rmw_gid_t publisher_gid; const char * typesupport_identifier_; } CustomPublisherInfo; -#endif // RMW_FASTRTPS_CPP__CUSTOM_PUBLISHER_INFO_HPP_ +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp similarity index 89% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_service_info.hpp rename to rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 8423dfdd3..10eb0dab6 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__CUSTOM_SERVICE_INFO_HPP_ -#define RMW_FASTRTPS_CPP__CUSTOM_SERVICE_INFO_HPP_ +#ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ #include #include @@ -26,14 +26,15 @@ #include "fastrtps/subscriber/Subscriber.h" #include "fastrtps/subscriber/SubscriberListener.h" #include "fastrtps/subscriber/SampleInfo.h" -#include "rmw_fastrtps_cpp/TypeSupport.hpp" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" class ServiceListener; typedef struct CustomServiceInfo { - void * request_type_support_; - void * response_type_support_; + rmw_fastrtps_shared_cpp::TypeSupport * request_type_support_; + rmw_fastrtps_shared_cpp::TypeSupport * response_type_support_; eprosima::fastrtps::Subscriber * request_subscriber_; eprosima::fastrtps::Publisher * response_publisher_; ServiceListener * listener_; @@ -70,7 +71,7 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener request.buffer_ = new eprosima::fastcdr::FastBuffer(); eprosima::fastrtps::SampleInfo_t sinfo; - rmw_fastrtps_cpp::SerializedData data; + rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; data.data = request.buffer_; if (sub->takeNextData(&data, &sinfo)) { @@ -151,4 +152,4 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener std::condition_variable * conditionVariable_; }; -#endif // RMW_FASTRTPS_CPP__CUSTOM_SERVICE_INFO_HPP_ +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp similarity index 88% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_subscriber_info.hpp rename to rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index c5047c0b7..e597d6caf 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ -#define RMW_FASTRTPS_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ +#ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ #include #include @@ -23,13 +23,15 @@ #include "fastrtps/subscriber/Subscriber.h" #include "fastrtps/subscriber/SubscriberListener.h" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + class SubListener; typedef struct CustomSubscriberInfo { eprosima::fastrtps::Subscriber * subscriber_; SubListener * listener_; - void * type_support_; + rmw_fastrtps_shared_cpp::TypeSupport * type_support_; const char * typesupport_identifier_; } CustomSubscriberInfo; @@ -112,4 +114,4 @@ class SubListener : public eprosima::fastrtps::SubscriberListener std::condition_variable * conditionVariable_; }; -#endif // RMW_FASTRTPS_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp new file mode 100644 index 000000000..59d6e90c1 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -0,0 +1,258 @@ +// 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 RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_ + +#include "./visibility_control.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/names_and_types.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_client( + const char * identifier, + rmw_node_t * node, + rmw_client_t * client); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_compare_gids_equal( + const char * identifier, + const rmw_gid_t * gid1, + const rmw_gid_t * gid2, + bool * result); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_count_publishers( + const char * identifier, + const rmw_node_t * node, + const char * topic_name, + size_t * count); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_count_subscribers( + const char * identifier, + const rmw_node_t * node, + const char * topic_name, + size_t * count); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_gid_for_publisher( + const char * identifier, + const rmw_publisher_t * publisher, + rmw_gid_t * gid); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_guard_condition_t * +__rmw_create_guard_condition(const char * identifier); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_trigger_guard_condition( + const char * identifier, + const rmw_guard_condition_t * guard_condition_handle); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_set_log_severity(rmw_log_severity_t severity); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_node_t * +__rmw_create_node( + const char * identifier, + const char * name, + const char * namespace_, + size_t domain_id, + const rmw_node_security_options_t * security_options); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_node( + const char * identifier, + rmw_node_t * node); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +const rmw_guard_condition_t * +__rmw_node_get_graph_guard_condition(const rmw_node_t * node); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_node_names( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publish( + const char * identifier, + const rmw_publisher_t * publisher, + const void * ros_message); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publish_serialized_message( + const char * identifier, + const rmw_publisher_t * publisher, + const rmw_serialized_message_t * serialized_message); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_publisher( + const char * identifier, + rmw_node_t * node, + rmw_publisher_t * publisher); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_send_request( + const char * identifier, + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_request( + const char * identifier, + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_request, + bool * taken); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_response( + const char * identifier, + const rmw_client_t * client, + rmw_request_id_t * request_header, + void * ros_response, + bool * taken); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_send_response( + const char * identifier, + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_service( + const char * identifier, + rmw_node_t * node, + rmw_service_t * service); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_service_names_and_types( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * service_names_and_types); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_service_server_is_available( + const char * identifier, + const rmw_node_t * node, + const rmw_client_t * client, + bool * is_available); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_subscription( + const char * identifier, + rmw_node_t * node, + rmw_subscription_t * subscription); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_with_info( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_serialized_message( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_serialized_message_with_info( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_topic_names_and_types( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_wait( + rmw_subscriptions_t * subscriptions, + rmw_guard_conditions_t * guard_conditions, + rmw_services_t * services, + rmw_clients_t * clients, + rmw_wait_set_t * wait_set, + const rmw_time_t * wait_timeout); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_wait_set_t * +__rmw_create_wait_set(const char * identifier, size_t max_conditions); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_wait_set(const char * identifier, rmw_wait_set_t * wait_set); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/visibility_control.h b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/visibility_control.h new file mode 100644 index 000000000..de545f6e8 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RMW_FASTRTPS_SHARED_CPP__VISIBILITY_CONTROL_H_ +#define RMW_FASTRTPS_SHARED_CPP__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RMW_FASTRTPS_SHARED_CPP_EXPORT __attribute__ ((dllexport)) + #define RMW_FASTRTPS_SHARED_CPP_IMPORT __attribute__ ((dllimport)) + #else + #define RMW_FASTRTPS_SHARED_CPP_EXPORT __declspec(dllexport) + #define RMW_FASTRTPS_SHARED_CPP_IMPORT __declspec(dllimport) + #endif + #ifdef RMW_FASTRTPS_SHARED_CPP_BUILDING_LIBRARY + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC RMW_FASTRTPS_SHARED_CPP_EXPORT + #else + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC RMW_FASTRTPS_SHARED_CPP_IMPORT + #endif + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC_TYPE RMW_FASTRTPS_SHARED_CPP_PUBLIC + #define RMW_FASTRTPS_SHARED_CPP_LOCAL +#else + #define RMW_FASTRTPS_SHARED_CPP_EXPORT __attribute__ ((visibility("default"))) + #define RMW_FASTRTPS_SHARED_CPP_IMPORT + #if __GNUC__ >= 4 + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC __attribute__ ((visibility("default"))) + #define RMW_FASTRTPS_SHARED_CPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC + #define RMW_FASTRTPS_SHARED_CPP_LOCAL + #endif + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC_TYPE +#endif + +#endif // RMW_FASTRTPS_SHARED_CPP__VISIBILITY_CONTROL_H_ diff --git a/rmw_fastrtps_shared_cpp/package.xml b/rmw_fastrtps_shared_cpp/package.xml new file mode 100644 index 000000000..1885c64cd --- /dev/null +++ b/rmw_fastrtps_shared_cpp/package.xml @@ -0,0 +1,34 @@ + + + + rmw_fastrtps_shared_cpp + 0.5.1 + Code shared on static and dynamic type support of rmw_fastrtps_cpp. + Dirk Thomas + Apache License 2.0 + Ricardo González + + ament_cmake_ros + fastrtps_cmake_module + + ament_cmake + + fastcdr + fastrtps + fastrtps_cmake_module + rcutils + rmw + + fastcdr + fastrtps + fastrtps_cmake_module + rcutils + rmw + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake b/rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake new file mode 100644 index 000000000..860e9724a --- /dev/null +++ b/rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake @@ -0,0 +1,24 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +# copied from rmw_fastrtps_shared_cpp/rmw_fastrtps_shared_cpp-extras.cmake + +find_package(fastrtps_cmake_module REQUIRED) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +list(APPEND rmw_fastrtps_shared_cpp_INCLUDE_DIRS ${FastRTPS_INCLUDE_DIR}) +# specific order: dependents before dependencies +list(APPEND rmw_fastrtps_shared_cpp_LIBRARIES fastrtps fastcdr) diff --git a/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp new file mode 100644 index 000000000..607129481 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/TypeSupport_impl.cpp @@ -0,0 +1,119 @@ +// 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 +#include +#include +#include +#include + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +TypeSupport::TypeSupport() +{ + m_isGetKeyDefined = false; + max_size_bound_ = false; +} + +void TypeSupport::deleteData(void * data) +{ + assert(data); + delete static_cast(data); +} + +void * TypeSupport::createData() +{ + return new eprosima::fastcdr::FastBuffer(); +} + +bool TypeSupport::serialize( + void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) +{ + assert(data); + assert(payload); + + auto ser_data = static_cast(data); + if (ser_data->is_cdr_buffer) { + auto ser = static_cast(ser_data->data); + if (payload->max_size >= ser->getSerializedDataLength()) { + payload->length = static_cast(ser->getSerializedDataLength()); + payload->encapsulation = ser->endianness() == + eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + memcpy(payload->data, ser->getBufferPointer(), ser->getSerializedDataLength()); + return true; + } + } else { + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload->data), + 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)) { + payload->encapsulation = ser.endianness() == + eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + payload->length = (uint32_t)ser.getSerializedDataLength(); + return true; + } + } + + return false; +} + +bool TypeSupport::deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t * payload, + void * data) +{ + assert(data); + assert(payload); + + auto ser_data = static_cast(data); + if (ser_data->is_cdr_buffer) { + auto buffer = static_cast(ser_data->data); + if (!buffer->reserve(payload->length)) { + return false; + } + memcpy(buffer->getBuffer(), payload->data, payload->length); + return true; + } + + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload->data), + payload->length); + eprosima::fastcdr::Cdr deser( + fastbuffer, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + return deserializeROSmessage(deser, ser_data->data); +} + +std::function TypeSupport::getSerializedSizeProvider(void * data) +{ + assert(data); + + auto ser_data = static_cast(data); + auto ser_size = [this, ser_data]() -> uint32_t + { + if (ser_data->is_cdr_buffer) { + auto ser = static_cast(ser_data->data); + return static_cast(ser->getSerializedDataLength()); + } + return static_cast(this->getEstimatedSerializedSize(ser_data->data)); + }; + return ser_size; +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_cpp/src/demangle.cpp b/rmw_fastrtps_shared_cpp/src/demangle.cpp similarity index 93% rename from rmw_fastrtps_cpp/src/demangle.cpp rename to rmw_fastrtps_shared_cpp/src/demangle.cpp index 2741384b3..d52ae3a8f 100644 --- a/rmw_fastrtps_cpp/src/demangle.cpp +++ b/rmw_fastrtps_shared_cpp/src/demangle.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -80,7 +80,7 @@ _demangle_service_from_topic(const std::string & topic_name) suffix_position = topic_name.rfind(suffix); if (suffix_position != std::string::npos) { if (topic_name.length() - suffix_position - suffix.length() != 0) { - RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_cpp", + RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_shared_cpp", "service topic has service prefix and a suffix, but not at the end" ", report this: '%s'", topic_name.c_str()) continue; @@ -90,7 +90,7 @@ _demangle_service_from_topic(const std::string & topic_name) } } if (std::string::npos == suffix_position) { - RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_cpp", + RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_shared_cpp", "service topic has prefix but no suffix" ", report this: '%s'", topic_name.c_str()) return ""; @@ -122,7 +122,7 @@ _demangle_service_type_only(const std::string & dds_type_name) suffix_position = dds_type_name.rfind(suffix); if (suffix_position != std::string::npos) { if (dds_type_name.length() - suffix_position - suffix.length() != 0) { - RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_cpp", + RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_shared_cpp", "service type contains '::srv::dds_::' and a suffix, but not at the end" ", report this: '%s'", dds_type_name.c_str()) continue; @@ -132,7 +132,7 @@ _demangle_service_type_only(const std::string & dds_type_name) } } if (std::string::npos == suffix_position) { - RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_cpp", + RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_shared_cpp", "service type contains '::srv::dds_::' but does not have a suffix" ", report this: '%s'", dds_type_name.c_str()) return ""; diff --git a/rmw_fastrtps_cpp/src/demangle.hpp b/rmw_fastrtps_shared_cpp/src/demangle.hpp similarity index 94% rename from rmw_fastrtps_cpp/src/demangle.hpp rename to rmw_fastrtps_shared_cpp/src/demangle.hpp index 18a9433e4..65efe9918 100644 --- a/rmw_fastrtps_cpp/src/demangle.hpp +++ b/rmw_fastrtps_shared_cpp/src/demangle.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_shared_cpp/src/namespace_prefix.cpp b/rmw_fastrtps_shared_cpp/src/namespace_prefix.cpp new file mode 100644 index 000000000..179c06c13 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/namespace_prefix.cpp @@ -0,0 +1,53 @@ +// 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 +#include + +#include "namespace_prefix.hpp" + +extern "C" +{ +// static for internal linkage +const char * const ros_topic_prefix = "rt"; +const char * const ros_service_requester_prefix = "rq"; +const char * const ros_service_response_prefix = "rr"; + +const std::vector _ros_prefixes = +{ros_topic_prefix, ros_service_requester_prefix, ros_service_response_prefix}; +} // extern "C" + +/// Return the ROS specific prefix if it exists, otherwise "". +std::string +_get_ros_prefix_if_exists(const std::string & topic_name) +{ + for (const auto & prefix : _ros_prefixes) { + if (topic_name.rfind(prefix, 0) == 0 && topic_name.at(prefix.length()) == '/') { + return prefix; + } + } + return ""; +} + +/// Strip the ROS specific prefix if it exists from the topic name. +std::string +_strip_ros_prefix_if_exists(const std::string & topic_name) +{ + for (const auto & prefix : _ros_prefixes) { + if (topic_name.rfind(prefix, 0) == 0 && topic_name.at(prefix.length()) == '/') { + return topic_name.substr(prefix.length()); + } + } + return topic_name; +} diff --git a/rmw_fastrtps_shared_cpp/src/namespace_prefix.hpp b/rmw_fastrtps_shared_cpp/src/namespace_prefix.hpp new file mode 100644 index 000000000..a230f3cc9 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/namespace_prefix.hpp @@ -0,0 +1,37 @@ +// 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 NAMESPACE_PREFIX_HPP_ +#define NAMESPACE_PREFIX_HPP_ + +#include +#include + +extern "C" +{ +extern const char * const ros_topic_prefix; +extern const char * const ros_service_requester_prefix; +extern const char * const ros_service_response_prefix; + +extern const std::vector _ros_prefixes; +} // extern "C" + +/// Return the ROS specific prefix if it exists, otherwise "". +std::string +_get_ros_prefix_if_exists(const std::string & topic_name); + +/// Returns the topic name stripped of and ROS specific prefix if exists. +std::string +_strip_ros_prefix_if_exists(const std::string & topic_name); +#endif // NAMESPACE_PREFIX_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp new file mode 100644 index 000000000..53b2b6262 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -0,0 +1,160 @@ +// 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 + +#include "qos.hpp" + +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/attributes/SubscriberAttributes.h" + +#include "rmw/error_handling.h" + +extern "C" +{ +bool +get_datareader_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastrtps::SubscriberAttributes & sattr) +{ + switch (qos_policies.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + sattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + sattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS history policy"); + return false; + } + + switch (qos_policies.reliability) { + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + sattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + sattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); + return false; + } + + switch (qos_policies.durability) { + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + sattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + sattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS durability policy"); + return false; + } + + if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) { + sattr.topic.historyQos.depth = static_cast(qos_policies.depth); + } + + // ensure the history depth is at least the requested queue size + assert(sattr.topic.historyQos.depth >= 0); + if ( + eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == sattr.topic.historyQos.kind && + static_cast(sattr.topic.historyQos.depth) < qos_policies.depth) + { + if (qos_policies.depth > (std::numeric_limits::max)()) { + RMW_SET_ERROR_MSG( + "failed to set history depth since the requested queue size exceeds the DDS type"); + return false; + } + sattr.topic.historyQos.depth = static_cast(qos_policies.depth); + } + + return true; +} + +bool +get_datawriter_qos( + const rmw_qos_profile_t & qos_policies, eprosima::fastrtps::PublisherAttributes & pattr) +{ + switch (qos_policies.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + pattr.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS history policy"); + return false; + } + + switch (qos_policies.durability) { + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + pattr.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + pattr.qos.m_durability.kind = eprosima::fastrtps::VOLATILE_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS durability policy"); + return false; + } + + switch (qos_policies.reliability) { + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + pattr.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + pattr.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); + return false; + } + + if (qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT) { + pattr.topic.historyQos.depth = static_cast(qos_policies.depth); + } + + // ensure the history depth is at least the requested queue size + assert(pattr.topic.historyQos.depth >= 0); + if ( + eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == pattr.topic.historyQos.kind && + static_cast(pattr.topic.historyQos.depth) < qos_policies.depth) + { + if (qos_policies.depth > (std::numeric_limits::max)()) { + RMW_SET_ERROR_MSG( + "failed to set history depth since the requested queue size exceeds the DDS type"); + return false; + } + pattr.topic.historyQos.depth = static_cast(qos_policies.depth); + } + + return true; +} +} // extern "C" diff --git a/rmw_fastrtps_shared_cpp/src/qos.hpp b/rmw_fastrtps_shared_cpp/src/qos.hpp new file mode 100644 index 000000000..c490929b6 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/qos.hpp @@ -0,0 +1,45 @@ +// 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 QOS_HPP_ +#define QOS_HPP_ + +#include "rmw/rmw.h" + +namespace eprosima +{ +namespace fastrtps +{ +class SubscriberAttributes; +class PublisherAttributes; +} // namespace fastrtps +} // namespace eprosima + +extern "C" +{ +RMW_LOCAL +bool +get_datareader_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastrtps::SubscriberAttributes & sattr); + +RMW_LOCAL +bool +get_datawriter_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastrtps::PublisherAttributes & pattr); +} +// extern "C" + +#endif // QOS_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/reader_info.hpp b/rmw_fastrtps_shared_cpp/src/reader_info.hpp similarity index 84% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/reader_info.hpp rename to rmw_fastrtps_shared_cpp/src/reader_info.hpp index 733068624..0bfaebc58 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/reader_info.hpp +++ b/rmw_fastrtps_shared_cpp/src/reader_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__READER_INFO_HPP_ -#define RMW_FASTRTPS_CPP__READER_INFO_HPP_ +#ifndef READER_INFO_HPP_ +#define READER_INFO_HPP_ #include #include @@ -33,6 +33,8 @@ #include "fastrtps/rtps/reader/ReaderListener.h" #include "fastrtps/rtps/reader/RTPSReader.h" +#include "types/guard_condition.hpp" + class ReaderInfo : public eprosima::fastrtps::rtps::ReaderListener { public: @@ -40,7 +42,7 @@ class ReaderInfo : public eprosima::fastrtps::rtps::ReaderListener eprosima::fastrtps::Participant * participant, rmw_guard_condition_t * graph_guard_condition) : participant_(participant), - graph_guard_condition_(graph_guard_condition) + graph_guard_condition_(static_cast(graph_guard_condition->data)) {} void @@ -88,7 +90,7 @@ class ReaderInfo : public eprosima::fastrtps::rtps::ReaderListener trigger = true; } else { RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", + "rmw_fastrtps_shared_cpp", "unexpected removal of subscription on topic '%s' with type '%s'", fqdn.c_str(), proxyData.typeName().c_str()); } @@ -97,19 +99,13 @@ class ReaderInfo : public eprosima::fastrtps::rtps::ReaderListener mapmutex.unlock(); if (trigger) { - rmw_ret_t ret = rmw_trigger_guard_condition(graph_guard_condition_); - if (ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "failed to trigger graph guard condition: %s", - rmw_get_error_string_safe()) - } + graph_guard_condition_->trigger(); } } std::map> topicNtypes; std::mutex mapmutex; eprosima::fastrtps::Participant * participant_; - rmw_guard_condition_t * graph_guard_condition_; + GuardCondition * graph_guard_condition_; }; -#endif // RMW_FASTRTPS_CPP__READER_INFO_HPP_ +#endif // READER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp new file mode 100644 index 000000000..dd76e4d96 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -0,0 +1,78 @@ +// 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 + +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/rmw.h" + +#include "namespace_prefix.hpp" +#include "qos.hpp" +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_destroy_client( + const char * identifier, + rmw_node_t * node, + rmw_client_t * client) +{ + (void)node; + if (!client) { + RMW_SET_ERROR_MSG("client handle is null"); + return RMW_RET_ERROR; + } + if (client->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(client->data); + if (info != nullptr) { + if (info->response_subscriber_ != nullptr) { + Domain::removeSubscriber(info->response_subscriber_); + } + if (info->request_publisher_ != nullptr) { + Domain::removePublisher(info->request_publisher_); + } + if (info->listener_ != nullptr) { + delete info->listener_; + } + if (info->request_type_support_ != nullptr) { + _unregister_type(info->participant_, info->request_type_support_); + } + if (info->response_type_support_ != nullptr) { + _unregister_type(info->participant_, info->response_type_support_); + } + delete info; + } + if (client->service_name != nullptr) { + rmw_free(const_cast(client->service_name)); + client->service_name = nullptr; + } + rmw_client_free(client); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_compare_gids_equal.cpp b/rmw_fastrtps_shared_cpp/src/rmw_compare_gids_equal.cpp new file mode 100644 index 000000000..83886c621 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_compare_gids_equal.cpp @@ -0,0 +1,62 @@ +// 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 "fastrtps/rtps/common/Guid.h" + +#include "rmw/rmw.h" +#include "rmw/error_handling.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_compare_gids_equal( + const char * identifier, + const rmw_gid_t * gid1, + const rmw_gid_t * gid2, + bool * result) +{ + if (!gid1) { + RMW_SET_ERROR_MSG("gid1 is null"); + return RMW_RET_ERROR; + } + + if (gid1->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("guid1 handle not from this implementation"); + return RMW_RET_ERROR; + } + + if (!gid2) { + RMW_SET_ERROR_MSG("gid2 is null"); + return RMW_RET_ERROR; + } + + if (gid2->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("gid2 handle not from this implementation"); + return RMW_RET_ERROR; + } + + if (!result) { + RMW_SET_ERROR_MSG("result is null"); + return RMW_RET_ERROR; + } + + *result = + memcmp(gid1->data, gid2->data, sizeof(eprosima::fastrtps::rtps::GUID_t)) == 0; + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp new file mode 100644 index 000000000..5e1ee2956 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp @@ -0,0 +1,112 @@ +// 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 +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "demangle.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "reader_info.hpp" +#include "writer_info.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_count_publishers( + const char * identifier, + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + // safechecks + + if (!node) { + RMW_SET_ERROR_MSG("null node handle"); + return RMW_RET_ERROR; + } + // Get participant pointer from node + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto impl = static_cast(node->data); + + WriterInfo * slave_target = impl->secondaryPubListener; + slave_target->mapmutex.lock(); + *count = 0; + for (const auto & it : slave_target->topicNtypes) { + const auto topic_fqdn = _demangle_if_ros_topic(it.first); + if (topic_fqdn == topic_name) { + *count += it.second.size(); + } + } + slave_target->mapmutex.unlock(); + + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_shared_cpp", + "looking for subscriber topic: %s, number of matches: %zu", + topic_name, *count) + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_count_subscribers( + const char * identifier, + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + // safechecks + + if (!node) { + RMW_SET_ERROR_MSG("null node handle"); + return RMW_RET_ERROR; + } + // Get participant pointer from node + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + CustomParticipantInfo * impl = static_cast(node->data); + + ReaderInfo * slave_target = impl->secondarySubListener; + *count = 0; + slave_target->mapmutex.lock(); + for (const auto & it : slave_target->topicNtypes) { + const auto topic_fqdn = _demangle_if_ros_topic(it.first); + if (topic_fqdn == topic_name) { + *count += it.second.size(); + } + } + slave_target->mapmutex.unlock(); + + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_shared_cpp", + "looking for subscriber topic: %s, number of matches: %zu", + topic_name, *count) + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_gid_for_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_gid_for_publisher.cpp new file mode 100644 index 000000000..2164dd56d --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_gid_for_publisher.cpp @@ -0,0 +1,55 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_get_gid_for_publisher( + const char * identifier, + const rmw_publisher_t * publisher, + rmw_gid_t * gid) +{ + if (!publisher) { + RMW_SET_ERROR_MSG("publisher is null"); + return RMW_RET_ERROR; + } + + if (publisher->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + if (!gid) { + RMW_SET_ERROR_MSG("gid is null"); + return RMW_RET_ERROR; + } + + auto info = static_cast(publisher->data); + + if (!info) { + RMW_SET_ERROR_MSG("publisher info handle is null"); + return RMW_RET_ERROR; + } + + *gid = info->publisher_gid; + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_guard_condition.cpp b/rmw_fastrtps_shared_cpp/src/rmw_guard_condition.cpp new file mode 100644 index 000000000..4dac80e26 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_guard_condition.cpp @@ -0,0 +1,44 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "types/guard_condition.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_guard_condition_t * +__rmw_create_guard_condition(const char * identifier) +{ + rmw_guard_condition_t * guard_condition_handle = new rmw_guard_condition_t; + guard_condition_handle->implementation_identifier = identifier; + guard_condition_handle->data = new GuardCondition(); + return guard_condition_handle; +} + +rmw_ret_t +__rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) +{ + if (guard_condition) { + delete static_cast(guard_condition->data); + delete guard_condition; + return RMW_RET_OK; + } + + return RMW_RET_ERROR; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp new file mode 100644 index 000000000..80710b1de --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_logging.cpp @@ -0,0 +1,56 @@ +// 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/rmw.h" +#include "rmw/error_handling.h" + +#include "rcutils/logging_macros.h" + +#include "fastrtps/log/Log.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +using eprosima::fastrtps::Log; + +rmw_ret_t +__rmw_set_log_severity(rmw_log_severity_t severity) +{ + Log::Kind log_kind; + + switch (severity) { + case RMW_LOG_SEVERITY_DEBUG: +// fall through + case RMW_LOG_SEVERITY_INFO: + log_kind = Log::Kind::Info; + break; + case RMW_LOG_SEVERITY_WARN: + log_kind = Log::Kind::Warning; + break; + case RMW_LOG_SEVERITY_ERROR: +// fall through + case RMW_LOG_SEVERITY_FATAL: + log_kind = Log::Kind::Error; + break; + default: + RCUTILS_LOG_ERROR("Unknown logging severity type %d", severity); + return RMW_RET_ERROR; + } + + eprosima::fastrtps::Log::SetVerbosity(log_kind); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp new file mode 100644 index 000000000..79ae5b09a --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp @@ -0,0 +1,376 @@ +// 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 +#include +#include +#include + +#include "rcutils/filesystem.h" +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "fastrtps/config.h" +#include "fastrtps/Domain.h" +#include "fastrtps/participant/Participant.h" +#include "fastrtps/attributes/ParticipantAttributes.h" +#include "fastrtps/publisher/Publisher.h" +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/publisher/PublisherListener.h" +#include "fastrtps/subscriber/Subscriber.h" +#include "fastrtps/subscriber/SubscriberListener.h" +#include "fastrtps/subscriber/SampleInfo.h" +#include "fastrtps/attributes/SubscriberAttributes.h" + +#include "fastrtps/rtps/RTPSDomain.h" + +#include "fastrtps/rtps/reader/RTPSReader.h" +#include "fastrtps/rtps/reader/StatefulReader.h" +#include "fastrtps/rtps/reader/ReaderListener.h" +#include "fastrtps/rtps/builtin/discovery/endpoint/EDPSimple.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "reader_info.hpp" +#include "writer_info.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using ParticipantAttributes = eprosima::fastrtps::ParticipantAttributes; +using StatefulReader = eprosima::fastrtps::rtps::StatefulReader; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_node_t * +create_node( + const char * identifier, + const char * name, + const char * namespace_, + ParticipantAttributes participantAttrs) +{ + if (!name) { + RMW_SET_ERROR_MSG("name is null"); + return nullptr; + } + + if (!namespace_) { + RMW_SET_ERROR_MSG("namespace_ is null"); + return nullptr; + } + + // Declare everything before beginning to create things. + ::ParticipantListener * listener = nullptr; + Participant * participant = nullptr; + rmw_guard_condition_t * graph_guard_condition = nullptr; + CustomParticipantInfo * node_impl = nullptr; + rmw_node_t * node_handle = nullptr; + ReaderInfo * tnat_1 = nullptr; + WriterInfo * tnat_2 = nullptr; + std::pair edp_readers; + + try { + listener = new ::ParticipantListener(); + } catch (std::bad_alloc &) { + RMW_SET_ERROR_MSG("failed to allocate participant listener"); + goto fail; + } + + participant = Domain::createParticipant(participantAttrs, listener); + if (!participant) { + RMW_SET_ERROR_MSG("create_node() could not create participant"); + return nullptr; + } + + graph_guard_condition = __rmw_create_guard_condition(identifier); + if (!graph_guard_condition) { + // error already set + goto fail; + } + + try { + node_impl = new CustomParticipantInfo(); + } catch (std::bad_alloc &) { + RMW_SET_ERROR_MSG("failed to allocate node impl struct"); + goto fail; + } + + node_handle = rmw_node_allocate(); + if (!node_handle) { + RMW_SET_ERROR_MSG("failed to allocate rmw_node_t"); + goto fail; + } + node_handle->implementation_identifier = identifier; + node_impl->participant = participant; + node_impl->listener = listener; + node_impl->graph_guard_condition = graph_guard_condition; + node_handle->data = node_impl; + + node_handle->name = + static_cast(rmw_allocate(sizeof(char) * strlen(name) + 1)); + if (!node_handle->name) { + RMW_SET_ERROR_MSG("failed to allocate memory"); + node_handle->namespace_ = nullptr; // to avoid free on uninitialized memory + goto fail; + } + memcpy(const_cast(node_handle->name), name, strlen(name) + 1); + + node_handle->namespace_ = + static_cast(rmw_allocate(sizeof(char) * strlen(namespace_) + 1)); + if (!node_handle->namespace_) { + RMW_SET_ERROR_MSG("failed to allocate memory"); + goto fail; + } + memcpy(const_cast(node_handle->namespace_), namespace_, strlen(namespace_) + 1); + + tnat_1 = new ReaderInfo(participant, graph_guard_condition); + tnat_2 = new WriterInfo(participant, graph_guard_condition); + + node_impl->secondarySubListener = tnat_1; + node_impl->secondaryPubListener = tnat_2; + + edp_readers = participant->getEDPReaders(); + if (!edp_readers.first) { + RMW_SET_ERROR_MSG("edp_readers.first is null"); + goto fail; + } + + if (!edp_readers.second) { + RMW_SET_ERROR_MSG("edp_readers.second is null"); + goto fail; + } + + if (!(edp_readers.first->setListener(tnat_1) & edp_readers.second->setListener(tnat_2))) { + RMW_SET_ERROR_MSG("Failed to attach ROS related logic to the Participant"); + goto fail; + } + + return node_handle; +fail: + delete tnat_2; + delete tnat_1; + if (node_handle) { + rmw_free(const_cast(node_handle->namespace_)); + node_handle->namespace_ = nullptr; + rmw_free(const_cast(node_handle->name)); + node_handle->name = nullptr; + } + rmw_node_free(node_handle); + delete node_impl; + if (graph_guard_condition) { + rmw_ret_t ret = __rmw_destroy_guard_condition(graph_guard_condition); + if (ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_shared_cpp", + "failed to destroy guard condition during error handling") + } + } + rmw_free(listener); + if (participant) { + Domain::removeParticipant(participant); + } + return nullptr; +} + +bool +get_security_file_paths( + std::array & security_files_paths, const char * node_secure_root) +{ + // here assume only 6 files for security + const char * file_names[6] = { + "ca.cert.pem", "cert.pem", "key.pem", + "ca.cert.pem", "governance.p7s", "permissions.p7s" + }; + size_t num_files = sizeof(file_names) / sizeof(char *); + + std::string file_prefix("file://"); + + for (size_t i = 0; i < num_files; i++) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * file_path = rcutils_join_path(node_secure_root, file_names[i], allocator); + + if (!file_path) { + return false; + } + + if (rcutils_is_readable(file_path)) { + security_files_paths[i] = file_prefix + std::string(file_path); + } else { + allocator.deallocate(file_path, allocator.state); + return false; + } + + allocator.deallocate(file_path, allocator.state); + } + + return true; +} + +rmw_node_t * +__rmw_create_node( + const char * identifier, + const char * name, + const char * namespace_, + size_t domain_id, + const rmw_node_security_options_t * security_options) +{ + if (!name) { + RMW_SET_ERROR_MSG("name is null"); + return nullptr; + } + if (!security_options) { + RMW_SET_ERROR_MSG("security_options is null"); + return nullptr; + } + + ParticipantAttributes participantAttrs; + + // Load default XML profile. + Domain::getDefaultParticipantAttributes(participantAttrs); + + participantAttrs.rtps.builtin.domainId = static_cast(domain_id); + // since the participant name is not part of the DDS spec + participantAttrs.rtps.setName(name); + // the node name is also set in the user_data + size_t name_length = strlen(name); + const char prefix[6] = "name="; + participantAttrs.rtps.userData.resize(name_length + sizeof(prefix)); + memcpy(participantAttrs.rtps.userData.data(), prefix, sizeof(prefix) - 1); + for (size_t i = 0; i < name_length; ++i) { + participantAttrs.rtps.userData[sizeof(prefix) - 1 + i] = name[i]; + } + participantAttrs.rtps.userData[sizeof(prefix) - 1 + name_length] = ';'; + + if (security_options->security_root_path) { + // if security_root_path provided, try to find the key and certificate files +#if HAVE_SECURITY + std::array security_files_paths; + + if (get_security_file_paths(security_files_paths, security_options->security_root_path)) { + eprosima::fastrtps::rtps::PropertyPolicy property_policy; + using Property = eprosima::fastrtps::rtps::Property; + property_policy.properties().emplace_back( + Property("dds.sec.auth.plugin", "builtin.PKI-DH")); + property_policy.properties().emplace_back( + Property("dds.sec.auth.builtin.PKI-DH.identity_ca", + security_files_paths[0])); + property_policy.properties().emplace_back( + Property("dds.sec.auth.builtin.PKI-DH.identity_certificate", + security_files_paths[1])); + property_policy.properties().emplace_back( + Property("dds.sec.auth.builtin.PKI-DH.private_key", + security_files_paths[2])); + property_policy.properties().emplace_back( + Property("dds.sec.crypto.plugin", "builtin.AES-GCM-GMAC")); + + property_policy.properties().emplace_back(Property( + "dds.sec.access.plugin", "builtin.Access-Permissions")); + property_policy.properties().emplace_back(Property( + "dds.sec.access.builtin.Access-Permissions.permissions_ca", security_files_paths[3])); + property_policy.properties().emplace_back(Property( + "dds.sec.access.builtin.Access-Permissions.governance", security_files_paths[4])); + property_policy.properties().emplace_back(Property( + "dds.sec.access.builtin.Access-Permissions.permissions", security_files_paths[5])); + + participantAttrs.rtps.properties = property_policy; + } else if (security_options->enforce_security) { + RMW_SET_ERROR_MSG("couldn't find all security files!"); + return nullptr; + } +#else + RMW_SET_ERROR_MSG( + "This Fast-RTPS version doesn't have the security libraries\n" + "Please compile Fast-RTPS using the -DSECURITY=ON CMake option"); + return nullptr; +#endif + } + return create_node(identifier, name, namespace_, participantAttrs); +} + +rmw_ret_t +__rmw_destroy_node( + const char * identifier, + rmw_node_t * node) +{ + rmw_ret_t result_ret = RMW_RET_OK; + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return RMW_RET_ERROR; + } + + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto impl = static_cast(node->data); + if (!impl) { + RMW_SET_ERROR_MSG("node impl is null"); + return RMW_RET_ERROR; + } + + Participant * participant = impl->participant; + + // Begin deleting things in the same order they were created in __rmw_create_node(). + std::pair edp_readers = participant->getEDPReaders(); + if (!edp_readers.first || !edp_readers.second) { + RMW_SET_ERROR_MSG("failed to get EDPReader listener"); + result_ret = RMW_RET_ERROR; + } + + if (edp_readers.first && !edp_readers.first->setListener(nullptr)) { + RMW_SET_ERROR_MSG("failed to unset EDPReader listener"); + result_ret = RMW_RET_ERROR; + } + delete impl->secondarySubListener; + if (edp_readers.second && !edp_readers.second->setListener(nullptr)) { + RMW_SET_ERROR_MSG("failed to unset EDPReader listener"); + result_ret = RMW_RET_ERROR; + } + delete impl->secondaryPubListener; + + rmw_free(const_cast(node->name)); + node->name = nullptr; + rmw_free(const_cast(node->namespace_)); + node->namespace_ = nullptr; + rmw_node_free(node); + + if (RMW_RET_OK != __rmw_destroy_guard_condition(impl->graph_guard_condition)) { + RMW_SET_ERROR_MSG("failed to destroy graph guard condition"); + result_ret = RMW_RET_ERROR; + } + + Domain::removeParticipant(participant); + + delete impl->listener; + impl->listener = nullptr; + delete impl; + + return result_ret; +} + +const rmw_guard_condition_t * +__rmw_node_get_graph_guard_condition(const rmw_node_t * node) +{ + auto impl = static_cast(node->data); + if (!impl) { + RMW_SET_ERROR_MSG("node impl is null"); + return nullptr; + } + return impl->graph_guard_condition; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp new file mode 100644 index 000000000..9bb5d64c2 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp @@ -0,0 +1,86 @@ +// 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 + +#include "rcutils/allocator.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/allocators.h" +#include "rmw/convert_rcutils_ret_to_rmw_ret.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/sanity_checks.h" + +#include "fastrtps/Domain.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + +using Participant = eprosima::fastrtps::Participant; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_get_node_names( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names) +{ + if (!node) { + RMW_SET_ERROR_MSG("null node handle"); + return RMW_RET_ERROR; + } + if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) { + return RMW_RET_ERROR; + } + + // Get participant pointer from node + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto impl = static_cast(node->data); + auto participant_names = impl->listener->get_discovered_names(); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcutils_ret_t rcutils_ret = + rcutils_string_array_init(node_names, participant_names.size() + 1, &allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RMW_SET_ERROR_MSG(rcutils_get_error_string_safe()) + return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + } + for (size_t i = 0; i < participant_names.size() + 1; ++i) { + if (0 == i) { + node_names->data[i] = rcutils_strdup(node->name, allocator); + } else { + node_names->data[i] = rcutils_strdup(participant_names[i - 1].c_str(), allocator); + } + if (!node_names->data[i]) { + RMW_SET_ERROR_MSG("failed to allocate memory for node name") + rcutils_ret = rcutils_string_array_fini(node_names); + if (rcutils_ret != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_shared_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string_safe()) + } + return RMW_RET_BAD_ALLOC; + } + } + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp new file mode 100644 index 000000000..5a67d58d3 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp @@ -0,0 +1,101 @@ +// 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 "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_publish( + const char * identifier, + const rmw_publisher_t * publisher, + const void * ros_message) +{ + auto error_allocator = rcutils_get_default_allocator(); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + publisher, "publisher pointer is null", return RMW_RET_ERROR, error_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + ros_message, "ros_message pointer is null", return RMW_RET_ERROR, error_allocator); + + if (publisher->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(publisher->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + info, "publisher info pointer is null", return RMW_RET_ERROR, error_allocator); + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_message); + if (!info->publisher_->write(&data)) { + RMW_SET_ERROR_MSG("cannot publish data"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_publish_serialized_message( + const char * identifier, + const rmw_publisher_t * publisher, + const rmw_serialized_message_t * serialized_message) +{ + auto error_allocator = rcutils_get_default_allocator(); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + publisher, "publisher pointer is null", return RMW_RET_ERROR, error_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + serialized_message, "serialized_message pointer is null", + return RMW_RET_ERROR, error_allocator); + + if (publisher->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(publisher->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + info, "publisher info pointer is null", return RMW_RET_ERROR, error_allocator); + + eprosima::fastcdr::FastBuffer buffer( + serialized_message->buffer, serialized_message->buffer_length); + eprosima::fastcdr::Cdr ser( + buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + if (!ser.jump(serialized_message->buffer_length)) { + RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); + return RMW_RET_ERROR; + } + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = &ser; + if (!info->publisher_->write(&data)) { + RMW_SET_ERROR_MSG("cannot publish data"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp new file mode 100644 index 000000000..8458e6033 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -0,0 +1,85 @@ +// 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 + +#include "fastrtps/Domain.h" +#include "fastrtps/participant/Participant.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "namespace_prefix.hpp" +#include "qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_destroy_publisher( + const char * identifier, + rmw_node_t * node, + rmw_publisher_t * publisher) +{ + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return RMW_RET_ERROR; + } + + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + if (!publisher) { + RMW_SET_ERROR_MSG("publisher handle is null"); + return RMW_RET_ERROR; + } + + if (publisher->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(publisher->data); + if (info != nullptr) { + if (info->publisher_ != nullptr) { + Domain::removePublisher(info->publisher_); + } + if (info->type_support_ != nullptr) { + auto impl = static_cast(node->data); + if (!impl) { + RMW_SET_ERROR_MSG("node impl is null"); + return RMW_RET_ERROR; + } + + Participant * participant = impl->participant; + _unregister_type(participant, info->type_support_); + } + delete info; + } + rmw_free(const_cast(publisher->topic_name)); + publisher->topic_name = nullptr; + rmw_publisher_free(publisher); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp new file mode 100644 index 000000000..3425008de --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -0,0 +1,112 @@ +// 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 + +#include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" + +#include "fastrtps/subscriber/Subscriber.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_send_request( + const char * identifier, + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id) +{ + assert(client); + assert(ros_request); + assert(sequence_id); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + if (client->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(client->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_request); + if (info->request_publisher_->write(&data, wparams)) { + returnedValue = RMW_RET_OK; + *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | + wparams.sample_identity().sequence_number().low; + } else { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; +} + +rmw_ret_t +__rmw_take_request( + const char * identifier, + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_request, + bool * taken) +{ + assert(service); + assert(request_header); + assert(ros_request); + assert(taken); + + *taken = false; + + if (service->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("service handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(service->data); + assert(info); + + CustomServiceRequest request = info->listener_->getRequest(); + + 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); + + // Get header + memcpy(request_header->writer_guid, &request.sample_identity_.writer_guid(), + sizeof(eprosima::fastrtps::rtps::GUID_t)); + request_header->sequence_number = ((int64_t)request.sample_identity_.sequence_number().high) << + 32 | request.sample_identity_.sequence_number().low; + + delete request.buffer_; + + *taken = true; + } + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp new file mode 100644 index 000000000..9287a423c --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -0,0 +1,113 @@ +// 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 + +#include "fastcdr/Cdr.h" + +#include "fastrtps/subscriber/Subscriber.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_take_response( + const char * identifier, + const rmw_client_t * client, + rmw_request_id_t * request_header, + void * ros_response, + bool * taken) +{ + assert(client); + assert(request_header); + assert(ros_response); + assert(taken); + + *taken = false; + + if (client->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("service handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(client->data); + assert(info); + + CustomClientResponse response; + + if (info->listener_->getResponse(response)) { + eprosima::fastcdr::Cdr deser( + *response.buffer_, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + info->response_type_support_->deserializeROSmessage(deser, ros_response); + + request_header->sequence_number = ((int64_t)response.sample_identity_.sequence_number().high) << + 32 | response.sample_identity_.sequence_number().low; + + *taken = true; + } + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_send_response( + const char * identifier, + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) +{ + assert(service); + assert(request_header); + assert(ros_response); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + if (service->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("service handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(service->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + memcpy(&wparams.related_sample_identity().writer_guid(), request_header->writer_guid, + sizeof(eprosima::fastrtps::rtps::GUID_t)); + wparams.related_sample_identity().sequence_number().high = + (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); + wparams.related_sample_identity().sequence_number().low = + (int32_t)(request_header->sequence_number & 0xFFFFFFFF); + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_response); + + if (info->response_publisher_->write(&data, wparams)) { + returnedValue = RMW_RET_OK; + } else { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp new file mode 100644 index 000000000..66691d79f --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -0,0 +1,90 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/rmw.h" + +#include "namespace_prefix.hpp" +#include "qos.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_destroy_service( + const char * identifier, + rmw_node_t * node, + rmw_service_t * service) +{ + (void)node; + if (!service) { + RMW_SET_ERROR_MSG("service handle is null"); + return RMW_RET_ERROR; + } + if (service->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + CustomServiceInfo * info = static_cast(service->data); + if (info != nullptr) { + if (info->request_subscriber_ != nullptr) { + Domain::removeSubscriber(info->request_subscriber_); + } + if (info->response_publisher_ != nullptr) { + Domain::removePublisher(info->response_publisher_); + } + if (info->listener_ != nullptr) { + delete info->listener_; + } + + if (info->request_type_support_ != nullptr) { + _unregister_type(info->participant_, info->request_type_support_); + } + if (info->response_type_support_ != nullptr) { + _unregister_type(info->participant_, info->response_type_support_); + } + delete info; + } + if (service->service_name != nullptr) { + rmw_free(const_cast(service->service_name)); + service->service_name = nullptr; + } + rmw_service_free(service); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service_names_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service_names_and_types.cpp new file mode 100644 index 000000000..91e5ac465 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_service_names_and_types.cpp @@ -0,0 +1,162 @@ +// 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 +#include +#include + +#include "rcutils/strdup.h" + +#include "rmw/allocators.h" +#include "rmw/convert_rcutils_ret_to_rmw_ret.h" +#include "rmw/error_handling.h" +#include "rmw/get_service_names_and_types.h" +#include "rmw/names_and_types.h" +#include "rmw/rmw.h" + +#include "demangle.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "reader_info.hpp" +#include "writer_info.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_get_service_names_and_types( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * service_names_and_types) +{ + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null") + return RMW_RET_INVALID_ARGUMENT; + } + if (!node) { + RMW_SET_ERROR_MSG_ALLOC("null node handle", *allocator) + return RMW_RET_INVALID_ARGUMENT; + } + rmw_ret_t ret = rmw_names_and_types_check_zero(service_names_and_types); + if (ret != RMW_RET_OK) { + return ret; + } + + // Get participant pointer from node + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG_ALLOC("node handle not from this implementation", *allocator); + return RMW_RET_ERROR; + } + + auto impl = static_cast(node->data); + + // Access the slave Listeners, which are the ones that have the topicnamesandtypes member + // Get info from publisher and subscriber + // Combined results from the two lists + std::map> services; + { + ReaderInfo * slave_target = impl->secondarySubListener; + slave_target->mapmutex.lock(); + for (auto it : slave_target->topicNtypes) { + std::string service_name = _demangle_service_from_topic(it.first); + if (!service_name.length()) { + // not a service + continue; + } + for (auto & itt : it.second) { + std::string service_type = _demangle_service_type_only(itt); + if (service_type.length()) { + services[service_name].insert(service_type); + } + } + } + slave_target->mapmutex.unlock(); + } + { + WriterInfo * slave_target = impl->secondaryPubListener; + slave_target->mapmutex.lock(); + for (auto it : slave_target->topicNtypes) { + std::string service_name = _demangle_service_from_topic(it.first); + if (!service_name.length()) { + // not a service + continue; + } + for (auto & itt : it.second) { + std::string service_type = _demangle_service_type_only(itt); + if (service_type.length()) { + services[service_name].insert(service_type); + } + } + } + slave_target->mapmutex.unlock(); + } + + // Fill out service_names_and_types + if (services.size()) { + // Setup string array to store names + rmw_ret_t rmw_ret = + rmw_names_and_types_init(service_names_and_types, services.size(), allocator); + if (rmw_ret != RMW_RET_OK) { + return rmw_ret; + } + // Setup cleanup function, in case of failure below + auto fail_cleanup = [&service_names_and_types]() { + rmw_ret_t rmw_ret = rmw_names_and_types_fini(service_names_and_types); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_shared_cpp", + "error during report of error: %s", rmw_get_error_string_safe()) + } + }; + // For each service, store the name, initialize the string array for types, and store all types + size_t index = 0; + for (const auto & service_n_types : services) { + // Duplicate and store the service_name + char * service_name = rcutils_strdup(service_n_types.first.c_str(), *allocator); + if (!service_name) { + RMW_SET_ERROR_MSG_ALLOC("failed to allocate memory for service name", *allocator); + fail_cleanup(); + return RMW_RET_BAD_ALLOC; + } + service_names_and_types->names.data[index] = service_name; + // Setup storage for types + { + rcutils_ret_t rcutils_ret = rcutils_string_array_init( + &service_names_and_types->types[index], + service_n_types.second.size(), + allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RMW_SET_ERROR_MSG(rcutils_get_error_string_safe()) + fail_cleanup(); + return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + } + } + // Duplicate and store each type for the service + size_t type_index = 0; + for (const auto & type : service_n_types.second) { + char * type_name = rcutils_strdup(type.c_str(), *allocator); + if (!type_name) { + RMW_SET_ERROR_MSG_ALLOC("failed to allocate memory for type name", *allocator) + fail_cleanup(); + return RMW_RET_BAD_ALLOC; + } + service_names_and_types->types[index].data[type_index] = type_name; + ++type_index; + } // for each type + ++index; + } // for each service + } + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp new file mode 100644 index 000000000..281509259 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp @@ -0,0 +1,111 @@ +// 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 + +#include "fastrtps/subscriber/Subscriber.h" + +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "demangle.hpp" +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_service_server_is_available( + const char * identifier, + const rmw_node_t * node, + const rmw_client_t * client, + bool * is_available) +{ + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return RMW_RET_ERROR; + } + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node handle, + node->implementation_identifier, identifier, + return RMW_RET_ERROR); + + if (!client) { + RMW_SET_ERROR_MSG("client handle is null"); + return RMW_RET_ERROR; + } + + if (!is_available) { + RMW_SET_ERROR_MSG("is_available is null"); + return RMW_RET_ERROR; + } + + auto client_info = static_cast(client->data); + if (!client_info) { + RMW_SET_ERROR_MSG("client info handle is null"); + return RMW_RET_ERROR; + } + + auto pub_topic_name = + client_info->request_publisher_->getAttributes().topic.getTopicName(); + + auto pub_fqdn = _demangle_if_ros_topic(pub_topic_name); + + auto sub_topic_name = + client_info->response_subscriber_->getAttributes().topic.getTopicName(); + + auto sub_fqdn = _demangle_if_ros_topic(sub_topic_name); + + *is_available = false; + size_t number_of_request_subscribers = 0; + rmw_ret_t ret = __rmw_count_subscribers( + identifier, + node, + pub_fqdn.c_str(), + &number_of_request_subscribers); + if (ret != RMW_RET_OK) { + // error string already set + return ret; + } + if (0 == number_of_request_subscribers) { + // not ready + return RMW_RET_OK; + } + + size_t number_of_response_publishers = 0; + ret = __rmw_count_publishers( + identifier, + node, + sub_fqdn.c_str(), + &number_of_response_publishers); + if (ret != RMW_RET_OK) { + // error string already set + return ret; + } + if (0 == number_of_response_publishers) { + // not ready + return RMW_RET_OK; + } + + // all conditions met, there is a service server available + *is_available = true; + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp new file mode 100644 index 000000000..1a52e8dec --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -0,0 +1,94 @@ +// 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 +#include + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "fastrtps/Domain.h" +#include "fastrtps/TopicDataType.h" +#include "fastrtps/participant/Participant.h" +#include "fastrtps/subscriber/Subscriber.h" + +#include "namespace_prefix.hpp" +#include "qos.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_destroy_subscription( + const char * identifier, + rmw_node_t * node, + rmw_subscription_t * subscription) +{ + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return RMW_RET_ERROR; + } + + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + if (!subscription) { + RMW_SET_ERROR_MSG("subscription handle is null"); + return RMW_RET_ERROR; + } + + if (subscription->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto info = static_cast(subscription->data); + + if (info != nullptr) { + if (info->subscriber_ != nullptr) { + Domain::removeSubscriber(info->subscriber_); + } + if (info->listener_ != nullptr) { + delete info->listener_; + } + if (info->type_support_ != nullptr) { + auto impl = static_cast(node->data); + if (!impl) { + RMW_SET_ERROR_MSG("node impl is null"); + return RMW_RET_ERROR; + } + + Participant * participant = impl->participant; + _unregister_type(participant, info->type_support_); + } + delete info; + } + rmw_free(const_cast(subscription->topic_name)); + subscription->topic_name = nullptr; + rmw_subscription_free(subscription); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp new file mode 100644 index 000000000..8a2dc6446 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -0,0 +1,213 @@ +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/serialized_message.h" +#include "rmw/rmw.h" + +#include "fastrtps/subscriber/Subscriber.h" +#include "fastrtps/subscriber/SampleInfo.h" +#include "fastrtps/attributes/SubscriberAttributes.h" + +#include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +void +_assign_message_info( + const char * identifier, + rmw_message_info_t * message_info, + const eprosima::fastrtps::SampleInfo_t * sinfo) +{ + rmw_gid_t * sender_gid = &message_info->publisher_gid; + sender_gid->implementation_identifier = identifier; + memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); + memcpy(sender_gid->data, &sinfo->sample_identity.writer_guid(), + sizeof(eprosima::fastrtps::rtps::GUID_t)); +} + +rmw_ret_t +_take( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info) +{ + *taken = false; + + if (subscription->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + CustomSubscriberInfo * info = static_cast(subscription->data); + auto error_msg_allocator = rcutils_get_default_allocator(); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + info, "custom subscriber info is null", return RMW_RET_ERROR, error_msg_allocator); + + eprosima::fastrtps::SampleInfo_t sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = ros_message; + if (info->subscriber_->takeNextData(&data, &sinfo)) { + info->listener_->data_taken(); + + if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { + if (message_info) { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; + } + } + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_take( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken) +{ + auto error_msg_allocator = rcutils_get_default_allocator(); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + subscription, "subscription pointer is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + ros_message, "ros_message pointer is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + taken, "boolean flag for taken is null", return RMW_RET_ERROR, error_msg_allocator); + + return _take(identifier, subscription, ros_message, taken, nullptr); +} + +rmw_ret_t +__rmw_take_with_info( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info) +{ + auto error_msg_allocator = rcutils_get_default_allocator(); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + subscription, "subscription pointer is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + ros_message, "ros_message pointer is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + taken, "boolean flag for taken is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + message_info, "message info pointer is null", return RMW_RET_ERROR, error_msg_allocator); + + return _take(identifier, subscription, ros_message, taken, message_info); +} + +rmw_ret_t +_take_serialized_message( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info) +{ + *taken = false; + + if (subscription->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + + CustomSubscriberInfo * info = static_cast(subscription->data); + auto error_msg_allocator = rcutils_get_default_allocator(); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + info, "custom subscriber info is null", return RMW_RET_ERROR, error_msg_allocator); + + eprosima::fastcdr::FastBuffer buffer; + eprosima::fastrtps::SampleInfo_t sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = &buffer; + if (info->subscriber_->takeNextData(&data, &sinfo)) { + info->listener_->data_taken(); + + if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { + auto buffer_size = static_cast(buffer.getBufferSize()); + if (serialized_message->buffer_capacity < buffer_size) { + auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); + if (ret != RMW_RET_OK) { + return ret; // Error message already set + } + } + serialized_message->buffer_length = buffer_size; + memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); + + if (message_info) { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; + } + } + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_take_serialized_message( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken) +{ + auto error_msg_allocator = rcutils_get_default_allocator(); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + subscription, "subscription pointer is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + serialized_message, "ros_message pointer is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + taken, "boolean flag for taken is null", return RMW_RET_ERROR, error_msg_allocator); + + return _take_serialized_message(identifier, subscription, serialized_message, taken, nullptr); +} + +rmw_ret_t +__rmw_take_serialized_message_with_info( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info) +{ + auto error_msg_allocator = rcutils_get_default_allocator(); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + subscription, "subscription pointer is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + serialized_message, "ros_message pointer is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + taken, "boolean flag for taken is null", return RMW_RET_ERROR, error_msg_allocator); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + message_info, "message info pointer is null", return RMW_RET_ERROR, error_msg_allocator); + + return _take_serialized_message( + identifier, subscription, serialized_message, taken, message_info); +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp new file mode 100644 index 000000000..a10ef72ee --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp @@ -0,0 +1,172 @@ +// 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 +#include +#include + +#include "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/allocators.h" +#include "rmw/convert_rcutils_ret_to_rmw_ret.h" +#include "rmw/error_handling.h" +#include "rmw/get_topic_names_and_types.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/names_and_types.h" +#include "rmw/rmw.h" + +#include "demangle.hpp" +#include "namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "reader_info.hpp" +#include "writer_info.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_get_topic_names_and_types( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null") + return RMW_RET_INVALID_ARGUMENT; + } + if (!node) { + RMW_SET_ERROR_MSG_ALLOC("null node handle", *allocator) + return RMW_RET_INVALID_ARGUMENT; + } + + rmw_ret_t ret = rmw_names_and_types_check_zero(topic_names_and_types); + if (ret != RMW_RET_OK) { + return ret; + } + + // Get participant pointer from node + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG_ALLOC("node handle not from this implementation", *allocator); + return RMW_RET_ERROR; + } + + auto impl = static_cast(node->data); + + // Access the slave Listeners, which are the ones that have the topicnamesandtypes member + // Get info from publisher and subscriber + // Combined results from the two lists + std::map> topics; + { + ReaderInfo * slave_target = impl->secondarySubListener; + slave_target->mapmutex.lock(); + for (auto it : slave_target->topicNtypes) { + if (!no_demangle && _get_ros_prefix_if_exists(it.first) != ros_topic_prefix) { + // if we are demangling and this is not prefixed with rt/, skip it + continue; + } + for (auto & itt : it.second) { + topics[it.first].insert(itt); + } + } + slave_target->mapmutex.unlock(); + } + { + WriterInfo * slave_target = impl->secondaryPubListener; + slave_target->mapmutex.lock(); + for (auto it : slave_target->topicNtypes) { + if (!no_demangle && _get_ros_prefix_if_exists(it.first) != ros_topic_prefix) { + // if we are demangling and this is not prefixed with rt/, skip it + continue; + } + for (auto & itt : it.second) { + topics[it.first].insert(itt); + } + } + slave_target->mapmutex.unlock(); + } + + // Copy data to results handle + if (topics.size() > 0) { + // Setup string array to store names + rmw_ret_t rmw_ret = rmw_names_and_types_init(topic_names_and_types, topics.size(), allocator); + if (rmw_ret != RMW_RET_OK) { + return rmw_ret; + } + // Setup cleanup function, in case of failure below + auto fail_cleanup = [&topic_names_and_types]() { + rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_shared_cpp", + "error during report of error: %s", rmw_get_error_string_safe()) + } + }; + // Setup demangling functions based on no_demangle option + auto demangle_topic = _demangle_if_ros_topic; + auto demangle_type = _demangle_if_ros_type; + if (no_demangle) { + auto noop = [](const std::string & in) { + return in; + }; + demangle_topic = noop; + demangle_type = noop; + } + // For each topic, store the name, initialize the string array for types, and store all types + size_t index = 0; + for (const auto & topic_n_types : topics) { + // Duplicate and store the topic_name + char * topic_name = rcutils_strdup(demangle_topic(topic_n_types.first).c_str(), *allocator); + if (!topic_name) { + RMW_SET_ERROR_MSG_ALLOC("failed to allocate memory for topic name", *allocator); + fail_cleanup(); + return RMW_RET_BAD_ALLOC; + } + topic_names_and_types->names.data[index] = topic_name; + // Setup storage for types + { + rcutils_ret_t rcutils_ret = rcutils_string_array_init( + &topic_names_and_types->types[index], + topic_n_types.second.size(), + allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RMW_SET_ERROR_MSG(rcutils_get_error_string_safe()) + fail_cleanup(); + return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + } + } + // Duplicate and store each type for the topic + size_t type_index = 0; + for (const auto & type : topic_n_types.second) { + char * type_name = rcutils_strdup(demangle_type(type).c_str(), *allocator); + if (!type_name) { + RMW_SET_ERROR_MSG_ALLOC("failed to allocate memory for type name", *allocator) + fail_cleanup(); + return RMW_RET_BAD_ALLOC; + } + topic_names_and_types->types[index].data[type_index] = type_name; + ++type_index; + } // for each type + ++index; + } // for each topic + } + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp b/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp new file mode 100644 index 000000000..ac3c72013 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp @@ -0,0 +1,41 @@ +// 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 + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "types/guard_condition.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_trigger_guard_condition( + const char * identifier, + const rmw_guard_condition_t * guard_condition_handle) +{ + assert(guard_condition_handle); + + if (guard_condition_handle->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("guard condition handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto guard_condition = static_cast(guard_condition_handle->data); + guard_condition->trigger(); + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp new file mode 100644 index 000000000..3c3c4a837 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -0,0 +1,219 @@ +// 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 "fastrtps/subscriber/Subscriber.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "types/custom_wait_set_info.hpp" +#include "types/guard_condition.hpp" + +// helper function for wait +bool +check_wait_set_for_data( + const rmw_subscriptions_t * subscriptions, + const rmw_guard_conditions_t * guard_conditions, + const rmw_services_t * services, + const rmw_clients_t * clients) +{ + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + // Short circuiting out of this function is possible + if (custom_subscriber_info && custom_subscriber_info->listener_->hasData()) { + return true; + } + } + } + + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + CustomClientInfo * custom_client_info = static_cast(data); + if (custom_client_info && custom_client_info->listener_->hasData()) { + return true; + } + } + } + + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + CustomServiceInfo * custom_service_info = static_cast(data); + if (custom_service_info && custom_service_info->listener_->hasData()) { + return true; + } + } + } + + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + if (guard_condition && guard_condition->hasTriggered()) { + return true; + } + } + } + return false; +} + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_wait( + rmw_subscriptions_t * subscriptions, + rmw_guard_conditions_t * guard_conditions, + rmw_services_t * services, + rmw_clients_t * clients, + rmw_wait_set_t * wait_set, + const rmw_time_t * wait_timeout) +{ + if (!wait_set) { + RMW_SET_ERROR_MSG("wait set handle is null"); + return RMW_RET_ERROR; + } + CustomWaitsetInfo * wait_set_info = static_cast(wait_set->data); + if (!wait_set_info) { + RMW_SET_ERROR_MSG("Waitset info struct is null"); + return RMW_RET_ERROR; + } + std::mutex * conditionMutex = &wait_set_info->condition_mutex; + std::condition_variable * conditionVariable = &wait_set_info->condition; + if (!conditionMutex) { + RMW_SET_ERROR_MSG("Mutex for wait set was null"); + return RMW_RET_ERROR; + } + if (!conditionVariable) { + RMW_SET_ERROR_MSG("Condition variable for wait set was null"); + return RMW_RET_ERROR; + } + + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + custom_subscriber_info->listener_->attachCondition(conditionMutex, conditionVariable); + } + } + + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + CustomClientInfo * custom_client_info = static_cast(data); + custom_client_info->listener_->attachCondition(conditionMutex, conditionVariable); + } + } + + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + custom_service_info->listener_->attachCondition(conditionMutex, conditionVariable); + } + } + + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + guard_condition->attachCondition(conditionMutex, conditionVariable); + } + } + + // This mutex prevents any of the listeners + // to change the internal state and notify the condition + // between the call to hasData() / hasTriggered() and wait() + // otherwise the decision to wait might be incorrect + std::unique_lock lock(*conditionMutex); + + bool hasData = check_wait_set_for_data(subscriptions, guard_conditions, services, clients); + auto predicate = [subscriptions, guard_conditions, services, clients]() { + return check_wait_set_for_data(subscriptions, guard_conditions, services, clients); + }; + + bool timeout = false; + if (!hasData) { + if (!wait_timeout) { + conditionVariable->wait(lock, predicate); + } else if (wait_timeout->sec > 0 || wait_timeout->nsec > 0) { + auto n = std::chrono::duration_cast( + std::chrono::seconds(wait_timeout->sec)); + n += std::chrono::nanoseconds(wait_timeout->nsec); + timeout = !conditionVariable->wait_for(lock, n, predicate); + } else { + timeout = true; + } + } + + // Unlock the condition variable mutex to prevent deadlocks that can occur if + // a listener triggers while the condition variable is being detached. + // Listeners will no longer be prevented from changing their internal state, + // but that should not cause issues (if a listener has data / has triggered + // after we check, it will be caught on the next call to this function). + lock.unlock(); + + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + custom_subscriber_info->listener_->detachCondition(); + if (!custom_subscriber_info->listener_->hasData()) { + subscriptions->subscribers[i] = 0; + } + } + } + + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + CustomClientInfo * custom_client_info = static_cast(data); + custom_client_info->listener_->detachCondition(); + if (!custom_client_info->listener_->hasData()) { + clients->clients[i] = 0; + } + } + } + + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + custom_service_info->listener_->detachCondition(); + if (!custom_service_info->listener_->hasData()) { + services->services[i] = 0; + } + } + } + + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + guard_condition->detachCondition(); + if (!guard_condition->getHasTriggered()) { + guard_conditions->guard_conditions[i] = 0; + } + } + } + + return timeout ? RMW_RET_TIMEOUT : RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp new file mode 100644 index 000000000..26ab3b9c5 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp @@ -0,0 +1,96 @@ +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/impl/cpp/macros.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "types/custom_wait_set_info.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_wait_set_t * +__rmw_create_wait_set(const char * identifier, size_t max_conditions) +{ + (void)max_conditions; + rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); + CustomWaitsetInfo * wait_set_info = nullptr; + + // From here onward, error results in unrolling in the goto fail block. + if (!wait_set) { + RMW_SET_ERROR_MSG("failed to allocate wait set"); + goto fail; + } + wait_set->implementation_identifier = identifier; + wait_set->data = rmw_allocate(sizeof(CustomWaitsetInfo)); + // This should default-construct the fields of CustomWaitsetInfo + wait_set_info = static_cast(wait_set->data); + RMW_TRY_PLACEMENT_NEW(wait_set_info, wait_set_info, goto fail, CustomWaitsetInfo, ) + if (!wait_set_info) { + RMW_SET_ERROR_MSG("failed to construct wait set info struct"); + goto fail; + } + + return wait_set; + +fail: + if (wait_set) { + if (wait_set->data) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + wait_set_info->~CustomWaitsetInfo(), wait_set_info) + rmw_free(wait_set->data); + } + rmw_wait_set_free(wait_set); + } + return nullptr; +} + +rmw_ret_t +__rmw_destroy_wait_set(const char * identifier, rmw_wait_set_t * wait_set) +{ + if (!wait_set) { + RMW_SET_ERROR_MSG("wait set handle is null"); + return RMW_RET_ERROR; + } + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + wait set handle, + wait_set->implementation_identifier, identifier, + return RMW_RET_ERROR) + + auto result = RMW_RET_OK; + auto wait_set_info = static_cast(wait_set->data); + if (!wait_set_info) { + RMW_SET_ERROR_MSG("wait set info is null"); + return RMW_RET_ERROR; + } + std::mutex * conditionMutex = &wait_set_info->condition_mutex; + if (!conditionMutex) { + RMW_SET_ERROR_MSG("wait set mutex is null"); + return RMW_RET_ERROR; + } + + if (wait_set->data) { + if (wait_set_info) { + RMW_TRY_DESTRUCTOR( + wait_set_info->~CustomWaitsetInfo(), wait_set_info, result = RMW_RET_ERROR) + } + rmw_free(wait_set->data); + } + rmw_wait_set_free(wait_set); + return result; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_cpp/src/types/custom_wait_set_info.hpp b/rmw_fastrtps_shared_cpp/src/types/custom_wait_set_info.hpp similarity index 91% rename from rmw_fastrtps_cpp/src/types/custom_wait_set_info.hpp rename to rmw_fastrtps_shared_cpp/src/types/custom_wait_set_info.hpp index 0d53fd27d..1ebdd5716 100644 --- a/rmw_fastrtps_cpp/src/types/custom_wait_set_info.hpp +++ b/rmw_fastrtps_shared_cpp/src/types/custom_wait_set_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/src/types/guard_condition.hpp b/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp similarity index 96% rename from rmw_fastrtps_cpp/src/types/guard_condition.hpp rename to rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp index 6ae157726..b4a08cda1 100644 --- a/rmw_fastrtps_cpp/src/types/guard_condition.hpp +++ b/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/writer_info.hpp b/rmw_fastrtps_shared_cpp/src/writer_info.hpp similarity index 83% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/writer_info.hpp rename to rmw_fastrtps_shared_cpp/src/writer_info.hpp index e51604380..1fb894cb1 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/writer_info.hpp +++ b/rmw_fastrtps_shared_cpp/src/writer_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__WRITER_INFO_HPP_ -#define RMW_FASTRTPS_CPP__WRITER_INFO_HPP_ +#ifndef WRITER_INFO_HPP_ +#define WRITER_INFO_HPP_ #include #include @@ -25,12 +25,15 @@ #include "fastrtps/participant/Participant.h" #include "fastrtps/rtps/builtin/data/WriterProxyData.h" +#include "fastrtps/rtps/reader/ReaderListener.h" #include "fastrtps/rtps/reader/RTPSReader.h" #include "rcutils/logging_macros.h" #include "rmw/rmw.h" +#include "types/guard_condition.hpp" + class WriterInfo : public eprosima::fastrtps::rtps::ReaderListener { public: @@ -38,7 +41,7 @@ class WriterInfo : public eprosima::fastrtps::rtps::ReaderListener eprosima::fastrtps::Participant * participant, rmw_guard_condition_t * graph_guard_condition) : participant_(participant), - graph_guard_condition_(graph_guard_condition) + graph_guard_condition_(static_cast(graph_guard_condition->data)) {} void @@ -86,7 +89,7 @@ class WriterInfo : public eprosima::fastrtps::rtps::ReaderListener trigger = true; } else { RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_cpp", + "rmw_fastrtps_shared_cpp", "unexpected removal of subscription on topic '%s' with type '%s'", fqdn.c_str(), proxyData.typeName().c_str()); } @@ -95,19 +98,13 @@ class WriterInfo : public eprosima::fastrtps::rtps::ReaderListener mapmutex.unlock(); if (trigger) { - rmw_ret_t ret = rmw_trigger_guard_condition(graph_guard_condition_); - if (ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "failed to trigger graph guard condition: %s", - rmw_get_error_string_safe()) - } + graph_guard_condition_->trigger(); } } std::map> topicNtypes; std::mutex mapmutex; eprosima::fastrtps::Participant * participant_; - rmw_guard_condition_t * graph_guard_condition_; + GuardCondition * graph_guard_condition_; }; -#endif // RMW_FASTRTPS_CPP__WRITER_INFO_HPP_ +#endif // WRITER_INFO_HPP_ diff --git a/rosidl_typesupport_fastrtps_c/CMakeLists.txt b/rosidl_typesupport_fastrtps_c/CMakeLists.txt new file mode 100644 index 000000000..bed191d6f --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.5) + +project(rosidl_typesupport_fastrtps_c) + +set(FASTRTPS_STATIC_DISABLE $ENV{FASTRTPS_STATIC_DISABLE} + CACHE BOOL "If fastrtps Static should be disabled.") + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +find_package(fastrtps_cmake_module REQUIRED) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) +if(FASTRTPS_STATIC_DISABLE) + ament_package() + message(STATUS "fastrtps static rmw implementation explicitly disabled - skipping '${PROJECT_NAME}'") + return() +endif() + +find_package(ament_cmake_python REQUIRED) +find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) + +ament_export_dependencies(rmw) +ament_export_dependencies(rosidl_cmake) +ament_export_dependencies(rosidl_generator_c) +ament_export_dependencies(rosidl_generator_dds_idl) +ament_export_dependencies(rosidl_typesupport_fastrtps_cpp) +ament_export_include_directories(include) + +ament_python_install_package(${PROJECT_NAME}) + +add_library(${PROJECT_NAME} SHARED src/identifier.cpp) +if(WIN32) + target_compile_definitions(${PROJECT_NAME} + PRIVATE "ROSIDL_TYPESUPPORT_FASTRTPS_C_BUILDING_DLL") +endif() +target_include_directories(${PROJECT_NAME} PUBLIC include) +ament_target_dependencies(${PROJECT_NAME} "rosidl_typesupport_fastrtps_cpp") +ament_export_libraries(${PROJECT_NAME}) + +ament_index_register_resource("rosidl_typesupport_c") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package( + CONFIG_EXTRAS "rosidl_typesupport_fastrtps_c-extras.cmake.in" +) + +install( + PROGRAMS bin/rosidl_typesupport_fastrtps_c + DESTINATION lib/rosidl_typesupport_fastrtps_c +) + +install( + DIRECTORY cmake resource + DESTINATION share/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) diff --git a/rosidl_typesupport_fastrtps_c/bin/rosidl_typesupport_fastrtps_c b/rosidl_typesupport_fastrtps_c/bin/rosidl_typesupport_fastrtps_c new file mode 100644 index 000000000..4b0498fc8 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/bin/rosidl_typesupport_fastrtps_c @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 + +import argparse +import os +import sys + +from rosidl_cmake import read_generator_arguments +from rosidl_parser import UnknownMessageType +from rosidl_typesupport_fastrtps_c import generate_typesupport_fastrtps_c + + +def is_valid_file(parser, file_name): + if not os.path.exists(file_name): + parser.error("File does not exist: '{0}'".format(file_name)) + file_name_abs = os.path.abspath(file_name) + if not os.path.isfile(file_name_abs): + parser.error("Path exists but is not a file: '{0}'".format(file_name_abs)) + return file_name + + +def main(argv=sys.argv[1:]): + parser = argparse.ArgumentParser( + description='Generate the C interfaces for fastrtps.', + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument( + '--generator-arguments-file', + required=True, + help='The location of the file containing the generator arguments') + args = parser.parse_args(argv) + + generator_args = read_generator_arguments(args.generator_arguments_file) + + try: + rc = generate_typesupport_fastrtps_c(generator_args) + except UnknownMessageType as e: + print(str(e), file=sys.stderr) + return 1 + if rc: + return rc + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/rosidl_typesupport_fastrtps_c/cmake/rosidl_typesupport_fastrtps_c_generate_interfaces.cmake b/rosidl_typesupport_fastrtps_c/cmake/rosidl_typesupport_fastrtps_c_generate_interfaces.cmake new file mode 100644 index 000000000..5df3b57d8 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/cmake/rosidl_typesupport_fastrtps_c_generate_interfaces.cmake @@ -0,0 +1,278 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# 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. + +find_package(fastrtps_cmake_module QUIET) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +set(_dds_idl_files "") +set(_dds_idl_base_path "${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_dds_idl") +foreach(_idl_file ${rosidl_generate_interfaces_IDL_FILES}) + get_filename_component(_extension "${_idl_file}" EXT) + if(_extension STREQUAL ".msg") + get_filename_component(_parent_folder "${_idl_file}" DIRECTORY) + get_filename_component(_parent_folder "${_parent_folder}" NAME) + get_filename_component(_name "${_idl_file}" NAME_WE) + list(APPEND _dds_idl_files + "${_dds_idl_base_path}/${PROJECT_NAME}/${_parent_folder}/dds_fastrtps/${_name}_.idl") + endif() +endforeach() + +set(_output_path "${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_c/${PROJECT_NAME}") +set(_generated_msg_files "") +set(_generated_srv_files "") +foreach(_idl_file ${rosidl_generate_interfaces_IDL_FILES}) + get_filename_component(_extension "${_idl_file}" EXT) + get_filename_component(_msg_name "${_idl_file}" NAME_WE) + string_camel_case_to_lower_case_underscore("${_msg_name}" _header_name) + if(_extension STREQUAL ".msg") + get_filename_component(_parent_folder "${_idl_file}" DIRECTORY) + get_filename_component(_parent_folder "${_parent_folder}" NAME) + if(_parent_folder STREQUAL "msg") + set(_var2 "_generated_msg_files") + elseif(_parent_folder STREQUAL "srv") + set(_var2 "_generated_srv_files") + else() + message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") + endif() + list(APPEND ${_var2} "${_output_path}/${_parent_folder}/${_header_name}__rosidl_typesupport_fastrtps_c.h") + list(APPEND ${_var2} "${_output_path}/${_parent_folder}/dds_fastrtps_c/${_header_name}__type_support_c.cpp") + elseif(_extension STREQUAL ".srv") + list(APPEND _generated_srv_files "${_output_path}/srv/${_header_name}__rosidl_typesupport_fastrtps_c.h") + list(APPEND _generated_srv_files "${_output_path}/srv/dds_fastrtps_c/${_header_name}__type_support_c.cpp") + else() + message(FATAL_ERROR "Interface file with unknown extension: ${_idl_file}") + endif() +endforeach() + +# If not on Windows, disable some warnings with fastrtps's generated code +if(NOT WIN32) + set(_fastrtps_compile_flags) + if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(_fastrtps_compile_flags + "-Wno-deprecated-register" + "-Wno-return-type-c-linkage" + "-Wno-unused-parameter" + ) + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(_fastrtps_compile_flags + # no-strict-aliasing necessary only for Release builds + "-Wno-strict-aliasing" + "-Wno-unused-parameter" + ) + endif() + if(NOT _fastrtps_compile_flags STREQUAL "") + string(REPLACE ";" " " _fastrtps_compile_flags "${_fastrtps_compile_flags}") + endif() +endif() + +set(_dependency_files "") +set(_dependencies "") +foreach(_pkg_name ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES}) + foreach(_idl_file ${${_pkg_name}_INTERFACE_FILES}) + get_filename_component(_extension "${_idl_file}" EXT) + if(_extension STREQUAL ".msg") + get_filename_component(_parent_folder "${_idl_file}" DIRECTORY) + get_filename_component(_parent_folder "${_parent_folder}" NAME) + get_filename_component(_name "${_idl_file}" NAME_WE) + set(_abs_idl_file "${${_pkg_name}_DIR}/../${_parent_folder}/dds_fastrtps/${_name}_.idl") + normalize_path(_abs_idl_file "${_abs_idl_file}") + list(APPEND _dependency_files "${_abs_idl_file}") + set(_abs_idl_file "${${_pkg_name}_DIR}/../${_idl_file}") + normalize_path(_abs_idl_file "${_abs_idl_file}") + list(APPEND _dependencies "${_pkg_name}:${_abs_idl_file}") + endif() + endforeach() +endforeach() + +set(target_dependencies + "${rosidl_typesupport_fastrtps_c_BIN}" + ${rosidl_typesupport_fastrtps_c_GENERATOR_FILES} + "${rosidl_typesupport_fastrtps_c_TEMPLATE_DIR}/msg__type_support_c.cpp.em" + "${rosidl_typesupport_fastrtps_c_TEMPLATE_DIR}/srv__type_support_c.cpp.em" + ${_dependency_files}) +foreach(dep ${target_dependencies}) + if(NOT EXISTS "${dep}") + message(FATAL_ERROR "Target dependency '${dep}' does not exist") + endif() +endforeach() + +set(generator_arguments_file "${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_c__arguments.json") +rosidl_write_generator_arguments( + "${generator_arguments_file}" + PACKAGE_NAME "${PROJECT_NAME}" + ROS_INTERFACE_FILES "${rosidl_generate_interfaces_IDL_FILES}" + ROS_INTERFACE_DEPENDENCIES "${_dependencies}" + OUTPUT_DIR "${_output_path}" + TEMPLATE_DIR "${rosidl_typesupport_fastrtps_c_TEMPLATE_DIR}" + TARGET_DEPENDENCIES ${target_dependencies} + ADDITIONAL_FILES ${_dds_idl_files} +) + +add_custom_command( + OUTPUT ${_generated_msg_files} ${_generated_srv_files} + COMMAND ${PYTHON_EXECUTABLE} ${rosidl_typesupport_fastrtps_c_BIN} + --generator-arguments-file "${generator_arguments_file}" + DEPENDS ${target_dependencies} ${_dds_idl_files} + COMMENT "Generating C type support for eProsima Fast-RTPS" + VERBATIM +) + +# generate header to switch between export and import for a specific package +set(_visibility_control_file +"${_output_path}/msg/rosidl_typesupport_fastrtps_c__visibility_control.h") +string(TOUPPER "${PROJECT_NAME}" PROJECT_NAME_UPPER) +configure_file( + "${rosidl_typesupport_fastrtps_c_TEMPLATE_DIR}/rosidl_typesupport_fastrtps_c__visibility_control.h.in" + "${_visibility_control_file}" + @ONLY +) + +set(_target_suffix "__rosidl_typesupport_fastrtps_c") + +link_directories(${fastrtps_LIBRARY_DIRS}) +add_library(${rosidl_generate_interfaces_TARGET}${_target_suffix} SHARED + ${_generated_msg_files} ${_generated_srv_files}) +if(rosidl_generate_interfaces_LIBRARY_NAME) + set_target_properties(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PROPERTIES OUTPUT_NAME "${rosidl_generate_interfaces_LIBRARY_NAME}${_target_suffix}") +endif() +set_target_properties(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PROPERTIES CXX_STANDARD 14) +if(fastrtps_GLIBCXX_USE_CXX11_ABI_ZERO) + target_compile_definitions(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PRIVATE fastrtps_GLIBCXX_USE_CXX11_ABI_ZERO) +endif() +ament_target_dependencies(${rosidl_generate_interfaces_TARGET}${_target_suffix} + "fastrtps" + "rmw" + "rosidl_typesupport_fastrtps_cpp" + "rosidl_typesupport_fastrtps_c" + "rosidl_generator_c" + "rosidl_typesupport_interface") +if(WIN32) + target_compile_definitions(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PRIVATE "ROSIDL_TYPESUPPORT_FASTRTPS_C_BUILDING_DLL_${PROJECT_NAME}") + target_compile_definitions(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PRIVATE "NDDS_USER_DLL_EXPORT_${PROJECT_NAME}") +endif() + +if(NOT WIN32) + set(_target_compile_flags "-Wall -Wextra -Wpedantic") +else() + set(_target_compile_flags + "/W4" # Enable level 3 warnings + "/wd4100" # Ignore unreferenced formal parameter warnings + "/wd4127" # Ignore conditional expression is constant warnings + "/wd4275" # Ignore "an exported class derived from a non-exported class" warnings + "/wd4305" # Ignore "initializing: truncation from..." warnings + "/wd4458" # Ignore class hides member variable warnings + "/wd4701" # Ignore unused variable warnings + ) +endif() +string(REPLACE ";" " " _target_compile_flags "${_target_compile_flags}") +set_target_properties(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PROPERTIES COMPILE_FLAGS "${_target_compile_flags}") +target_include_directories(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PUBLIC + ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_c + ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp + ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_c + ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_cpp +) +ament_target_dependencies(${rosidl_generate_interfaces_TARGET}${_target_suffix} + "fastrtps" + "rosidl_typesupport_fastrtps_cpp" + "rosidl_typesupport_fastrtps_c" + "${PROJECT_NAME}__rosidl_typesupport_fastrtps_cpp") +foreach(_pkg_name ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES}) + set(_msg_include_dir "${${_pkg_name}_DIR}/../../../include/${_pkg_name}/msg/dds_fastrtps_c") + set(_srv_include_dir "${${_pkg_name}_DIR}/../../../include/${_pkg_name}/srv/dds_fastrtps_c") + normalize_path(_msg_include_dir "${_msg_include_dir}") + normalize_path(_srv_include_dir "${_srv_include_dir}") + target_include_directories(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PUBLIC + "${_msg_include_dir}" + "${_srv_include_dir}" + ) + ament_target_dependencies(${rosidl_generate_interfaces_TARGET}${_target_suffix} + ${_pkg_name}) +endforeach() +target_link_libraries(${rosidl_generate_interfaces_TARGET}${_target_suffix} + ${rosidl_generate_interfaces_TARGET}__rosidl_generator_c + ${rosidl_generate_interfaces_TARGET}__rosidl_typesupport_fastrtps_cpp) + +add_dependencies( + ${rosidl_generate_interfaces_TARGET} + ${rosidl_generate_interfaces_TARGET}${_target_suffix} +) +add_dependencies( + ${rosidl_generate_interfaces_TARGET}${_target_suffix} + ${rosidl_generate_interfaces_TARGET}__rosidl_typesupport_fastrtps_cpp +) +add_dependencies( + ${rosidl_generate_interfaces_TARGET}${_target_suffix} + ${rosidl_generate_interfaces_TARGET}__cpp +) +add_dependencies( + ${rosidl_generate_interfaces_TARGET}${_target_suffix} + ${rosidl_generate_interfaces_TARGET}__dds_fastrtps_idl +) + +if(NOT rosidl_generate_interfaces_SKIP_INSTALL) + install( + DIRECTORY "${_output_path}/" + DESTINATION "include/${PROJECT_NAME}" + PATTERN "*.cpp" EXCLUDE + ) + + if(NOT _generated_msg_files STREQUAL "" OR NOT _generated_srv_files STREQUAL "") + ament_export_include_directories(include) + endif() + + install( + TARGETS ${rosidl_generate_interfaces_TARGET}${_target_suffix} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + ament_export_libraries(${rosidl_generate_interfaces_TARGET}${_target_suffix}) +endif() + +if(BUILD_TESTING AND rosidl_generate_interfaces_ADD_LINTER_TESTS) + if(NOT _generated_msg_files STREQUAL "" OR NOT _generated_srv_files STREQUAL "") + find_package(ament_cmake_cppcheck REQUIRED) + ament_cppcheck( + TESTNAME "cppcheck_rosidl_typesupport_fastrtps_c" + ${_generated_msg_files} ${_generated_srv_files}) + + find_package(ament_cmake_cpplint REQUIRED) + get_filename_component(_cpplint_root "${_output_path}" DIRECTORY) + ament_cpplint( + TESTNAME "cpplint_rosidl_typesupport_fastrtps_c" + # the generated code might contain longer lines for templated types + MAX_LINE_LENGTH 999 + ROOT "${_cpplint_root}" + ${_generated_msg_files} ${_generated_srv_files}) + + find_package(ament_cmake_uncrustify REQUIRED) + ament_uncrustify( + TESTNAME "uncrustify_rosidl_typesupport_fastrtps_c" + # the generated code might contain longer lines for templated types + MAX_LINE_LENGTH 999 + ${_generated_msg_files} ${_generated_srv_files}) + endif() +endif() diff --git a/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/identifier.h b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/identifier.h new file mode 100644 index 000000000..0ac3c9ae0 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/identifier.h @@ -0,0 +1,32 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// 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 ROSIDL_TYPESUPPORT_FASTRTPS_C__IDENTIFIER_H_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_C__IDENTIFIER_H_ + +#include "rosidl_typesupport_fastrtps_c/visibility_control.h" + +#if __cplusplus +extern "C" +{ +#endif + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC +extern const char * rosidl_typesupport_fastrtps_c__identifier; + +#if __cplusplus +} +#endif + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_C__IDENTIFIER_H_ diff --git a/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/visibility_control.h b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/visibility_control.h new file mode 100644 index 000000000..d0e373466 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/include/rosidl_typesupport_fastrtps_c/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// 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 ROSIDL_TYPESUPPORT_FASTRTPS_C__VISIBILITY_CONTROL_H_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_C__VISIBILITY_CONTROL_H_ + +#if __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_EXPORT __attribute__ ((dllexport)) + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT __attribute__ ((dllimport)) + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_EXPORT __declspec(dllexport) + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT __declspec(dllimport) + #endif + #ifdef ROSIDL_TYPESUPPORT_FASTRTPS_C_BUILDING_DLL + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC ROSIDL_TYPESUPPORT_FASTRTPS_C_EXPORT + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT + #endif + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_LOCAL +#else + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_EXPORT __attribute__ ((visibility("default"))) + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT + #if __GNUC__ >= 4 + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC __attribute__ ((visibility("default"))) + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_LOCAL + #endif +#endif + +#if __cplusplus +} +#endif + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_C__VISIBILITY_CONTROL_H_ diff --git a/rosidl_typesupport_fastrtps_c/package.xml b/rosidl_typesupport_fastrtps_c/package.xml new file mode 100644 index 000000000..bb0d45582 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/package.xml @@ -0,0 +1,41 @@ + + + + rosidl_typesupport_fastrtps_c + 0.5.1 + Generate the C interfaces for eProsima FastRTPS. + Dirk Thomas + Apache License 2.0 + Ricardo González + + ament_cmake + fastrtps_cmake_module + fastcdr + fastrtps + rosidl_cmake + rosidl_generator_c + rosidl_typesupport_fastrtps_cpp + + ament_cmake + fastrtps_cmake_module + fastcdr + fastrtps + rosidl_cmake + rosidl_generator_c + rosidl_generator_dds_idl + rosidl_typesupport_fastrtps_cpp + + rmw + + rosidl_parser + rosidl_typesupport_interface + + ament_lint_auto + ament_lint_common + + rosidl_typesupport_c_packages + + + ament_cmake + + diff --git a/rosidl_typesupport_fastrtps_c/resource/msg__rosidl_typesupport_fastrtps_c.h.em b/rosidl_typesupport_fastrtps_c/resource/msg__rosidl_typesupport_fastrtps_c.h.em new file mode 100644 index 000000000..67838c0d8 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/resource/msg__rosidl_typesupport_fastrtps_c.h.em @@ -0,0 +1,57 @@ +// generated from +// rosidl_typesupport_fastrtps_c/resource/msg__rosidl_typesupport_fastrtps_c.h.em +// generated code does not contain a copyright notice + +@####################################################################### +@# EmPy template for generating +@# __rosidl_typesupport_fastrtps_c.h files +@# +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .msg file +@# - subfolder (string) +@# The subfolder / subnamespace of the message +@# Either 'msg' or 'srv' +@# - get_header_filename_from_msg_name (function) +@####################################################################### +@ +@{ +header_guard_parts = [ + spec.base_type.pkg_name, subfolder, + get_header_filename_from_msg_name(spec.base_type.type) + '__rosidl_typesupport_fastrtps_c_h'] +header_guard_variable = '__'.join([x.upper() for x in header_guard_parts]) + '_' +}@ +#ifndef @(header_guard_variable) +#define @(header_guard_variable) + +#include + +#include "rosidl_generator_c/message_type_support_struct.h" +#include "rosidl_typesupport_interface/macros.h" + +#include "@(spec.base_type.pkg_name)/msg/rosidl_typesupport_fastrtps_c__visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(spec.base_type.pkg_name) +size_t get_serialized_size_@(spec.base_type.pkg_name)__@(subfolder)__@(spec.base_type.type)( + const void * untyped_ros_message, + size_t current_alignment); + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(spec.base_type.pkg_name) +size_t max_serialized_size_@(spec.base_type.pkg_name)__@(subfolder)__@(spec.base_type.type)( + bool & full_bounded, + size_t current_alignment); + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(spec.base_type.pkg_name) +const rosidl_message_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(spec.base_type.pkg_name), @(subfolder), @(spec.base_type.type))(); + +#ifdef __cplusplus +} +#endif + +#endif // @(header_guard_variable) diff --git a/rosidl_typesupport_fastrtps_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_fastrtps_c/resource/msg__type_support_c.cpp.em new file mode 100644 index 000000000..256414ed6 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/resource/msg__type_support_c.cpp.em @@ -0,0 +1,493 @@ +// generated from rosidl_typesupport_fastrtps_c/resource/msg__type_support_c.cpp.em +// generated code does not contain a copyright notice + +@########################################################################## +@# EmPy template for generating __type_support_c.cpp files for fastrtps +@# +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .msg file +@# - pkg (string) +@# name of the containing package; equivalent to spec.base_type.pkg_name +@# - msg (string) +@# name of the message; equivalent to spec.msg_name +@# - type (string) +@# full type of the message; equivalent to spec.base_type.type +@# - subfolder (string) +@# The subfolder / subnamespace of the message +@# Either 'msg' or 'srv' +@# - get_header_filename_from_msg_name (function) +@########################################################################## +@ +#include "@(spec.base_type.pkg_name)/@(subfolder)/@(get_header_filename_from_msg_name(spec.base_type.type))__rosidl_typesupport_fastrtps_c.h" + +#include +#include +#include + +// Provides the rosidl_typesupport_fastrtps_c__identifier symbol declaration. +#include "rosidl_typesupport_fastrtps_c/identifier.h" +// Provides the definition of the message_type_support_callbacks_t struct. +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" + +#include "@(pkg)/msg/rosidl_typesupport_fastrtps_c__visibility_control.h" +@{header_file_name = get_header_filename_from_msg_name(type)}@ +#include "@(pkg)/@(subfolder)/@(header_file_name)__struct.h" +#include "@(pkg)/@(subfolder)/@(header_file_name)__functions.h" + +#include "fastcdr/Cdr.h" + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +# ifdef __clang__ +# pragma clang diagnostic ignored "-Wdeprecated-register" +# pragma clang diagnostic ignored "-Wreturn-type-c-linkage" +# endif +#endif +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +// includes and forward declarations of message dependencies and their conversion functions + +@# // Include the message header for each non-primitive field. +#if defined(__cplusplus) +extern "C" +{ +#endif + +@{ +includes = {} +for field in spec.fields: + keys = set([]) + if field.type.is_primitive_type(): + if field.type.is_array: + keys.add('rosidl_generator_c/primitives_array.h') + keys.add('rosidl_generator_c/primitives_array_functions.h') + if field.type.type == 'string': + keys.add('rosidl_generator_c/string.h') + keys.add('rosidl_generator_c/string_functions.h') + else: + header_file_name = get_header_filename_from_msg_name(field.type.type) + keys.add('%s/msg/%s__functions.h' % (field.type.pkg_name, header_file_name)) + for key in keys: + if key not in includes: + includes[key] = set([]) + includes[key].add(field.name) +}@ +@[for key in sorted(includes.keys())]@ +#include "@(key)" // @(', '.join(includes[key])) +@[end for]@ + +// forward declare type support functions +@{ +forward_declares = {} +for field in spec.fields: + if not field.type.is_primitive_type(): + key = (field.type.pkg_name, field.type.type) + if key not in includes: + forward_declares[key] = set([]) + forward_declares[key].add(field.name) +}@ +@[for key in sorted(forward_declares.keys())]@ +@[ if key[0] != pkg]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(pkg) +@[ end if]@ +size_t get_serialized_size_@(key[0])__msg__@(key[1])( + const void * untyped_ros_message, + size_t current_alignment); + +@[ if key[0] != pkg]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(pkg) +@[ end if]@ +size_t max_serialized_size_@(key[0])__msg__@(key[1])( + bool & full_bounded, + size_t current_alignment); + +@[ if key[0] != pkg]@ +ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@(pkg) +@[ end if]@ +const rosidl_message_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(key[0]), msg, @(key[1]))(); +@[end for]@ + +@# // Make callback functions specific to this message type. + +using __ros_msg_type = @(pkg)__@(subfolder)__@(type); + +static bool __cdr_serialize( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr) +{ + if (!untyped_ros_message) { + fprintf(stderr, "ros message handle is null\n"); + return false; + } +@[if not spec.fields]@ + // No fields is a no-op. + (void)cdr; + (void)untyped_ros_message; +@[else]@ + const __ros_msg_type * ros_message = static_cast(untyped_ros_message); +@[end if]@ +@[for field in spec.fields]@ + // Field name: @(field.name) + { +@[ if not field.type.is_primitive_type()]@ + const message_type_support_callbacks_t * @(field.type.pkg_name)__msg__@(field.type.type)__callbacks = + static_cast( + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(field.type.pkg_name), msg, @(field.type.type) + )()->data); +@[ end if]@ +@[ if field.type.is_array]@ +@[ if field.type.array_size and not field.type.is_upper_bound]@ + size_t size = @(field.type.array_size); + auto array_ptr = ros_message->@(field.name); +@[ else]@ + size_t size = ros_message->@(field.name).size; + auto array_ptr = ros_message->@(field.name).data; +@[ if field.type.is_upper_bound]@ + if (size > @(field.type.array_size)) { + fprintf(stderr, "array size exceeds upper bound\n"); + return false; + } +@[ end if]@ + cdr << static_cast(size); +@[ end if]@ +@[ if field.type.type == 'string']@ + for (size_t i = 0; i < size; ++i) { + const rosidl_generator_c__String * str = &array_ptr[i]; + if (str->capacity == 0 || str->capacity <= str->size) { + fprintf(stderr, "string capacity not greater than size\n"); + return false; + } + if (str->data[str->size] != '\0') { + fprintf(stderr, "string not null-terminated\n"); + return false; + } + cdr << str->data; + } +@[ elif field.type.is_primitive_type()]@ + cdr.serializeArray(array_ptr, size); +@[ else]@ + for (size_t i = 0; i < size; ++i) { + if (!@(field.type.pkg_name)__msg__@(field.type.type)__callbacks->cdr_serialize( + &array_ptr[i], cdr)) + { + return false; + } + } +@[ end if]@ +@[ elif field.type.type == 'string']@ + const rosidl_generator_c__String * str = &ros_message->@(field.name); + if (str->capacity == 0 || str->capacity <= str->size) { + fprintf(stderr, "string capacity not greater than size\n"); + return false; + } + if (str->data[str->size] != '\0') { + fprintf(stderr, "string not null-terminated\n"); + return false; + } + cdr << str->data; +@[ elif field.type.type == 'bool']@ + cdr << (ros_message->@(field.name) ? true : false); +@[ elif field.type.is_primitive_type()]@ + cdr << ros_message->@(field.name); +@[ else]@ + if (!@(field.type.pkg_name)__msg__@(field.type.type)__callbacks->cdr_serialize( + &ros_message->@(field.name), cdr)) + { + return false; + } +@[ end if]@ + } + +@[end for]@ + return true; +} + +static bool __cdr_deserialize( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message) +{ + if (!untyped_ros_message) { + fprintf(stderr, "ros message handle is null\n"); + return false; + } +@[if not spec.fields]@ + // No fields is a no-op. + (void)cdr; + (void)untyped_ros_message; +@[else]@ + __ros_msg_type * ros_message = static_cast<__ros_msg_type *>(untyped_ros_message); +@[end if]@ +@[for field in spec.fields]@ + // Field name: @(field.name) + { +@[ if not field.type.is_primitive_type()]@ + const message_type_support_callbacks_t * @(field.type.pkg_name)__msg__@(field.type.type)__callbacks = + static_cast( + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(field.type.pkg_name), msg, @(field.type.type) + )()->data); +@[ end if]@ +@[ if field.type.is_array]@ +@[ if field.type.array_size and not field.type.is_upper_bound]@ + size_t size = @(field.type.array_size); + auto array_ptr = ros_message->@(field.name); +@[ else]@ +@{ +if field.type.type == 'string': + array_init = 'rosidl_generator_c__String__Array__init' + array_fini = 'rosidl_generator_c__String__Array__fini' +elif field.type.is_primitive_type(): + array_init = 'rosidl_generator_c__{field.type.type}__Array__init'.format(**locals()) + array_fini = 'rosidl_generator_c__{field.type.type}__Array__fini'.format(**locals()) +else: + array_init = '{field.type.pkg_name}__msg__{field.type.type}__Array__init'.format(**locals()) + array_fini = '{field.type.pkg_name}__msg__{field.type.type}__Array__fini'.format(**locals()) +}@ + uint32_t cdrSize; + cdr >> cdrSize; + size_t size = static_cast(cdrSize); + if (ros_message->@(field.name).data) { + @(array_fini)(&ros_message->@(field.name)); + } + if (!@(array_init)(&ros_message->@(field.name), size)) { + return "failed to create array for field '@(field.name)'"; + } + auto array_ptr = ros_message->@(field.name).data; +@[ end if]@ +@[ if field.type.type == 'string']@ + for (size_t i = 0; i < size; ++i) { + std::string tmp; + cdr >> tmp; + auto & ros_i = array_ptr[i]; + if (!ros_i.data) { + rosidl_generator_c__String__init(&ros_i); + } + bool succeeded = rosidl_generator_c__String__assign( + &ros_i, + tmp.c_str()); + if (!succeeded) { + fprintf(stderr, "failed to assign string into field '@(field.name)'\n"); + return false; + } + } +@[ elif field.type.is_primitive_type()]@ + cdr.deserializeArray(array_ptr, size); +@[ else]@ + for (size_t i = 0; i < size; ++i) { + if (!@(field.type.pkg_name)__msg__@(field.type.type)__callbacks->cdr_deserialize( + cdr, &array_ptr[i])) + { + return false; + } + } +@[ end if]@ +@[ elif field.type.type == 'string']@ + std::string tmp; + cdr >> tmp; + if (!ros_message->@(field.name).data) { + rosidl_generator_c__String__init(&ros_message->@(field.name)); + } + bool succeeded = rosidl_generator_c__String__assign( + &ros_message->@(field.name), + tmp.c_str()); + if (!succeeded) { + fprintf(stderr, "failed to assign string into field '@(field.name)'\n"); + return false; + } +@[ elif field.type.is_primitive_type()]@ + cdr >> ros_message->@(field.name); +@[ else]@ + if (!@(field.type.pkg_name)__msg__@(field.type.type)__callbacks->cdr_deserialize( + cdr, &ros_message->@(field.name))) + { + return false; + } +@[ end if]@ + } + +@[end for]@ + return true; +} + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(spec.base_type.pkg_name) +size_t get_serialized_size_@(spec.base_type.pkg_name)__@(subfolder)__@(spec.base_type.type)( + const void * untyped_ros_message, + size_t current_alignment) +{ +@[if not spec.fields]@ + (void)untyped_ros_message; + (void)current_alignment; +@[else]@ + const __ros_msg_type * ros_message = static_cast(untyped_ros_message); + (void)ros_message; + size_t initial_alignment = current_alignment; + + const size_t padding = 4; + (void)padding; +@[end if]@ + +@[for field in spec.fields]@ + // field.name @(field.name) +@[ if field.type.is_array]@ + { +@[ if field.type.array_size and not field.type.is_upper_bound]@ + size_t array_size = @(field.type.array_size); + auto array_ptr = ros_message->@(field.name); +@[ else]@ + size_t array_size = ros_message->@(field.name).size; + auto array_ptr = ros_message->@(field.name).data; + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); +@[ end if]@ +@[ if field.type.type == 'string']@ + for (size_t index = 0; index < array_size; ++index) { + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + array_ptr[index].size + 1; + } +@[ elif field.type.is_primitive_type()]@ + (void)array_ptr; + size_t item_size = sizeof(array_ptr[0]); + current_alignment += array_size * item_size + + eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); +@[ else] + for (size_t index = 0; index < array_size; ++index) { + current_alignment += get_serialized_size_@(field.type.pkg_name)__msg__@(field.type.type)( + &array_ptr[index], current_alignment); + } +@[ end if]@ + } +@[ else]@ +@[ if field.type.type == 'string']@ + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + ros_message->@(field.name).size + 1; +@[ elif field.type.is_primitive_type()]@ + { + size_t item_size = sizeof(ros_message->@(field.name)); + current_alignment += item_size + + eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + } +@[ else] + current_alignment += get_serialized_size_@(field.type.pkg_name)__msg__@(field.type.type)( + &(ros_message->@(field.name)), current_alignment); +@[ end if]@ +@[ end if]@ +@[end for]@ + +@[if not spec.fields]@ + return 0; +@[else]@ + return current_alignment - initial_alignment; +@[end if]@ +} + +static uint32_t __get_serialized_size(const void * untyped_ros_message) +{ + return static_cast( + get_serialized_size_@(spec.base_type.pkg_name)__@(subfolder)__@(spec.base_type.type)( + untyped_ros_message, 0)); +} + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(spec.base_type.pkg_name) +size_t max_serialized_size_@(spec.base_type.pkg_name)__@(subfolder)__@(spec.base_type.type)( + bool & full_bounded, + size_t current_alignment) +{ + size_t initial_alignment = current_alignment; + + const size_t padding = 4; + (void)padding; + (void)full_bounded; + +@[for field in spec.fields]@ + // field.name @(field.name) + { +@[ if field.type.is_array]@ +@[ if field.type.array_size]@ + size_t array_size = @(field.type.array_size); +@[ else]@ + size_t array_size = 0; +@[ end if]@ +@[ if not field.type.array_size or field.type.is_upper_bound]@ + full_bounded = false; + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); +@[ end if]@ +@[ else]@ + size_t array_size = 1; +@[ end if]@ + +@[ if field.type.type == 'string']@ + full_bounded = false; + for (size_t index = 0; index < array_size; ++index) { + current_alignment += padding + +@[ if field.type.string_upper_bound]@ + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + @(field.type.string_upper_bound) + 1; +@[ else]@ + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + 1; +@[ end if]@ + } +@[ elif field.type.is_primitive_type()]@ +@[ if field.type.type == 'bool' or field.type.type == 'byte' or field.type.type == 'char' or field.type.type == 'uint8' or field.type.type == 'int8' ] + current_alignment += array_size * sizeof(uint8_t); +@[ elif field.type.type == 'int16' or field.type.type == 'uint16'] + current_alignment += array_size * sizeof(uint16_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t)); +@[ elif field.type.type == 'int32' or field.type.type == 'uint32' or field.type.type == 'float32'] + current_alignment += array_size * sizeof(uint32_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); +@[ elif field.type.type == 'int64' or field.type.type == 'uint64' or field.type.type == 'float64'] + current_alignment += array_size * sizeof(uint64_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); +@[ end if]@ +@[ else] + for (size_t index = 0; index < array_size; ++index) { + current_alignment += + max_serialized_size_@(field.type.pkg_name)__msg__@(field.type.type)( + full_bounded, current_alignment); + } +@[ end if]@ + } +@[end for]@ + + return current_alignment - initial_alignment; +} + +static size_t __max_serialized_size(bool & full_bounded) +{ + return max_serialized_size_@(spec.base_type.pkg_name)__@(subfolder)__@(spec.base_type.type)( + full_bounded, 0); +} + +@ +@# // Collect the callback functions and provide a function to get the type support struct. + +static message_type_support_callbacks_t __callbacks = { + "@(pkg)", + "@(msg)", + __cdr_serialize, + __cdr_deserialize, + __get_serialized_size, + __max_serialized_size +}; + +static rosidl_message_type_support_t __type_support = { + rosidl_typesupport_fastrtps_c__identifier, + &__callbacks, + get_message_typesupport_handle_function, +}; + +const rosidl_message_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(pkg), @(subfolder), @(msg))() { + return &__type_support; +} + +#if defined(__cplusplus) +} +#endif diff --git a/rosidl_typesupport_fastrtps_c/resource/rosidl_typesupport_fastrtps_c__visibility_control.h.in b/rosidl_typesupport_fastrtps_c/resource/rosidl_typesupport_fastrtps_c__visibility_control.h.in new file mode 100644 index 000000000..16b92154a --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/resource/rosidl_typesupport_fastrtps_c__visibility_control.h.in @@ -0,0 +1,43 @@ +// generated from +// rosidl_typesupport_fastrtps_c/resource/rosidl_typesupport_fastrtps_c__visibility_control.h.in +// generated code does not contain a copyright notice + +#ifndef @PROJECT_NAME_UPPER@__MSG__ROSIDL_TYPESUPPORT_FASTRTPS_C__VISIBILITY_CONTROL_H_ +#define @PROJECT_NAME_UPPER@__MSG__ROSIDL_TYPESUPPORT_FASTRTPS_C__VISIBILITY_CONTROL_H_ + +#if __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_EXPORT_@PROJECT_NAME@ __attribute__ ((dllexport)) + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@PROJECT_NAME@ __attribute__ ((dllimport)) + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_EXPORT_@PROJECT_NAME@ __declspec(dllexport) + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@PROJECT_NAME@ __declspec(dllimport) + #endif + #ifdef ROSIDL_TYPESUPPORT_FASTRTPS_C_BUILDING_DLL_@PROJECT_NAME@ + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@PROJECT_NAME@ ROSIDL_TYPESUPPORT_FASTRTPS_C_EXPORT_@PROJECT_NAME@ + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@PROJECT_NAME@ ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@PROJECT_NAME@ + #endif +#else + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_EXPORT_@PROJECT_NAME@ __attribute__ ((visibility("default"))) + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_IMPORT_@PROJECT_NAME@ + #if __GNUC__ >= 4 + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@PROJECT_NAME@ __attribute__ ((visibility("default"))) + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@PROJECT_NAME@ + #endif +#endif + +#if __cplusplus +} +#endif + +#endif // @PROJECT_NAME_UPPER@__MSG__ROSIDL_TYPESUPPORT_FASTRTPS_C__VISIBILITY_CONTROL_H_ diff --git a/rosidl_typesupport_fastrtps_c/resource/srv__rosidl_typesupport_fastrtps_c.h.em b/rosidl_typesupport_fastrtps_c/resource/srv__rosidl_typesupport_fastrtps_c.h.em new file mode 100644 index 000000000..8f1181ef0 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/resource/srv__rosidl_typesupport_fastrtps_c.h.em @@ -0,0 +1,42 @@ +// generated from +// rosidl_typesupport_fastrtps_c/resource/srv__rosidl_typesupport_fastrtps_c.h.em +// generated code does not contain a copyright notice + +@####################################################################### +@# EmPy template for generating +@# __rosidl_typesupport_fastrtps_c.h files +@# +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .srv file +@# - get_header_filename_from_srv_name (function) +@####################################################################### +@ +@{ +header_guard_parts = [ + spec.pkg_name, 'srv', + get_header_filename_from_msg_name(spec.srv_name) + '__rosidl_typesupport_fastrtps_c_h'] +header_guard_variable = '__'.join([x.upper() for x in header_guard_parts]) + '_' +}@ +#ifndef @(header_guard_variable) +#define @(header_guard_variable) + +#include "rosidl_generator_c/service_type_support_struct.h" +#include "rosidl_typesupport_interface/macros.h" + +#include "@(spec.pkg_name)/msg/rosidl_typesupport_fastrtps_c__visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +ROSIDL_TYPESUPPORT_FASTRTPS_C_PUBLIC_@(spec.pkg_name) +const rosidl_service_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__SERVICE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(spec.pkg_name), @(spec.srv_name))(); + +#ifdef __cplusplus +} +#endif + +#endif // @(header_guard_variable) diff --git a/rosidl_typesupport_fastrtps_c/resource/srv__type_support_c.cpp.em b/rosidl_typesupport_fastrtps_c/resource/srv__type_support_c.cpp.em new file mode 100644 index 000000000..61dc12089 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/resource/srv__type_support_c.cpp.em @@ -0,0 +1,55 @@ +// generated from rosidl_typesupport_fastrtps_c/resource/srv__type_support_c.cpp.em +// generated code does not contain a copyright notice + +@####################################################################### +@# EmPy template for generating __type_support_c.cpp files +@# +@# Context: +@# - spec (rosidl_parser.ServiceSpecification) +@# Parsed specification of the .srv file +@# - get_header_filename_from_msg_name (function) +@####################################################################### +@ +#include "@(spec.pkg_name)/srv/@(get_header_filename_from_msg_name(spec.srv_name))__rosidl_typesupport_fastrtps_c.h" + +// Provides the definition of the service_type_support_callbacks_t struct. +#include + +#include "rosidl_typesupport_cpp/service_type_support.hpp" +#include "rosidl_typesupport_fastrtps_c/identifier.h" + +#include "@(spec.pkg_name)/msg/rosidl_typesupport_fastrtps_c__visibility_control.h" +@{req_header_file_name = get_header_filename_from_msg_name(spec.srv_name + '__request')}@ +@{res_header_file_name = get_header_filename_from_msg_name(spec.srv_name + '__response')}@ +#include "@(spec.pkg_name)/srv/@(req_header_file_name).h" +#include "@(spec.pkg_name)/srv/@(res_header_file_name).h" + +#include "@(spec.pkg_name)/srv/@(get_header_filename_from_msg_name(spec.srv_name + '_Request'))__rosidl_typesupport_fastrtps_c.h" +#include "@(spec.pkg_name)/srv/@(get_header_filename_from_msg_name(spec.srv_name + '_Response'))__rosidl_typesupport_fastrtps_c.h" + +#if defined(__cplusplus) +extern "C" +{ +#endif + +static service_type_support_callbacks_t callbacks = { + "@(spec.pkg_name)", + "@(spec.srv_name)", + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(spec.pkg_name), srv, @(spec.srv_name)_Request)(), + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(spec.pkg_name), srv, @(spec.srv_name)_Response)(), +}; + +static rosidl_service_type_support_t handle = { + rosidl_typesupport_fastrtps_c__identifier, + &callbacks, + get_service_typesupport_handle_function, +}; + +const rosidl_service_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__SERVICE_SYMBOL_NAME(rosidl_typesupport_fastrtps_c, @(spec.pkg_name), @(spec.srv_name))() { + return &handle; +} + +#if defined(__cplusplus) +} +#endif diff --git a/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c-extras.cmake.in b/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c-extras.cmake.in new file mode 100644 index 000000000..29e484fe9 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c-extras.cmake.in @@ -0,0 +1,34 @@ +# generated from +# rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c-extras.cmake.in + +find_package(fastrtps_cmake_module QUIET) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +if(NOT FastRTPS_FOUND) + message(STATUS + "Could not find eProsima Fast-RTPS: skipping rosidl_typesupport_fastrtps_c" + ) +else() + find_package(ament_cmake_core QUIET REQUIRED) + ament_register_extension( + "rosidl_generate_interfaces" + "rosidl_typesupport_fastrtps_c" + "rosidl_typesupport_fastrtps_c_generate_interfaces.cmake") + + set(rosidl_typesupport_fastrtps_c_BIN + "${rosidl_typesupport_fastrtps_c_DIR}/../../../lib/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c") + normalize_path(rosidl_typesupport_fastrtps_c_BIN + "${rosidl_typesupport_fastrtps_c_BIN}") + + set(rosidl_typesupport_fastrtps_c_GENERATOR_FILES + "${rosidl_typesupport_fastrtps_c_DIR}/../../../@PYTHON_INSTALL_DIR@/rosidl_typesupport_fastrtps_c/__init__.py") + normalize_path(rosidl_typesupport_fastrtps_c_GENERATOR_FILES + "${rosidl_typesupport_fastrtps_c_GENERATOR_FILES}") + + set(rosidl_typesupport_fastrtps_c_TEMPLATE_DIR + "${rosidl_typesupport_fastrtps_c_DIR}/../resource") + normalize_path(rosidl_typesupport_fastrtps_c_TEMPLATE_DIR + "${rosidl_typesupport_fastrtps_c_TEMPLATE_DIR}") +endif() diff --git a/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c/__init__.py b/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c/__init__.py new file mode 100644 index 000000000..02d93b96a --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c/__init__.py @@ -0,0 +1,103 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# 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. + +import os + +from rosidl_cmake import convert_camel_case_to_lower_case_underscore +from rosidl_cmake import expand_template +from rosidl_cmake import extract_message_types +from rosidl_cmake import get_newest_modification_time +from rosidl_parser import parse_message_file +from rosidl_parser import parse_service_file +from rosidl_parser import validate_field_types + + +def generate_typesupport_fastrtps_c(args): + template_dir = args['template_dir'] + mapping_msgs = { + os.path.join(template_dir, 'msg__rosidl_typesupport_fastrtps_c.h.em'): + '%s__rosidl_typesupport_fastrtps_c.h', + os.path.join(template_dir, 'msg__type_support_c.cpp.em'): + '%s__type_support_c.cpp', + } + + mapping_srvs = { + os.path.join(template_dir, 'srv__rosidl_typesupport_fastrtps_c.h.em'): + '%s__rosidl_typesupport_fastrtps_c.h', + os.path.join(template_dir, 'srv__type_support_c.cpp.em'): + '%s__type_support_c.cpp', + } + + for template_file in mapping_msgs.keys(): + assert os.path.exists(template_file), 'Could not find template: ' + template_file + + for template_file in mapping_srvs.keys(): + assert os.path.exists(template_file), 'Could not find template: ' + template_file + + pkg_name = args['package_name'] + known_msg_types = extract_message_types( + pkg_name, args['ros_interface_files'], args.get('ros_interface_dependencies', [])) + + functions = { + 'get_header_filename_from_msg_name': convert_camel_case_to_lower_case_underscore, + } + # generate_dds_fastrtps_cpp() and therefore the make target depend on the additional files + # therefore they must be listed here even if the generated type support files are independent + latest_target_timestamp = get_newest_modification_time( + args['target_dependencies'] + args.get('additional_files', [])) + + for idl_file in args['ros_interface_files']: + extension = os.path.splitext(idl_file)[1] + if extension == '.msg': + spec = parse_message_file(pkg_name, idl_file) + validate_field_types(spec, known_msg_types) + subfolder = os.path.basename(os.path.dirname(idl_file)) + for template_file, generated_filename in mapping_msgs.items(): + generated_file = os.path.join(args['output_dir'], subfolder) + if generated_filename.endswith('.cpp'): + generated_file = os.path.join(generated_file, 'dds_fastrtps_c') + generated_file = os.path.join( + generated_file, generated_filename % + convert_camel_case_to_lower_case_underscore(spec.base_type.type)) + + data = { + 'spec': spec, + 'pkg': spec.base_type.pkg_name, + 'msg': spec.msg_name, + 'type': spec.base_type.type, + 'subfolder': subfolder, + } + data.update(functions) + expand_template( + template_file, data, generated_file, + minimum_timestamp=latest_target_timestamp) + + elif extension == '.srv': + spec = parse_service_file(pkg_name, idl_file) + validate_field_types(spec, known_msg_types) + for template_file, generated_filename in mapping_srvs.items(): + generated_file = os.path.join(args['output_dir'], 'srv') + if generated_filename.endswith('.cpp'): + generated_file = os.path.join(generated_file, 'dds_fastrtps_c') + generated_file = os.path.join( + generated_file, generated_filename % + convert_camel_case_to_lower_case_underscore(spec.srv_name)) + + data = {'spec': spec} + data.update(functions) + expand_template( + template_file, data, generated_file, + minimum_timestamp=latest_target_timestamp) + + return 0 diff --git a/rosidl_typesupport_fastrtps_c/src/identifier.cpp b/rosidl_typesupport_fastrtps_c/src/identifier.cpp new file mode 100644 index 000000000..bf6466606 --- /dev/null +++ b/rosidl_typesupport_fastrtps_c/src/identifier.cpp @@ -0,0 +1,29 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// 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 + +#include + +#if defined(__cplusplus) +extern "C" +{ +#endif + +const char * + rosidl_typesupport_fastrtps_c__identifier = "rosidl_typesupport_fastrtps_c"; + +#if defined(__cplusplus) +} +#endif diff --git a/rosidl_typesupport_fastrtps_cpp/CMakeLists.txt b/rosidl_typesupport_fastrtps_cpp/CMakeLists.txt new file mode 100644 index 000000000..35b1221f3 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.5) + +project(rosidl_typesupport_fastrtps_cpp) + +set(FASTRTPS_STATIC_DISABLE $ENV{FASTRTPS_STATIC_DISABLE} + CACHE BOOL "If fastrtps Static should be disabled.") + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +find_package(fastrtps_cmake_module QUIET) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) +if(FASTRTPS_STATIC_DISABLE) + ament_package() + message(STATUS "fastrtps static rmw implementation explicitly disabled - skipping '${PROJECT_NAME}'") + return() +endif() + +find_package(ament_cmake_python REQUIRED) + +ament_export_dependencies(fastrtps) +ament_export_dependencies(rmw) +ament_export_dependencies(rosidl_cmake) +ament_export_dependencies(rosidl_generator_c) +ament_export_dependencies(rosidl_generator_cpp) +ament_export_dependencies(rosidl_generator_dds_idl) +ament_export_dependencies(rosidl_typesupport_interface) + +ament_export_include_directories(include) + +ament_python_install_package(${PROJECT_NAME}) + +add_library(${PROJECT_NAME} SHARED src/identifier.cpp) +if(WIN32) + target_compile_definitions(${PROJECT_NAME} + PRIVATE "ROSIDL_TYPESUPPORT_FASTRTPS_CPP_BUILDING_DLL") +endif() +target_include_directories(${PROJECT_NAME} + PUBLIC + include +) +ament_export_libraries(${PROJECT_NAME}) + +ament_index_register_resource("rosidl_typesupport_cpp") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package( + CONFIG_EXTRAS "rosidl_typesupport_fastrtps_cpp-extras.cmake.in" +) + +install( + PROGRAMS bin/rosidl_typesupport_fastrtps_cpp + DESTINATION lib/rosidl_typesupport_fastrtps_cpp +) +install( + DIRECTORY cmake resource + DESTINATION share/${PROJECT_NAME} +) +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) diff --git a/rosidl_typesupport_fastrtps_cpp/bin/rosidl_typesupport_fastrtps_cpp b/rosidl_typesupport_fastrtps_cpp/bin/rosidl_typesupport_fastrtps_cpp new file mode 100644 index 000000000..08f9e27b6 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/bin/rosidl_typesupport_fastrtps_cpp @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 + +import argparse +import sys + +from rosidl_cmake import extract_message_types +from rosidl_cmake import read_generator_arguments +from rosidl_parser import UnknownMessageType +from rosidl_typesupport_fastrtps_cpp import generate_cpp +from rosidl_typesupport_fastrtps_cpp import generate_dds_fastrtps_cpp +from rosidl_typesupport_fastrtps_cpp import parse_ros_interface_files + + +def main(argv=sys.argv[1:]): + parser = argparse.ArgumentParser( + description='Generate the C++ interfaces for fastrtps.', + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument( + '--generator-arguments-file', + required=True, + help='The location of the file containing the generator arguments') + parser.add_argument( + '--dds-interface-base-path', + required=True, + help='The base location of the DDS interface files') + parser.add_argument( + '--idl-pp', + required=True, + help='The location of the IDL preprocessor') + args = parser.parse_args(argv) + + generator_args = read_generator_arguments(args.generator_arguments_file) + + message_specs, service_specs = parse_ros_interface_files( + generator_args['package_name'], generator_args['ros_interface_files']) + + known_msg_types = extract_message_types( + generator_args['package_name'], generator_args['ros_interface_files'], + generator_args.get('ros_interface_dependencies', [])) + + try: + rc = generate_cpp(generator_args, message_specs, service_specs, known_msg_types) + except UnknownMessageType as e: + print(str(e), file=sys.stderr) + return 1 + if rc: + return rc + return generate_dds_fastrtps_cpp( + generator_args['package_name'], + generator_args.get('additional_files', []), + args.dds_interface_base_path, + generator_args.get('ros_interface_dependencies', []), + generator_args['output_dir'], + args.idl_pp, + message_specs, + service_specs, + ) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/rosidl_typesupport_fastrtps_cpp/cmake/rosidl_typesupport_fastrtps_cpp_generate_interfaces.cmake b/rosidl_typesupport_fastrtps_cpp/cmake/rosidl_typesupport_fastrtps_cpp_generate_interfaces.cmake new file mode 100644 index 000000000..e14de45a0 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/cmake/rosidl_typesupport_fastrtps_cpp_generate_interfaces.cmake @@ -0,0 +1,286 @@ +# Copyright 2014-2015 Open Source Robotics Foundation, Inc. +# +# 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. + +find_package(fastrtps_cmake_module QUIET) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +set(_ros_idl_files "") +foreach(_idl_file ${rosidl_generate_interfaces_IDL_FILES}) + get_filename_component(_extension "${_idl_file}" EXT) + # Skip .srv files + if(_extension STREQUAL ".msg") + list(APPEND _ros_idl_files "${_idl_file}") + endif() +endforeach() + +rosidl_generate_dds_interfaces( + ${rosidl_generate_interfaces_TARGET}__dds_fastrtps_idl + IDL_FILES ${_ros_idl_files} + DEPENDENCY_PACKAGE_NAMES ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES} + OUTPUT_SUBFOLDERS "dds_fastrtps" +) + +set(_dds_idl_files "") +set(_dds_idl_base_path "${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_dds_idl") +foreach(_idl_file ${rosidl_generate_interfaces_IDL_FILES}) + get_filename_component(_extension "${_idl_file}" EXT) + if(_extension STREQUAL ".msg") + get_filename_component(_parent_folder "${_idl_file}" DIRECTORY) + get_filename_component(_parent_folder "${_parent_folder}" NAME) + get_filename_component(_name "${_idl_file}" NAME_WE) + list(APPEND _dds_idl_files + "${_dds_idl_base_path}/${PROJECT_NAME}/${_parent_folder}/dds_fastrtps/${_name}_.idl") + endif() +endforeach() + +set(_output_path "${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_cpp/${PROJECT_NAME}") +set(_generated_msg_files "") +set(_generated_srv_files "") +foreach(_idl_file ${rosidl_generate_interfaces_IDL_FILES}) + get_filename_component(_extension "${_idl_file}" EXT) + get_filename_component(_msg_name "${_idl_file}" NAME_WE) + string_camel_case_to_lower_case_underscore("${_msg_name}" _header_name) + if(_extension STREQUAL ".msg") + get_filename_component(_parent_folder "${_idl_file}" DIRECTORY) + get_filename_component(_parent_folder "${_parent_folder}" NAME) + if(_parent_folder STREQUAL "msg") + set(_var2 "_generated_msg_files") + elseif(_parent_folder STREQUAL "srv") + set(_var2 "_generated_srv_files") + else() + message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") + endif() + + list(APPEND ${_var2} "${_output_path}/${_parent_folder}/${_header_name}__rosidl_typesupport_fastrtps_cpp.hpp") + list(APPEND ${_var2} "${_output_path}/${_parent_folder}/dds_fastrtps/${_header_name}__type_support.cpp") + elseif(_extension STREQUAL ".srv") + list(APPEND _generated_srv_files "${_output_path}/srv/${_header_name}__rosidl_typesupport_fastrtps_cpp.hpp") + list(APPEND _generated_srv_files "${_output_path}/srv/dds_fastrtps/${_header_name}__type_support.cpp") + else() + message(FATAL_ERROR "Interface file with unknown extension: ${_idl_file}") + endif() +endforeach() + +# If not on Windows, disable some warnings with fastrtps's generated code +if(NOT WIN32) + set(_fastrtps_compile_flags) + if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(_fastrtps_compile_flags + "-Wno-deprecated-register" + "-Wno-return-type-c-linkage" + "-Wno-unused-parameter" + ) + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(_fastrtps_compile_flags + # no-strict-aliasing necessary only for Release builds + "-Wno-strict-aliasing" + "-Wno-unused-parameter" + ) + endif() + if(NOT _fastrtps_compile_flags STREQUAL "") + string(REPLACE ";" " " _fastrtps_compile_flags "${_fastrtps_compile_flags}") + endif() +endif() + +set(_dependency_files "") +set(_dependencies "") +foreach(_pkg_name ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES}) + foreach(_idl_file ${${_pkg_name}_INTERFACE_FILES}) + get_filename_component(_extension "${_idl_file}" EXT) + if(_extension STREQUAL ".msg") + get_filename_component(_parent_folder "${_idl_file}" DIRECTORY) + get_filename_component(_parent_folder "${_parent_folder}" NAME) + get_filename_component(_name "${_idl_file}" NAME_WE) + set(_abs_idl_file "${${_pkg_name}_DIR}/../${_parent_folder}/dds_fastrtps/${_name}_.idl") + normalize_path(_abs_idl_file "${_abs_idl_file}") + list(APPEND _dependency_files "${_abs_idl_file}") + set(_abs_idl_file "${${_pkg_name}_DIR}/../${_idl_file}") + normalize_path(_abs_idl_file "${_abs_idl_file}") + list(APPEND _dependencies "${_pkg_name}:${_abs_idl_file}") + endif() + endforeach() +endforeach() + +set(target_dependencies + "${rosidl_typesupport_fastrtps_cpp_BIN}" + ${rosidl_typesupport_fastrtps_cpp_GENERATOR_FILES} + "${rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR}/msg__rosidl_typesupport_fastrtps_cpp.hpp.em" + "${rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR}/msg__type_support.cpp.em" + "${rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR}/srv__rosidl_typesupport_fastrtps_cpp.hpp.em" + "${rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR}/srv__type_support.cpp.em" + ${_dependency_files}) +foreach(dep ${target_dependencies}) + if(NOT EXISTS "${dep}") + message(FATAL_ERROR "Target dependency '${dep}' does not exist") + endif() +endforeach() + +set(generator_arguments_file "${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_cpp__arguments.json") +rosidl_write_generator_arguments( + "${generator_arguments_file}" + PACKAGE_NAME "${PROJECT_NAME}" + ROS_INTERFACE_FILES "${rosidl_generate_interfaces_IDL_FILES}" + ROS_INTERFACE_DEPENDENCIES "${_dependencies}" + OUTPUT_DIR "${_output_path}" + TEMPLATE_DIR "${rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR}" + TARGET_DEPENDENCIES ${target_dependencies} + ADDITIONAL_FILES ${_dds_idl_files} +) + +set(_idl_pp "") +add_custom_command( + OUTPUT ${_generated_msg_files} ${_generated_srv_files} + COMMAND ${PYTHON_EXECUTABLE} ${rosidl_typesupport_fastrtps_cpp_BIN} + --generator-arguments-file "${generator_arguments_file}" + --dds-interface-base-path "${_dds_idl_base_path}" + --idl-pp "${_idl_pp}" + DEPENDS ${target_dependencies} ${_dds_idl_files} + COMMENT "Generating C++ type support for eProsima Fast-RTPS" + VERBATIM +) + +# generate header to switch between export and import for a specific package +set(_visibility_control_file +"${_output_path}/msg/rosidl_typesupport_fastrtps_cpp__visibility_control.h") +string(TOUPPER "${PROJECT_NAME}" PROJECT_NAME_UPPER) +configure_file( + "${rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR}/rosidl_typesupport_fastrtps_cpp__visibility_control.h.in" + "${_visibility_control_file}" + @ONLY +) + +set(_target_suffix "__rosidl_typesupport_fastrtps_cpp") + +# link_directories(${fastrtps_LIBRARY_DIRS}) +add_library(${rosidl_generate_interfaces_TARGET}${_target_suffix} SHARED + ${_generated_msg_files} ${_generated_srv_files}) +if(rosidl_generate_interfaces_LIBRARY_NAME) + set_target_properties(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PROPERTIES OUTPUT_NAME "${rosidl_generate_interfaces_LIBRARY_NAME}${_target_suffix}") +endif() +set_target_properties(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PROPERTIES CXX_STANDARD 14) +if(fastrtps_GLIBCXX_USE_CXX11_ABI_ZERO) + target_compile_definitions(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PRIVATE fastrtps_GLIBCXX_USE_CXX11_ABI_ZERO) +endif() +if(WIN32) + target_compile_definitions(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PRIVATE "ROSIDL_TYPESUPPORT_FASTRTPS_CPP_BUILDING_DLL_${PROJECT_NAME}") + target_compile_definitions(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PRIVATE "EPROSIMA_USER_DLL_EXPORT") +endif() +if(NOT WIN32) + set(_target_compile_flags "-Wall -Wextra -Wpedantic") +else() + set(_target_compile_flags + "/W4" + "/wd4100" + "/wd4127" + "/wd4275" + "/wd4305" + "/wd4458" + "/wd4701" + ) +endif() +string(REPLACE ";" " " _target_compile_flags "${_target_compile_flags}") +set_target_properties(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PROPERTIES COMPILE_FLAGS "${_target_compile_flags}") +target_include_directories(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PUBLIC + ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp + ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_cpp +) +ament_target_dependencies(${rosidl_generate_interfaces_TARGET}${_target_suffix} + "fastrtps" + "rmw" + "rosidl_typesupport_fastrtps_cpp" + "rosidl_typesupport_interface") +foreach(_pkg_name ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES}) + set(_msg_include_dir "${${_pkg_name}_DIR}/../../../include/${_pkg_name}/msg/dds_fastrtps") + set(_srv_include_dir "${${_pkg_name}_DIR}/../../../include/${_pkg_name}/srv/dds_fastrtps") + normalize_path(_msg_include_dir "${_msg_include_dir}") + normalize_path(_srv_include_dir "${_srv_include_dir}") + target_include_directories(${rosidl_generate_interfaces_TARGET}${_target_suffix} + PUBLIC + "${_msg_include_dir}" + "${_srv_include_dir}" + ) + ament_target_dependencies(${rosidl_generate_interfaces_TARGET}${_target_suffix} + ${_pkg_name}) +endforeach() +target_link_libraries(${rosidl_generate_interfaces_TARGET}${_target_suffix} fastrtps fastcdr) +add_dependencies( + ${rosidl_generate_interfaces_TARGET} + ${rosidl_generate_interfaces_TARGET}${_target_suffix} +) +add_dependencies( + ${rosidl_generate_interfaces_TARGET}${_target_suffix} + ${rosidl_generate_interfaces_TARGET}__cpp +) +add_dependencies( + ${rosidl_generate_interfaces_TARGET}__dds_fastrtps_idl + ${rosidl_generate_interfaces_TARGET}${_target_suffix} +) + +if(NOT rosidl_generate_interfaces_SKIP_INSTALL) + install( + DIRECTORY "${_output_path}/" + DESTINATION "include/${PROJECT_NAME}" + PATTERN "*.cpp" EXCLUDE + ) + + if( + NOT _generated_msg_files STREQUAL "" OR + NOT _generated_srv_files STREQUAL "" + ) + ament_export_include_directories(include) + endif() + + install( + TARGETS ${rosidl_generate_interfaces_TARGET}${_target_suffix} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + ament_export_libraries(${rosidl_generate_interfaces_TARGET}${_target_suffix}) +endif() + +if(BUILD_TESTING AND rosidl_generate_interfaces_ADD_LINTER_TESTS) + if(NOT _generated_msg_files STREQUAL "" OR NOT _generated_srv_files STREQUAL "") + find_package(ament_cmake_cppcheck REQUIRED) + ament_cppcheck( + TESTNAME "cppcheck_rosidl_typesupport_fastrtps_cpp" + ${_generated_msg_files} ${_generated_srv_files}) + + find_package(ament_cmake_cpplint REQUIRED) + get_filename_component(_cpplint_root "${_output_path}" DIRECTORY) + ament_cpplint( + TESTNAME "cpplint_rosidl_typesupport_fastrtps_cpp" + # the generated code might contain longer lines for templated types + MAX_LINE_LENGTH 999 + ROOT "${_cpplint_root}" + ${_generated_msg_files} ${_generated_srv_files}) + + find_package(ament_cmake_uncrustify REQUIRED) + ament_uncrustify( + TESTNAME "uncrustify_rosidl_typesupport_fastrtps_cpp" + # the generated code might contain longer lines for templated types + MAX_LINE_LENGTH 999 + ${_generated_msg_files} ${_generated_srv_files}) + endif() +endif() diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/identifier.hpp b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/identifier.hpp new file mode 100644 index 000000000..910689139 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/identifier.hpp @@ -0,0 +1,28 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// 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 ROSIDL_TYPESUPPORT_FASTRTPS_CPP__IDENTIFIER_HPP_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__IDENTIFIER_HPP_ + +#include + +namespace rosidl_typesupport_fastrtps_cpp +{ + +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT +extern const char * typesupport_identifier; + +} // namespace rosidl_typesupport_fastrtps_cpp + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__IDENTIFIER_HPP_ diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support.h b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support.h new file mode 100644 index 000000000..0a0f06b63 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support.h @@ -0,0 +1,44 @@ +// Copyright 2014-2015 Open Source Robotics Foundation, Inc. +// +// 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 ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_H_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_H_ + +#include "rosidl_generator_c/message_type_support_struct.h" + +#include + +typedef struct message_type_support_callbacks_t +{ + const char * package_name_; + const char * message_name_; + + // Function for message serialization + bool (* cdr_serialize)( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr); + + // Function for message deserialization + bool (* cdr_deserialize)( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message); + + // Function to get size of data + uint32_t (* get_serialized_size)(const void *); + + // Function for type support initialization + size_t (* max_serialized_size)(bool & full_bounded); +} message_type_support_callbacks_t; + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_H_ diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support_decl.hpp b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support_decl.hpp new file mode 100644 index 000000000..8d0f5de58 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/message_type_support_decl.hpp @@ -0,0 +1,33 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// 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 ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_DECL_HPP_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_DECL_HPP_ + +// Provides the definition of the rosidl_message_type_support_t struct. +#include +// Provides visibility macros like ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC. +#include + +namespace rosidl_typesupport_fastrtps_cpp +{ + +// This is implemented in the shared library provided by this package. +template +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC +const rosidl_message_type_support_t * get_message_type_support_handle(); + +} // namespace rosidl_typesupport_fastrtps_cpp + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__MESSAGE_TYPE_SUPPORT_DECL_HPP_ diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/service_type_support.h b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/service_type_support.h new file mode 100644 index 000000000..2fe7fa2d4 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/service_type_support.h @@ -0,0 +1,33 @@ +// Copyright 2014-2015 Open Source Robotics Foundation, Inc. +// +// 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 ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERVICE_TYPE_SUPPORT_H_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERVICE_TYPE_SUPPORT_H_ + +#include +#include +#include "rosidl_generator_c/service_type_support_struct.h" + +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" + +typedef struct service_type_support_callbacks_t +{ + const char * package_name_; + const char * service_name_; + + const rosidl_message_type_support_t * request_members_; + const rosidl_message_type_support_t * response_members_; +} service_type_support_callbacks_t; + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERVICE_TYPE_SUPPORT_H_ diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/service_type_support_decl.hpp b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/service_type_support_decl.hpp new file mode 100644 index 000000000..c989fcb2f --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/service_type_support_decl.hpp @@ -0,0 +1,33 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// 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 ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERVICE_TYPE_SUPPORT_DECL_HPP_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERVICE_TYPE_SUPPORT_DECL_HPP_ + +// Provides the definition of the rosidl_service_type_support_t struct. +#include +// Provides visibility macros like ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC. +#include + +namespace rosidl_typesupport_fastrtps_cpp +{ + +// This is implemented in the shared library provided by this package. +template +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC +const rosidl_service_type_support_t * get_service_type_support_handle(); + +} // namespace rosidl_typesupport_fastrtps_cpp + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__SERVICE_TYPE_SUPPORT_DECL_HPP_ diff --git a/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/visibility_control.h b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/visibility_control.h new file mode 100644 index 000000000..1e94d2c91 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/include/rosidl_typesupport_fastrtps_cpp/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// 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 ROSIDL_TYPESUPPORT_FASTRTPS_CPP__VISIBILITY_CONTROL_H_ +#define ROSIDL_TYPESUPPORT_FASTRTPS_CPP__VISIBILITY_CONTROL_H_ + +#if __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT __attribute__ ((dllexport)) + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT __attribute__ ((dllimport)) + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT __declspec(dllexport) + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT __declspec(dllimport) + #endif + #ifdef ROSIDL_TYPESUPPORT_FASTRTPS_CPP_BUILDING_DLL + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT + #endif + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_LOCAL +#else + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT __attribute__ ((visibility("default"))) + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT + #if __GNUC__ >= 4 + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC __attribute__ ((visibility("default"))) + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_LOCAL + #endif +#endif + +#if __cplusplus +} +#endif + +#endif // ROSIDL_TYPESUPPORT_FASTRTPS_CPP__VISIBILITY_CONTROL_H_ diff --git a/rosidl_typesupport_fastrtps_cpp/package.xml b/rosidl_typesupport_fastrtps_cpp/package.xml new file mode 100644 index 000000000..8bb797e8b --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/package.xml @@ -0,0 +1,41 @@ + + + + rosidl_typesupport_fastrtps_cpp + 0.5.1 + Generate the C++ interfaces for eProsima FastRTPS. + Dirk Thomas + Apache License 2.0 + Ricardo González + + ament_cmake + fastrtps_cmake_module + fastcdr + fastrtps + rosidl_cmake + rosidl_generator_c + rosidl_generator_cpp + + ament_cmake + fastrtps_cmake_module + fastcdr + fastrtps + rosidl_cmake + rosidl_generator_c + rosidl_generator_cpp + rosidl_generator_dds_idl + + rmw + + rosidl_parser + rosidl_typesupport_interface + + ament_lint_auto + ament_lint_common + + rosidl_typesupport_cpp_packages + + + ament_cmake + + diff --git a/rosidl_typesupport_fastrtps_cpp/resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em b/rosidl_typesupport_fastrtps_cpp/resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em new file mode 100644 index 000000000..1858c00b5 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em @@ -0,0 +1,99 @@ +// generated from +// rosidl_typesupport_fastrtps_cpp/resource/msg__rosidl_typesupport_fastrtps_cpp.hpp.em +// generated code does not contain a copyright notice + +@####################################################################### +@# EmPy template for generating +@# __rosidl_typesupport_fastrtps_cpp.hpp files +@# +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .msg file +@# - subfolder (string) +@# The subfolder / subnamespace of the message +@# Either 'msg' or 'srv' +@# - get_header_filename_from_msg_name (function) +@####################################################################### +@ +@{ +header_guard_parts = [ + spec.base_type.pkg_name, subfolder, + get_header_filename_from_msg_name(spec.base_type.type) + '__rosidl_typesupport_fastrtps_cpp_hpp'] +header_guard_variable = '__'.join([x.upper() for x in header_guard_parts]) + '_' +}@ +#ifndef @(header_guard_variable) +#define @(header_guard_variable) + +#include "rosidl_generator_c/message_type_support_struct.h" +#include "rosidl_typesupport_interface/macros.h" + +#include "@(spec.base_type.pkg_name)/msg/rosidl_typesupport_fastrtps_cpp__visibility_control.h" + +#include "@(spec.base_type.pkg_name)/@(subfolder)/@(get_header_filename_from_msg_name(spec.base_type.type))__struct.hpp" +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +# ifdef __clang__ +# pragma clang diagnostic ignored "-Wdeprecated-register" +# pragma clang diagnostic ignored "-Wreturn-type-c-linkage" +# endif +#endif +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#include "fastcdr/Cdr.h" + +namespace @(spec.base_type.pkg_name) +{ + +namespace @(subfolder) +{ + +namespace typesupport_fastrtps_cpp +{ + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +cdr_serialize( + const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message, + eprosima::fastcdr::Cdr & cdr); + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +cdr_deserialize( + eprosima::fastcdr::Cdr & cdr, + @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message); + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +get_serialized_size( + const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message, + size_t current_alignment); + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +max_serialized_size_@(spec.base_type.type)( + bool & full_bounded, + size_t current_alignment); + +} // namespace typesupport_fastrtps_cpp + +} // namespace @(subfolder) + +} // namespace @(spec.base_type.pkg_name) + +#ifdef __cplusplus +extern "C" +{ +#endif + +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +const rosidl_message_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_cpp, @(spec.base_type.pkg_name), @(subfolder), @(spec.base_type.type))(); + +#ifdef __cplusplus +} +#endif + +#endif // @(header_guard_variable) diff --git a/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em new file mode 100644 index 000000000..2cca0268e --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em @@ -0,0 +1,420 @@ +// generated from rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em +// generated code does not contain a copyright notice + +@####################################################################### +@# EmPy template for generating __type_support.cpp files +@# +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .msg file +@# - subfolder (string) +@# The subfolder / subnamespace of the message +@# Either 'msg' or 'srv' +@# - get_header_filename_from_msg_name (function) +@####################################################################### +@ +#include "@(spec.base_type.pkg_name)/@(subfolder)/@(get_header_filename_from_msg_name(spec.base_type.type))__rosidl_typesupport_fastrtps_cpp.hpp" + +#include +#include +#include + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "rosidl_typesupport_fastrtps_cpp/identifier.hpp" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support_decl.hpp" + +#include "fastcdr/Cdr.h" + +// forward declaration of message dependencies and their conversion functions +@[for field in spec.fields]@ +@[ if not field.type.is_primitive_type()]@ +namespace @(field.type.pkg_name) +{ +namespace msg +{ +namespace typesupport_fastrtps_cpp +{ +bool cdr_serialize( + const @(field.type.pkg_name)::msg::@(field.type.type) &, + eprosima::fastcdr::Cdr &); +bool cdr_deserialize( + eprosima::fastcdr::Cdr &, + @(field.type.pkg_name)::msg::@(field.type.type) &); +size_t get_serialized_size( + const @(field.type.pkg_name)::msg::@(field.type.type) &, + size_t current_alignment); +size_t +max_serialized_size_@(field.type.type)( + bool & full_bounded, + size_t current_alignment); +} // namespace typesupport_fastrtps_cpp +} // namespace msg +} // namespace @(field.type.pkg_name) + +@[ end if]@ +@[end for]@ + +namespace @(spec.base_type.pkg_name) +{ + +namespace @(subfolder) +{ + +namespace typesupport_fastrtps_cpp +{ + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +cdr_serialize( + const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message, + eprosima::fastcdr::Cdr & cdr) +{ +@[if not spec.fields]@ + (void)ros_message; + (void)cdr; +@[end if]@ +@[for field in spec.fields]@ + // field.name @(field.name) +@[ if field.type.is_array]@ + { +@[ if field.type.array_size and not field.type.is_upper_bound]@ +@[ if field.type.is_primitive_type()]@ + cdr << ros_message.@(field.name); +@[ else]@ + for (size_t i = 0; i < @(field.type.array_size); i++) { + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::cdr_serialize( + ros_message.@(field.name)[i], + cdr); + } +@[ end if]@ +@[ else]@ +@[ if field.type.is_upper_bound or not field.type.is_primitive_type()]@ + size_t size = ros_message.@(field.name).size(); +@[ if field.type.is_upper_bound]@ + if (size > @(field.type.array_size)) { + throw std::runtime_error("array size exceeds upper bound"); + } +@[ end if]@ +@[ end if]@ +@[ if field.type.is_primitive_type() and not field.type.is_upper_bound]@ + cdr << ros_message.@(field.name); +@[ else]@ + cdr << static_cast(size); + for (size_t i = 0; i < size; i++) { +@[ if field.type.type == 'bool']@ + cdr << (ros_message.@(field.name)[i] ? true : false); +@[ elif field.type.is_primitive_type()]@ + cdr << ros_message.@(field.name)[i]; +@[ else]@ + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::cdr_serialize( + ros_message.@(field.name)[i], + cdr); +@[ end if]@ + } +@[ end if]@ +@[ end if]@ + } +@[ elif field.type.type == 'bool']@ + cdr << (ros_message.@(field.name) ? true : false); +@[ elif field.type.is_primitive_type()]@ + cdr << ros_message.@(field.name); +@[ else]@ + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::cdr_serialize( + ros_message.@(field.name), + cdr); +@[ end if]@ +@[end for]@ + return true; +} + +bool +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +cdr_deserialize( + eprosima::fastcdr::Cdr & cdr, + @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message) +{ +@[if not spec.fields]@ + (void)ros_message; + (void)cdr; +@[end if]@ +@[for field in spec.fields]@ + // field.name @(field.name) +@[ if field.type.is_array]@ + { +@[ if field.type.array_size and not field.type.is_upper_bound]@ +@[ if field.type.is_primitive_type()]@ + cdr >> ros_message.@(field.name); +@[ else]@ + for (size_t i = 0; i < @(field.type.array_size); i++) { + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::cdr_deserialize( + cdr, + ros_message.@(field.name)[i]); + } +@[ end if]@ +@[ else]@ +@[ if field.type.is_primitive_type() and not field.type.is_upper_bound]@ + cdr >> ros_message.@(field.name); +@[ else]@ + uint32_t cdrSize; + cdr >> cdrSize; + size_t size = static_cast(cdrSize); + ros_message.@(field.name).resize(size); + for (size_t i = 0; i < size; i++) { +@[ if field.type.type == 'bool']@ + uint8_t tmp; + cdr >> tmp; + ros_message.@(field.name)[i] = tmp ? true : false; +@[ elif field.type.is_primitive_type()]@ + cdr >> ros_message.@(field.name)[i]; +@[ else]@ + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::cdr_deserialize( + cdr, ros_message.@(field.name)[i]); +@[ end if]@ + } +@[ end if]@ +@[ end if]@ + } +@[ elif field.type.type == 'bool']@ + { + uint8_t tmp; + cdr >> tmp; + ros_message.@(field.name) = tmp ? true : false; + } +@[ elif field.type.is_primitive_type()]@ + cdr >> ros_message.@(field.name); +@[ else]@ + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::cdr_deserialize( + cdr, ros_message.@(field.name)); +@[ end if]@ + +@[end for]@ + return true; +} + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +get_serialized_size( + const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message, + size_t current_alignment) +{ +@[if not spec.fields]@ + (void)ros_message; + (void)current_alignment; +@[else]@ + size_t initial_alignment = current_alignment; + + const size_t padding = 4; + (void)padding; + +@[end if]@ +@[for field in spec.fields]@ + // field.name @(field.name) +@[ if field.type.is_array]@ + { +@[ if field.type.array_size and not field.type.is_upper_bound]@ + size_t array_size = @(field.type.array_size); +@[ else]@ + size_t array_size = ros_message.@(field.name).size(); +@[ if field.type.is_upper_bound]@ + if (array_size > @(field.type.array_size)) { + throw std::runtime_error("array size exceeds upper bound"); + } +@[ end if]@ + + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); +@[ end if]@ +@[ if field.type.type == 'string']@ + for (size_t index = 0; index < array_size; ++index) { + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + ros_message.@(field.name)[index].size() + 1; + } +@[ elif field.type.is_primitive_type()]@ + size_t item_size = sizeof(ros_message.@(field.name)[0]); + current_alignment += array_size * item_size + + eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); +@[ else] + for (size_t index = 0; index < array_size; ++index) { + current_alignment += + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::get_serialized_size( + ros_message.@(field.name)[index], current_alignment); + } +@[ end if]@ + } +@[ else]@ +@[ if field.type.type == 'string']@ + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + ros_message.@(field.name).size() + 1; +@[ elif field.type.is_primitive_type()]@ + { + size_t item_size = sizeof(ros_message.@(field.name)); + current_alignment += item_size + + eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + } +@[ else] + current_alignment += + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::get_serialized_size( + ros_message.@(field.name), current_alignment); +@[ end if]@ +@[ end if]@ +@[end for]@ + +@[if not spec.fields]@ + return 0; +@[else]@ + return current_alignment - initial_alignment; +@[end if]@ +} + +size_t +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.base_type.pkg_name) +max_serialized_size_@(spec.base_type.type)( + bool & full_bounded, + size_t current_alignment) +{ + size_t initial_alignment = current_alignment; + + const size_t padding = 4; + (void)padding; + (void)full_bounded; + +@[for field in spec.fields]@ + // field.name @(field.name) + { +@[ if field.type.is_array]@ +@[ if field.type.array_size]@ + size_t array_size = @(field.type.array_size); +@[ else]@ + size_t array_size = 0; +@[ end if]@ +@[ if not field.type.array_size or field.type.is_upper_bound]@ + full_bounded = false; + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); +@[ end if]@ +@[ else]@ + size_t array_size = 1; +@[ end if]@ + +@[ if field.type.type == 'string']@ + full_bounded = false; + for (size_t index = 0; index < array_size; ++index) { + current_alignment += padding + +@[ if field.type.string_upper_bound]@ + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + @(field.type.string_upper_bound) + 1; +@[ else]@ + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + 1; +@[ end if]@ + } +@[ elif field.type.is_primitive_type()]@ +@[ if field.type.type == 'bool' or field.type.type == 'byte' or field.type.type == 'char' or field.type.type == 'uint8' or field.type.type == 'int8' ] + current_alignment += array_size * sizeof(uint8_t); +@[ elif field.type.type == 'int16' or field.type.type == 'uint16'] + current_alignment += array_size * sizeof(uint16_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t)); +@[ elif field.type.type == 'int32' or field.type.type == 'uint32' or field.type.type == 'float32'] + current_alignment += array_size * sizeof(uint32_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); +@[ elif field.type.type == 'int64' or field.type.type == 'uint64' or field.type.type == 'float64'] + current_alignment += array_size * sizeof(uint64_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); +@[ end if]@ +@[ else] + for (size_t index = 0; index < array_size; ++index) { + current_alignment += + @(field.type.pkg_name)::msg::typesupport_fastrtps_cpp::max_serialized_size_@(field.type.type)( + full_bounded, current_alignment); + } +@[ end if]@ + } +@[end for]@ + + return current_alignment - initial_alignment; +} + +static bool __cdr_serialize( + const void * untyped_ros_message, + eprosima::fastcdr::Cdr & cdr) +{ + auto typed_message = + static_cast( + untyped_ros_message); + return cdr_serialize(*typed_message, cdr); +} + +static bool __cdr_deserialize( + eprosima::fastcdr::Cdr & cdr, + void * untyped_ros_message) +{ + auto typed_message = + static_cast<@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *>( + untyped_ros_message); + return cdr_deserialize(cdr, *typed_message); +} + +static uint32_t __get_serialized_size( + const void * untyped_ros_message) +{ + auto typed_message = + static_cast( + untyped_ros_message); + return static_cast(get_serialized_size(*typed_message, 0)); +} + +static size_t __max_serialized_size(bool & full_bounded) +{ + return max_serialized_size_@(spec.base_type.type)(full_bounded, 0); +} + +static message_type_support_callbacks_t callbacks = { + "@(spec.base_type.pkg_name)", + "@(spec.base_type.type)", + __cdr_serialize, + __cdr_deserialize, + __get_serialized_size, + __max_serialized_size +}; + +static rosidl_message_type_support_t handle = { + rosidl_typesupport_fastrtps_cpp::typesupport_identifier, + &callbacks, + get_message_typesupport_handle_function, +}; + +} // namespace typesupport_fastrtps_cpp + +} // namespace @(subfolder) + +} // namespace @(spec.base_type.pkg_name) + +namespace rosidl_typesupport_fastrtps_cpp +{ + +template<> +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT_@(spec.base_type.pkg_name) +const rosidl_message_type_support_t * +get_message_type_support_handle<@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type)>() +{ + return &@(spec.base_type.pkg_name)::@(subfolder)::typesupport_fastrtps_cpp::handle; +} + +} // namespace rosidl_typesupport_fastrtps_cpp + +#ifdef __cplusplus +extern "C" +{ +#endif + +const rosidl_message_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_cpp, @(spec.base_type.pkg_name), @(subfolder), @(spec.base_type.type))() { + return &@(spec.base_type.pkg_name)::@(subfolder)::typesupport_fastrtps_cpp::handle; +} + +#ifdef __cplusplus +} +#endif diff --git a/rosidl_typesupport_fastrtps_cpp/resource/rosidl_typesupport_fastrtps_cpp__visibility_control.h.in b/rosidl_typesupport_fastrtps_cpp/resource/rosidl_typesupport_fastrtps_cpp__visibility_control.h.in new file mode 100644 index 000000000..edce4ea73 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/resource/rosidl_typesupport_fastrtps_cpp__visibility_control.h.in @@ -0,0 +1,43 @@ +// generated from +// rosidl_typesupport_fastrtps_cpp/resource/rosidl_typesupport_fastrtps_cpp__visibility_control.h.in +// generated code does not contain a copyright notice + +#ifndef @PROJECT_NAME_UPPER@__MSG__ROSIDL_TYPESUPPORT_FASTRTPS_CPP__VISIBILITY_CONTROL_H_ +#define @PROJECT_NAME_UPPER@__MSG__ROSIDL_TYPESUPPORT_FASTRTPS_CPP__VISIBILITY_CONTROL_H_ + +#if __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT_@PROJECT_NAME@ __attribute__ ((dllexport)) + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT_@PROJECT_NAME@ __attribute__ ((dllimport)) + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT_@PROJECT_NAME@ __declspec(dllexport) + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT_@PROJECT_NAME@ __declspec(dllimport) + #endif + #ifdef ROSIDL_TYPESUPPORT_FASTRTPS_CPP_BUILDING_DLL_@PROJECT_NAME@ + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@PROJECT_NAME@ ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT_@PROJECT_NAME@ + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@PROJECT_NAME@ ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT_@PROJECT_NAME@ + #endif +#else + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT_@PROJECT_NAME@ __attribute__ ((visibility("default"))) + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_IMPORT_@PROJECT_NAME@ + #if __GNUC__ >= 4 + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@PROJECT_NAME@ __attribute__ ((visibility("default"))) + #else + #define ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@PROJECT_NAME@ + #endif +#endif + +#if __cplusplus +} +#endif + +#endif // @PROJECT_NAME_UPPER@__MSG__ROSIDL_TYPESUPPORT_FASTRTPS_CPP__VISIBILITY_CONTROL_H_ diff --git a/rosidl_typesupport_fastrtps_cpp/resource/srv__rosidl_typesupport_fastrtps_cpp.hpp.em b/rosidl_typesupport_fastrtps_cpp/resource/srv__rosidl_typesupport_fastrtps_cpp.hpp.em new file mode 100644 index 000000000..0ce70978c --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/resource/srv__rosidl_typesupport_fastrtps_cpp.hpp.em @@ -0,0 +1,44 @@ +// generated from +// rosidl_typesupport_fastrtps_cpp/resource/srv__rosidl_typesupport_fastrtps_cpp.hpp.em +// generated code does not contain a copyright notice + +@####################################################################### +@# EmPy template for generating +@# __rosidl_typesupport_fastrtps_cpp.hpp files +@# +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .srv file +@# - get_header_filename_from_srv_name (function) +@####################################################################### +@ +@{ +header_guard_parts = [ + spec.pkg_name, 'srv', + get_header_filename_from_msg_name(spec.srv_name) + '__rosidl_typesupport_fastrtps_cpp_hpp'] +header_guard_variable = '__'.join([x.upper() for x in header_guard_parts]) + '_' +}@ +#ifndef @(header_guard_variable) +#define @(header_guard_variable) + +#include + +#include "rosidl_typesupport_cpp/service_type_support.hpp" +#include "rosidl_typesupport_interface/macros.h" + +#include "@(spec.pkg_name)/msg/rosidl_typesupport_fastrtps_cpp__visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_PUBLIC_@(spec.pkg_name) +const rosidl_service_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__SERVICE_SYMBOL_NAME(rosidl_typesupport_fastrtps_cpp, @(spec.pkg_name), @(spec.srv_name))(); + +#ifdef __cplusplus +} +#endif + +#endif // @(header_guard_variable) diff --git a/rosidl_typesupport_fastrtps_cpp/resource/srv__type_support.cpp.em b/rosidl_typesupport_fastrtps_cpp/resource/srv__type_support.cpp.em new file mode 100644 index 000000000..bf720150d --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/resource/srv__type_support.cpp.em @@ -0,0 +1,64 @@ +// generated from rosidl_typesupport_fastrtps_cpp/resource/srv__type_support.cpp.em +// generated code does not contain a copyright notice + +@####################################################################### +@# EmPy template for generating __type_support.cpp files +@# +@# Context: +@# - spec (rosidl_parser.ServiceSpecification) +@# Parsed specification of the .srv file +@# - get_header_filename_from_msg_name (function) +@####################################################################### +@ +#include "@(spec.pkg_name)/srv/@(get_header_filename_from_msg_name(spec.srv_name))__rosidl_typesupport_fastrtps_cpp.hpp" + +#include "rmw/error_handling.h" +#include "rosidl_typesupport_fastrtps_cpp/identifier.hpp" +#include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/service_type_support_decl.hpp" + +#include "@(spec.pkg_name)/srv/@(get_header_filename_from_msg_name(spec.srv_name))__struct.hpp" +#include "@(spec.pkg_name)/srv/@(get_header_filename_from_msg_name(spec.srv_name + '_Request'))__rosidl_typesupport_fastrtps_cpp.hpp" +#include "@(spec.pkg_name)/srv/@(get_header_filename_from_msg_name(spec.srv_name + '_Response'))__rosidl_typesupport_fastrtps_cpp.hpp" + +namespace @(spec.pkg_name) +{ + +namespace srv +{ + +namespace typesupport_fastrtps_cpp +{ + +static service_type_support_callbacks_t callbacks = { + "@(spec.pkg_name)", + "@(spec.srv_name)", + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_cpp, @(spec.pkg_name), srv, @(spec.srv_name)_Request)(), + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_fastrtps_cpp, @(spec.pkg_name), srv, @(spec.srv_name)_Response)(), +}; + +static rosidl_service_type_support_t handle = { + rosidl_typesupport_fastrtps_cpp::typesupport_identifier, + &callbacks, + get_service_typesupport_handle_function, +}; + +} // namespace typesupport_fastrtps_cpp + +} // namespace srv + +} // namespace @(spec.pkg_name) + +#ifdef __cplusplus +extern "C" +{ +#endif + +const rosidl_service_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__SERVICE_SYMBOL_NAME(rosidl_typesupport_fastrtps_cpp, @(spec.pkg_name), @(spec.srv_name))() { + return &@(spec.pkg_name)::srv::typesupport_fastrtps_cpp::handle; +} + +#ifdef __cplusplus +} +#endif diff --git a/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp-extras.cmake.in b/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp-extras.cmake.in new file mode 100644 index 000000000..6fb87c476 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp-extras.cmake.in @@ -0,0 +1,35 @@ +# generated from +# rosidl_typesupport_fastrtps_cpp/ +# rosidl_typesupport_fastrtps_cpp-extras.cmake.in + +find_package(fastrtps_cmake_module QUIET) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +if(NOT FastRTPS_FOUND) + message(STATUS + "Could not find eProsima Fast-RTPS - skip rosidl_typesupport_fastrtps_cpp" + ) +else() + find_package(ament_cmake_core QUIET REQUIRED) + ament_register_extension( + "rosidl_generate_interfaces" + "rosidl_typesupport_fastrtps_cpp" + "rosidl_typesupport_fastrtps_cpp_generate_interfaces.cmake") + + set(rosidl_typesupport_fastrtps_cpp_BIN + "${rosidl_typesupport_fastrtps_cpp_DIR}/../../../lib/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp") + normalize_path(rosidl_typesupport_fastrtps_cpp_BIN + "${rosidl_typesupport_fastrtps_cpp_BIN}") + + set(rosidl_typesupport_fastrtps_cpp_GENERATOR_FILES + "${rosidl_typesupport_fastrtps_cpp_DIR}/../../../@PYTHON_INSTALL_DIR@/rosidl_typesupport_fastrtps_cpp/__init__.py") + normalize_path(rosidl_typesupport_fastrtps_cpp_GENERATOR_FILES + "${rosidl_typesupport_fastrtps_cpp_GENERATOR_FILES}") + + set(rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR + "${rosidl_typesupport_fastrtps_cpp_DIR}/../resource") + normalize_path(rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR + "${rosidl_typesupport_fastrtps_cpp_TEMPLATE_DIR}") +endif() diff --git a/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp/__init__.py b/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp/__init__.py new file mode 100644 index 000000000..68f2d7311 --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp/__init__.py @@ -0,0 +1,106 @@ +# Copyright 2014 Open Source Robotics Foundation, Inc. +# +# 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. + +import os + +from rosidl_cmake import convert_camel_case_to_lower_case_underscore +from rosidl_cmake import expand_template +from rosidl_cmake import get_newest_modification_time +from rosidl_parser import parse_message_file +from rosidl_parser import parse_service_file +from rosidl_parser import validate_field_types + + +def parse_ros_interface_files(pkg_name, ros_interface_files): + message_specs = [] + service_specs = [] + for idl_file in ros_interface_files: + extension = os.path.splitext(idl_file)[1] + if extension == '.msg': + message_spec = parse_message_file(pkg_name, idl_file) + message_specs.append((idl_file, message_spec)) + elif extension == '.srv': + service_spec = parse_service_file(pkg_name, idl_file) + service_specs.append(service_spec) + return (message_specs, service_specs) + + +def generate_dds_fastrtps_cpp( + pkg_name, dds_interface_files, dds_interface_base_path, deps, + output_basepath, idl_pp, message_specs, service_specs): + return 0 + + +def generate_cpp(args, message_specs, service_specs, known_msg_types): + template_dir = args['template_dir'] + mapping_msgs = { + os.path.join(template_dir, 'msg__rosidl_typesupport_fastrtps_cpp.hpp.em'): + '%s__rosidl_typesupport_fastrtps_cpp.hpp', + os.path.join(template_dir, 'msg__type_support.cpp.em'): + '%s__type_support.cpp', + } + mapping_srvs = { + os.path.join(template_dir, 'srv__rosidl_typesupport_fastrtps_cpp.hpp.em'): + '%s__rosidl_typesupport_fastrtps_cpp.hpp', + os.path.join(template_dir, 'srv__type_support.cpp.em'): + '%s__type_support.cpp', + } + + for template_file in mapping_msgs.keys(): + assert os.path.exists(template_file), 'Could not find template: ' + template_file + for template_file in mapping_srvs.keys(): + assert os.path.exists(template_file), 'Could not find template: ' + template_file + + functions = { + 'get_header_filename_from_msg_name': convert_camel_case_to_lower_case_underscore, + } + # generate_dds_fastrtps_cpp() and therefore the make target depend on the additional files + # therefore they must be listed here even if the generated type support files are independent + latest_target_timestamp = get_newest_modification_time( + args['target_dependencies'] + args.get('additional_files', [])) + + for idl_file, spec in message_specs: + validate_field_types(spec, known_msg_types) + subfolder = os.path.basename(os.path.dirname(idl_file)) + for template_file, generated_filename in mapping_msgs.items(): + generated_file = os.path.join(args['output_dir'], subfolder) + if generated_filename.endswith('.cpp'): + generated_file = os.path.join(generated_file, 'dds_fastrtps') + generated_file = os.path.join( + generated_file, generated_filename % + convert_camel_case_to_lower_case_underscore(spec.base_type.type)) + + data = {'spec': spec, 'subfolder': subfolder} + data.update(functions) + expand_template( + template_file, data, generated_file, + minimum_timestamp=latest_target_timestamp) + + for spec in service_specs: + validate_field_types(spec, known_msg_types) + for template_file, generated_filename in mapping_srvs.items(): + generated_file = os.path.join(args['output_dir'], 'srv') + if generated_filename.endswith('.cpp'): + generated_file = os.path.join(generated_file, 'dds_fastrtps') + generated_file = os.path.join( + generated_file, generated_filename % + convert_camel_case_to_lower_case_underscore(spec.srv_name)) + + data = {'spec': spec} + data.update(functions) + expand_template( + template_file, data, generated_file, + minimum_timestamp=latest_target_timestamp) + + return 0 diff --git a/rosidl_typesupport_fastrtps_cpp/src/identifier.cpp b/rosidl_typesupport_fastrtps_cpp/src/identifier.cpp new file mode 100644 index 000000000..8c991d57b --- /dev/null +++ b/rosidl_typesupport_fastrtps_cpp/src/identifier.cpp @@ -0,0 +1,23 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// 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 + +namespace rosidl_typesupport_fastrtps_cpp +{ + +ROSIDL_TYPESUPPORT_FASTRTPS_CPP_EXPORT +const char * typesupport_identifier = "rosidl_typesupport_fastrtps_cpp"; + +} // namespace rosidl_typesupport_fastrtps_cpp