From 630d20d0a8582396407aaa9dac905d0513013a2e Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Mon, 29 May 2023 14:18:37 +0200 Subject: [PATCH 01/34] reverting last commits to cf69b76 Signed-off-by: Miguel Barro --- README.md | 4 +++ ros2/CMakeLists.txt | 26 ++++++++++++++++--- ros2/src/MetaPublisher.cpp | 6 ----- ros2/src/SystemHandle.cpp | 10 ------- ros2/src/SystemHandle__foxy.cpp | 10 ------- ros2/test/CMakeLists.txt | 7 +++-- ros2/test/integration/ros2__geometry_msgs.cpp | 12 ++++----- utils/ros2-mix-generator/CMakeLists.txt | 1 - .../cmake/is_ros2_rosidl_mix.cmake | 2 +- .../resources/convert__msg.cpp.em | 18 ------------- .../resources/convert__msg.hpp.em | 5 ---- .../resources/convert__srv.cpp.em | 20 -------------- 12 files changed, 37 insertions(+), 84 deletions(-) diff --git a/README.md b/README.md index 6e83ac2..c450b5b 100644 --- a/README.md +++ b/README.md @@ -265,6 +265,10 @@ whole *Integration Service* product suite, there are some specific flags which a ~/is_ws$ colcon build --cmake-args -DBUILD_ROS2_TESTS=ON ``` +* `IS_ROS2_DISTRO`: This flag is intended to select the *ROS 2* distro that should be used to compile the *ROS 2 System Handle*. + If not set, the version will be retrieved from the last *ROS distro* sourced in the compilation environment; + this means that if the last *ROS* environment sourced corresponds to *ROS 1*, the compilation process will stop and warn the user about it. + * `MIX_ROS_PACKAGES`: It accepts as an argument a list of [ROS packages](https://index.ros.org/packages/), such as `std_msgs`, `geometry_msgs`, `sensor_msgs`, `nav_msgs`... for which the required transformation library to convert the specific ROS 2 type definitions into *xTypes*, and the other way around, will be built. diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index b17a3d3..21cce64 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -20,7 +20,7 @@ ################################################################################## cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) -project(is-ros2 VERSION "3.1.0" LANGUAGES CXX) +project(is-ros2 VERSION "3.0.0" LANGUAGES CXX) ################################################################################### # Configure options @@ -44,6 +44,26 @@ endif() # External dependencies for the Integration Service ROS 2 SystemHandle library ################################################################################### if(BUILD_LIBRARY) + # Check if the user specified which ROS 2 version he/she is using. If not, warn and try to automatically get it + if(NOT IS_ROS2_DISTRO) + + message(WARNING " + The variable 'IS_ROS2_DISTRO' was not used. You might want to set it to + specify which ROS 2 version should be used to compile the ROS 2 System Handle. + By default, a ROS 2 version from the sourced environment will be retrieved automatically.") + + if ("$ENV{ROS_VERSION}" STREQUAL "1") + message(FATAL_ERROR " + A ROS 1 version was sourced last in your build environment. + Please use the 'IS_ROS2_VERSION' variable!") + else() + set(IS_ROS2_DISTRO $ENV{ROS_DISTRO}) + endif() + + endif() + + message(STATUS "Compiling the ${PROJECT_NAME} System Handle for ROS 2 ${IS_ROS2_DISTRO}...") + find_package(is-core REQUIRED) find_package(rclcpp REQUIRED) endif() @@ -55,8 +75,8 @@ if(BUILD_LIBRARY) add_library(${PROJECT_NAME} SHARED src/Factory.cpp + $,src/SystemHandle__foxy.cpp,src/SystemHandle.cpp> src/MetaPublisher.cpp - $,src/SystemHandle.cpp,src/SystemHandle__foxy.cpp> ) if (Sanitizers_FOUND) @@ -199,4 +219,4 @@ if(BUILD_API_REFERENCE) set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION "eProsima ROS2 System Handle doxygen documentation") set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) -endif() +endif() \ No newline at end of file diff --git a/ros2/src/MetaPublisher.cpp b/ros2/src/MetaPublisher.cpp index 3748eed..0637347 100644 --- a/ros2/src/MetaPublisher.cpp +++ b/ros2/src/MetaPublisher.cpp @@ -46,7 +46,6 @@ class MetaPublisher : public is::TopicPublisher , _message_type(message_type) , _node(node) , _qos_profile(qos_profile) - , logger_("is::sh::ROS2::Publisher") { // Do nothing } @@ -67,10 +66,6 @@ class MetaPublisher : public is::TopicPublisher _message_type, _node, topic_name, _qos_profile); } - logger_ << utils::Logger::Level::INFO - << "Sending message from Integration Service to ROS 2 for topic '" << topic_name - << "': [[ " << message << " ]]" << std::endl; - return publisher->publish(message); } @@ -80,7 +75,6 @@ class MetaPublisher : public is::TopicPublisher const eprosima::xtypes::DynamicType& _message_type; rclcpp::Node& _node; const rclcpp::QoS _qos_profile; - utils::Logger logger_; using TopicPublisherPtr = std::shared_ptr; using PublisherMap = std::unordered_map; diff --git a/ros2/src/SystemHandle.cpp b/ros2/src/SystemHandle.cpp index 6a6eba1..a5c8794 100644 --- a/ros2/src/SystemHandle.cpp +++ b/ros2/src/SystemHandle.cpp @@ -185,11 +185,6 @@ rclcpp::QoS parse_rmw_qos_configuration( qos.liveliness_lease_duration(lease_duration); } } - else - { - _logger << utils::Logger::Level::INFO - << "Creating entity using the default QoS." << std::endl; - } return qos; } @@ -411,11 +406,6 @@ bool SystemHandle::configure( } } - if (success) - { - _logger << utils::Logger::Level::INFO << "Configured!" << std::endl; - } - return success; } diff --git a/ros2/src/SystemHandle__foxy.cpp b/ros2/src/SystemHandle__foxy.cpp index 9c6b475..a19db57 100644 --- a/ros2/src/SystemHandle__foxy.cpp +++ b/ros2/src/SystemHandle__foxy.cpp @@ -209,11 +209,6 @@ rclcpp::QoS parse_rmw_qos_configuration( qos.liveliness_lease_duration(lease_duration); } } - else - { - _logger << utils::Logger::Level::INFO - << "Creating entity using the default QoS." << std::endl; - } return qos; } @@ -460,11 +455,6 @@ bool SystemHandle::configure( } } - if (success) - { - _logger << utils::Logger::Level::INFO << "Configured!" << std::endl; - } - return success; } diff --git a/ros2/test/CMakeLists.txt b/ros2/test/CMakeLists.txt index 945dbbf..8bb8e39 100644 --- a/ros2/test/CMakeLists.txt +++ b/ros2/test/CMakeLists.txt @@ -54,7 +54,6 @@ macro(compile_test) target_include_directories(${TEST_NAME} PRIVATE - ${rclcpp_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} ${nav_msgs_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} @@ -82,10 +81,10 @@ compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msg compile_test(${PROJECT_NAME}_primitive_msgs SOURCE integration/ros2__primitives_msgs.cpp) compile_test(${PROJECT_NAME}_test_qos SOURCE integration/ros2__test_qos.cpp) -if (rclcpp_VERSION VERSION_GREATER 2.4.2) - compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain.cpp) -else() +if ("${IS_ROS2_DISTRO}" STREQUAL "foxy") compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) +else() + compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain.cpp) endif() set(test_msgs_config_file "ros2__test_msgs.yaml") diff --git a/ros2/test/integration/ros2__geometry_msgs.cpp b/ros2/test/integration/ros2__geometry_msgs.cpp index dbb9a4b..ce69dfa 100644 --- a/ros2/test/integration/ros2__geometry_msgs.cpp +++ b/ros2/test/integration/ros2__geometry_msgs.cpp @@ -401,12 +401,12 @@ TEST(ROS2, Request_reply_between_ros2_and_mock) // Make sure that we got the expected request message auto start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 1min) + while (std::chrono::steady_clock::now() - start_time < 30s) { executor.spin_some(); // Note: future_goal gets set after future_start, so waiting on // future_goal alone is sufficient for waiting on both. - if (future_goal.wait_for(1s) == std::future_status::ready) + if (future_goal.wait_for(100ms) == std::future_status::ready) { break; } @@ -420,10 +420,10 @@ TEST(ROS2, Request_reply_between_ros2_and_mock) EXPECT_EQ(requested_goal, plan_response.plan.poses.back()); start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 1min) + while (std::chrono::steady_clock::now() - start_time < 30s) { executor.spin_some(); - if (future_response_msg.wait_for(1s) == std::future_status::ready) + if (future_response_msg.wait_for(100ms) == std::future_status::ready) { break; } @@ -457,10 +457,10 @@ TEST(ROS2, Request_reply_between_ros2_and_mock) // should quit instead of waiting for the future and potentially hanging // forever. start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 1min) + while (std::chrono::steady_clock::now() - start_time < 30s) { executor.spin_some(); - if (future_response.wait_for(1s) == std::future_status::ready) + if (future_response.wait_for(100ms) == std::future_status::ready) { break; } diff --git a/utils/ros2-mix-generator/CMakeLists.txt b/utils/ros2-mix-generator/CMakeLists.txt index 21fcc9d..b4b5cb2 100644 --- a/utils/ros2-mix-generator/CMakeLists.txt +++ b/utils/ros2-mix-generator/CMakeLists.txt @@ -35,7 +35,6 @@ endif() find_package(is-core REQUIRED) find_package(rosidl_parser REQUIRED) find_package(is-ros2 REQUIRED) -find_package(Python3 REQUIRED) list(APPEND MIX_ROS_PACKAGES_LIST std_msgs) diff --git a/utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake b/utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake index f2fd7cb..1f52281 100644 --- a/utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake +++ b/utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake @@ -71,7 +71,7 @@ function(is_ros2_rosidl_mix) rosidl SCRIPT INTERPRETER - ${Python3_EXECUTABLE} + ${PYTHON_EXECUTABLE} FIND ${CMAKE_CURRENT_LIST_DIR}/scripts/is_ros2_rosidl_find_package_info.py GENERATE diff --git a/utils/ros2-mix-generator/resources/convert__msg.cpp.em b/utils/ros2-mix-generator/resources/convert__msg.cpp.em index a4dbf43..152bf2d 100644 --- a/utils/ros2-mix-generator/resources/convert__msg.cpp.em +++ b/utils/ros2-mix-generator/resources/convert__msg.cpp.em @@ -58,7 +58,6 @@ public: const rclcpp::QoS& qos_profile) : _callback(callback) , _message_type(message_type) - , _topic_name(topic_name) { auto subscription_options = rclcpp::SubscriptionOptionsWithAllocator>(); subscription_options.ignore_local_publications = true; // Enable ignore_local_publications option @@ -77,16 +76,8 @@ private: void subscription_callback( const Ros2_Msg& msg) { - logger << utils::Logger::Level::INFO - << "Receiving message from ROS 2 for topic '" - << _topic_name << "'" << std::endl; - xtypes::DynamicData data(_message_type); convert_to_xtype(msg, data); - - logger << utils::Logger::Level::INFO - << "Received message: [[ " << data << " ]]" << std::endl; - (*_callback)(data, nullptr); } @@ -95,8 +86,6 @@ private: const xtypes::DynamicType& _message_type; - std::string _topic_name; - // Hang onto the subscription handle to make sure the connection to the topic // stays alive rclcpp::Subscription::SharedPtr _subscription; @@ -128,7 +117,6 @@ public: rclcpp::Node& node, const std::string& topic_name, const rclcpp::QoS& qos_profile) - : _topic_name(topic_name) { _publisher = node.create_publisher( topic_name, @@ -141,10 +129,6 @@ public: Ros2_Msg ros2_msg; convert_to_ros2(message, ros2_msg); - logger << utils::Logger::Level::INFO - << "Sending message from Integration Service to ROS 2 for topic '" << _topic_name << "': " - << "[[ " << message << " ]]" << std::endl; - _publisher->publish(ros2_msg); return true; } @@ -153,8 +137,6 @@ private: rclcpp::Publisher::SharedPtr _publisher; - std::string _topic_name; - }; //============================================================================== diff --git a/utils/ros2-mix-generator/resources/convert__msg.hpp.em b/utils/ros2-mix-generator/resources/convert__msg.hpp.em index ee5a340..3b318b2 100644 --- a/utils/ros2-mix-generator/resources/convert__msg.hpp.em +++ b/utils/ros2-mix-generator/resources/convert__msg.hpp.em @@ -60,9 +60,6 @@ alphabetical_fields = sorted(spec.fields, key=lambda x: x.name) // Include the header for the conversions #include -// Include the header for the logger -#include - // Include the header for the concrete ros2 message type #include <@(ros2_msg_dependency)> @@ -127,8 +124,6 @@ inline void convert_to_xtype(const Ros2_Msg& from, eprosima::xtypes::WritableDyn (void)to; } -static eprosima::is::utils::Logger logger ("is::sh::ROS2"); - } // namespace @(namespace_variable) } // namespace ros2 } // namespace sh diff --git a/utils/ros2-mix-generator/resources/convert__srv.cpp.em b/utils/ros2-mix-generator/resources/convert__srv.cpp.em index 9b80c6e..93108e5 100644 --- a/utils/ros2-mix-generator/resources/convert__srv.cpp.em +++ b/utils/ros2-mix-generator/resources/convert__srv.cpp.em @@ -53,9 +53,6 @@ alphabetical_response_fields = sorted(spec.response.fields, key=lambda x: x.name // Include the header for the conversions #include -// Include the header for the logger -#include - // Include the header for the concrete service type #include <@(ros2_srv_dependency)> @@ -78,8 +75,6 @@ namespace sh { namespace ros2 { namespace @(namespace_variable_srv) { -static eprosima::is::utils::Logger logger ("is::sh::ROS2"); - using Ros2_Srv = @(cpp_srv_type); using Ros2_Request = Ros2_Srv::Request; using Ros2_Response = Ros2_Srv::Response; @@ -188,7 +183,6 @@ public: : _callback(callback) , _handle(std::make_shared()) , _request_data(request_type()) - , _service_name(service_name) { _service = node.create_service( service_name, @@ -209,11 +203,6 @@ public: std::static_pointer_cast(call_handle); response_to_ros2(result, _response); - - logger << utils::Logger::Level::INFO - << "Translating reply from Integration Service to ROS 2 for service reply topic '" - << _service_name << "_Reply': [[ " << result << " ]]" << std::endl; - handle->promise->set_value(_response); } @@ -224,10 +213,6 @@ private: const std::shared_ptr& request, const std::shared_ptr& response) { - logger << utils::Logger::Level::INFO - << "Receiving request from ROS 2 for service request topic '" - << _service_name << "_Request'" << std::endl; - request_to_xtype(*request, _request_data); std::promise response_promise; @@ -252,7 +237,6 @@ private: }; ServiceClientSystem::RequestCallback* _callback; - std::string _service_name; const std::shared_ptr _handle; xtypes::DynamicData _request_data; Ros2_Response _response; @@ -304,10 +288,6 @@ public: return; } - logger << utils::Logger::Level::INFO - << "Translating request from Integration Service to ROS 2 for service request topic '" - << _service_name << "_Request': [[ " << request << " ]]" << std::endl; - // This helps the lambda to value-capture the address of the Integration Service client. // TODO(MXG): Would it be dangerous for the lambda to reference-capture the // Integration Service client? The lambda might be called after this reference has left From 2c239b37eb359f1b06bce1af52cd0c9ccae83473 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Tue, 15 Jun 2021 16:22:25 +0200 Subject: [PATCH 02/34] Add new ROS2 dynamic SH Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/CMakeLists.txt | 175 +- ros2/config.hpp.in | 34 + ros2/dynamic/CMakeLists.txt | 224 ++ ros2/dynamic/CMakeLists.txt.user | 371 +++ .../dynamic/include/is/sh/ros2/Conversion.hpp | 233 ++ .../include/is/sh/ros2/Participant.hpp | 216 ++ ros2/dynamic/include/is/sh/ros2/Publisher.hpp | 152 + .../is/sh/ros2/ROS2MiddlewareException.hpp | 62 + .../dynamic/include/is/sh/ros2/Subscriber.hpp | 175 ++ ros2/dynamic/resources/CMakeLists.txt | 42 + ros2/dynamic/resources/generator.bash | 34 + ros2/dynamic/resources/package.xml | 20 + ros2/dynamic/src/Conversion.cpp | 2681 +++++++++++++++++ ros2/dynamic/src/Participant.cpp | 294 ++ ros2/dynamic/src/Publisher.cpp | 209 ++ ros2/dynamic/src/Subscriber.cpp | 333 ++ ros2/dynamic/src/SystemHandle.cpp | 632 ++++ ros2/{ => static}/CHANGELOG.rst | 0 ros2/static/CMakeLists.txt | 200 ++ ros2/{ => static}/docs/class.puml | 0 ros2/{ => static}/docs/design.md | 0 ros2/{ => static}/docs/type-inference.png | Bin .../include/is/sh/ros2/Factory.hpp | 0 ros2/{ => static}/src/Factory.cpp | 0 ros2/{ => static}/src/MetaPublisher.cpp | 0 ros2/{ => static}/src/MetaPublisher.hpp | 0 ros2/{ => static}/src/SystemHandle.cpp | 0 ros2/{ => static}/src/SystemHandle.hpp | 0 ros2/{ => static}/src/SystemHandle__foxy.cpp | 0 ros2/{ => static}/test/CHANGELOG.rst | 0 ros2/{ => static}/test/CMakeLists.txt | 0 .../test/integration/ros2__geometry_msgs.cpp | 0 .../integration/ros2__primitives_msgs.cpp | 0 .../test/integration/ros2__test_domain.cpp | 0 .../integration/ros2__test_domain__foxy.cpp | 0 .../test/resources/ros2__domain_change.yaml | 0 .../test/resources/ros2__websocket__test.yaml | 0 utils/ros2-mix-generator/CMakeLists.txt | 8 + 38 files changed, 5931 insertions(+), 164 deletions(-) create mode 100644 ros2/config.hpp.in create mode 100644 ros2/dynamic/CMakeLists.txt create mode 100644 ros2/dynamic/CMakeLists.txt.user create mode 100644 ros2/dynamic/include/is/sh/ros2/Conversion.hpp create mode 100644 ros2/dynamic/include/is/sh/ros2/Participant.hpp create mode 100644 ros2/dynamic/include/is/sh/ros2/Publisher.hpp create mode 100644 ros2/dynamic/include/is/sh/ros2/ROS2MiddlewareException.hpp create mode 100644 ros2/dynamic/include/is/sh/ros2/Subscriber.hpp create mode 100644 ros2/dynamic/resources/CMakeLists.txt create mode 100755 ros2/dynamic/resources/generator.bash create mode 100644 ros2/dynamic/resources/package.xml create mode 100644 ros2/dynamic/src/Conversion.cpp create mode 100644 ros2/dynamic/src/Participant.cpp create mode 100644 ros2/dynamic/src/Publisher.cpp create mode 100644 ros2/dynamic/src/Subscriber.cpp create mode 100644 ros2/dynamic/src/SystemHandle.cpp rename ros2/{ => static}/CHANGELOG.rst (100%) create mode 100644 ros2/static/CMakeLists.txt rename ros2/{ => static}/docs/class.puml (100%) rename ros2/{ => static}/docs/design.md (100%) rename ros2/{ => static}/docs/type-inference.png (100%) rename ros2/{ => static}/include/is/sh/ros2/Factory.hpp (100%) rename ros2/{ => static}/src/Factory.cpp (100%) rename ros2/{ => static}/src/MetaPublisher.cpp (100%) rename ros2/{ => static}/src/MetaPublisher.hpp (100%) rename ros2/{ => static}/src/SystemHandle.cpp (100%) rename ros2/{ => static}/src/SystemHandle.hpp (100%) rename ros2/{ => static}/src/SystemHandle__foxy.cpp (100%) rename ros2/{ => static}/test/CHANGELOG.rst (100%) rename ros2/{ => static}/test/CMakeLists.txt (100%) rename ros2/{ => static}/test/integration/ros2__geometry_msgs.cpp (100%) rename ros2/{ => static}/test/integration/ros2__primitives_msgs.cpp (100%) rename ros2/{ => static}/test/integration/ros2__test_domain.cpp (100%) rename ros2/{ => static}/test/integration/ros2__test_domain__foxy.cpp (100%) rename ros2/{ => static}/test/resources/ros2__domain_change.yaml (100%) rename ros2/{ => static}/test/resources/ros2__websocket__test.yaml (100%) diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index 21cce64..4fb1d4c 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -26,23 +26,11 @@ project(is-ros2 VERSION "3.0.0" LANGUAGES CXX) # Configure options ################################################################################### option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) +set(IS_ROS2_SH_MODE "Static" CACHE STRING "Select the ROS 2 SystemHandle Mode") ################################################################################### # Load external CMake Modules. ################################################################################### -if(BUILD_LIBRARY) - set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) - - find_package(Sanitizers QUIET) - - if(SANITIZE_ADDRESS) - message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") - endif() -endif() - -################################################################################### -# External dependencies for the Integration Service ROS 2 SystemHandle library -################################################################################### if(BUILD_LIBRARY) # Check if the user specified which ROS 2 version he/she is using. If not, warn and try to automatically get it if(NOT IS_ROS2_DISTRO) @@ -64,159 +52,18 @@ if(BUILD_LIBRARY) message(STATUS "Compiling the ${PROJECT_NAME} System Handle for ROS 2 ${IS_ROS2_DISTRO}...") - find_package(is-core REQUIRED) - find_package(rclcpp REQUIRED) -endif() - -################################################################################### -# Configure the Integration Service ROS 2 SystemHandle library -################################################################################### -if(BUILD_LIBRARY) - add_library(${PROJECT_NAME} - SHARED - src/Factory.cpp - $,src/SystemHandle__foxy.cpp,src/SystemHandle.cpp> - src/MetaPublisher.cpp - ) - - if (Sanitizers_FOUND) - add_sanitizers(${PROJECT_NAME}) - endif() - - set_target_properties(${PROJECT_NAME} PROPERTIES - VERSION - ${PROJECT_VERSION} - SOVERSION - ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} - CXX_STANDARD - 17 - CXX_STANDARD_REQUIRED - YES - ) - - target_compile_options(${PROJECT_NAME} - PRIVATE - $<$:-pedantic> - $<$:-fstrict-aliasing> - $<$:-Wall> - $<$:-Wextra> - $<$:-Wcast-align> - $<$:-Wshadow> - $<$:/W4> - $<$:/wd4700> - $<$:/wd4996> - $<$:/wd4820> - $<$:/wd4255> - $<$:/wd4668> - ) - - include(GNUInstallDirs) - message(STATUS "Configuring [${PROJECT_NAME}]...") - - target_link_libraries(${PROJECT_NAME} - PUBLIC - is::core - ${rclcpp_LIBRARIES} - ) - - target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ - ${rclcpp_INCLUDE_DIRS} - ) - - is_generate_export_header(ros2) - - if (NOT rosidl_runtime_cpp_FOUND) - target_compile_definitions(${PROJECT_NAME} - PUBLIC - "IS_SH_ROS2__ROSIDL_GENERATOR_CPP" - ) - endif() -endif() - -################################################################################### -# Install the Integration Service ROS 2 SystemHandle library -################################################################################### -if(BUILD_LIBRARY) - is_install_middleware_plugin( - MIDDLEWARE - ros2 - TARGET - ${PROJECT_NAME} - DEPENDENCIES - rclcpp - ) - - file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/include/ - DESTINATION - ${CMAKE_INSTALL_PREFIX}/include/ + # Generate the config.hpp file + configure_file(${PROJECT_SOURCE_DIR}/config.hpp.in + ${CMAKE_INSTALL_PREFIX}/include/is/sh/ros2/config.hpp ) -endif() -################################################################################### -# Integration Service ROS 2 SystemHandle tests -################################################################################### -if(BUILD_LIBRARY) - if(BUILD_TESTS OR BUILD_ROS2_TESTS) - add_subdirectory(test) - endif() -endif() + # Build ROS2 Static SH or ROS2 Dynamic SH + set(IS_ROS2_SH_MODE_LOWERCASE "" CACHE STRING "Build mode to lowercase") + string(TOLOWER "${IS_ROS2_SH_MODE}" IS_ROS2_SH_MODE_LOWERCASE) -################################################################################### -# Integration Service ROS 2 SystemHandle API Reference -################################################################################### -if(BUILD_API_REFERENCE) - if(NOT BUILD_LIBRARY) - find_file(IS_DOXYGEN_CONFIG_FILE doxygen-config.in PATHS ${CMAKE_INSTALL_PREFIX}/../is-core) + if("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "static") + add_subdirectory(static) + elseif("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "dynamic") + add_subdirectory(dynamic) endif() - find_package(Doxygen REQUIRED) - # Create doxygen directories - add_custom_target(doc-dirs - COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen - COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen/html - COMMENT "Creating documentation directories" VERBATIM) - - set(IS_ROS2_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/include/is/sh/ros2") - set(IS_ROS2_SOURCE_DIR "${CMAKE_SOURCE_DIR}/src") - file(GLOB_RECURSE HPP_FILES - "${IS_ROS2_INCLUDE_DIR}/*.h*" - "${IS_ROS2_SOURCE_DIR}/*.h*") - - # Doxygen related variables - set(DOXYGEN_INPUT_DIR "${IS_ROS2_INCLUDE_DIR} ${IS_ROS2_SOURCE_DIR}") - set(DOXYGEN_OUTPUT_DIR "${PROJECT_BINARY_DIR}/doxygen") - set(DOXYGEN_INDEX_FILE "${PROJECT_BINARY_DIR}/doxygen/xml/index.xml") - set(DOXYFILE_IN "${IS_DOXYGEN_CONFIG_FILE}") - set(DOXYFILE_OUT ${PROJECT_BINARY_DIR}/doxygen-config) - set(DOXYGEN_TAGFILES "../is-core/is_core.tag=../is-core/doxygen/html") - set(DOXYGEN_HTML_DIR "${PROJECT_BINARY_DIR}/doxygen/html") - - # Configure doxygen - configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) - - # Doxygen command - add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} - COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} - DEPENDS ${HPP_FILES} - MAIN_DEPENDENCY ${DOXYFILE_OUT} ${DOXYFILE_IN} - COMMENT "Generating doxygen documentation") - - # Generate API reference - add_custom_target(doxygen-${PROJECT_NAME} ALL - DEPENDS ${DOXYGEN_INDEX_FILE} - COMMENT "Generated API documentation with doxygen" VERBATIM) - add_dependencies(doxygen-${PROJECT_NAME} doc-dirs) - - # Install doxygen generated XML files - install(DIRECTORY ${PROJECT_BINARY_DIR}/doxygen/xml - DESTINATION doxygen - COMPONENT doxygen-${PROJECT_NAME}) - set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "ROS2 System Handle doxygen") - set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION - "eProsima ROS2 System Handle doxygen documentation") - set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) endif() \ No newline at end of file diff --git a/ros2/config.hpp.in b/ros2/config.hpp.in new file mode 100644 index 0000000..0ec6272 --- /dev/null +++ b/ros2/config.hpp.in @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2021 - present 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 _IS_ROS2_DISTRO_HPP_ +#define _IS_ROS2_DISTRO_HPP_ + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +const std::string ROS2_DISTRO = "@IS_ROS2_DISTRO@"; + +} //namespace ros2 +} //namespace is +} //namespace sh +} //namespace eprosima + + +#endif // _IS_ROS2_DISTRO_HPP_ \ No newline at end of file diff --git a/ros2/dynamic/CMakeLists.txt b/ros2/dynamic/CMakeLists.txt new file mode 100644 index 0000000..4dc4da1 --- /dev/null +++ b/ros2/dynamic/CMakeLists.txt @@ -0,0 +1,224 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# Copyright (C) 2020 - present 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. +# +# library and integration-service executable CMake project + +################################################################################## +# CMake build rules for the Integration Service ROS 2 SystemHandle library +################################################################################## +cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) + +################################################################################### +# Configure options +################################################################################### +option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) + +################################################################################### +# Load external CMake Modules. +################################################################################### +if(BUILD_LIBRARY) + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) + + find_package(Sanitizers QUIET) + + if(SANITIZE_ADDRESS) + message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") + endif() +endif() + +################################################################################### +# External dependencies for the Integration Service ROS 2 SystemHandle library +################################################################################### +if(BUILD_LIBRARY) + # Check if the user specified which ROS 2 version he/she is using. If not, warn and try to automatically get it + if(NOT IS_ROS2_DISTRO) + + message(WARNING " + The variable 'IS_ROS2_DISTRO' was not used. You might want to set it to + specify which ROS 2 version should be used to compile the ROS 2 System Handle. + By default, a ROS 2 version from the sourced environment will be retrieved automatically.") + + if ("$ENV{ROS_VERSION}" STREQUAL "1") + message(FATAL_ERROR " + A ROS 1 version was sourced last in your build environment. + Please use the 'IS_ROS2_VERSION' variable!") + else() + set(IS_ROS2_DISTRO $ENV{ROS_DISTRO}) + endif() + + endif() + + message(STATUS "Compiling the ${PROJECT_NAME} System Handle for ROS 2 ${IS_ROS2_DISTRO}...") + + find_package(is-core REQUIRED) + find_package(fastrtps REQUIRED) +endif() + +################################################################################### +# Configure the Integration Service ROS 2 SystemHandle library +################################################################################### +if(BUILD_LIBRARY) + add_library(${PROJECT_NAME} + SHARED + src/Conversion.cpp + src/Participant.cpp + src/Publisher.cpp + src/Subscriber.cpp + src/SystemHandle.cpp + #$,src/SystemHandle__foxy.cpp,src/SystemHandle.cpp> + ) + + if (Sanitizers_FOUND) + add_sanitizers(${PROJECT_NAME}) + endif() + + set_target_properties(${PROJECT_NAME} PROPERTIES + VERSION + ${PROJECT_VERSION} + SOVERSION + ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + CXX_STANDARD + 17 + CXX_STANDARD_REQUIRED + YES + ) + + target_compile_options(${PROJECT_NAME} + PRIVATE + $<$:-pedantic> + $<$:-fstrict-aliasing> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wcast-align> + $<$:-Wshadow> + $<$:/W4> + $<$:/wd4700> + $<$:/wd4996> + $<$:/wd4820> + $<$:/wd4255> + $<$:/wd4668> + ) + + include(GNUInstallDirs) + message(STATUS "Configuring [${PROJECT_NAME}]...") + + target_link_libraries(${PROJECT_NAME} + PUBLIC + is::core + fastrtps + ) + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + $ + ) + + #is_generate_export_header(ros2) + +endif() + +################################################################################### +# Install the Integration Service ROS 2 SystemHandle library +################################################################################### +if(BUILD_LIBRARY) + is_install_middleware_plugin( + MIDDLEWARE + ros2_dynamic + TARGET + ${PROJECT_NAME} + ) + + file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/include/ + DESTINATION + ${CMAKE_INSTALL_PREFIX}/include/ + ) + + file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/resources/ + DESTINATION + /tmp/ + ) + +endif() + +################################################################################### +# Integration Service ROS 2 SystemHandle tests +################################################################################### +# if(BUILD_LIBRARY) +# if(BUILD_TESTS OR BUILD_ROS2_TESTS) +# add_subdirectory(test) +# endif() +# endif() + +################################################################################### +# Integration Service ROS 2 SystemHandle API Reference +################################################################################### +# if(BUILD_API_REFERENCE) +# if(NOT BUILD_LIBRARY) +# find_file(IS_DOXYGEN_CONFIG_FILE doxygen-config.in PATHS ${CMAKE_INSTALL_PREFIX}/../is-core) +# endif() +# find_package(Doxygen REQUIRED) +# # Create doxygen directories +# add_custom_target(doc-dirs +# COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen +# COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen/html +# COMMENT "Creating documentation directories" VERBATIM) + +# set(IS_ROS2_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/include/is/sh/ros2") +# set(IS_ROS2_SOURCE_DIR "${CMAKE_SOURCE_DIR}/src") +# file(GLOB_RECURSE HPP_FILES +# "${IS_ROS2_INCLUDE_DIR}/*.h*" +# "${IS_ROS2_SOURCE_DIR}/*.h*") + +# # Doxygen related variables +# set(DOXYGEN_INPUT_DIR "${IS_ROS2_INCLUDE_DIR} ${IS_ROS2_SOURCE_DIR}") +# set(DOXYGEN_OUTPUT_DIR "${PROJECT_BINARY_DIR}/doxygen") +# set(DOXYGEN_INDEX_FILE "${PROJECT_BINARY_DIR}/doxygen/xml/index.xml") +# set(DOXYFILE_IN "${IS_DOXYGEN_CONFIG_FILE}") +# set(DOXYFILE_OUT ${PROJECT_BINARY_DIR}/doxygen-config) +# set(DOXYGEN_TAGFILES "../is-core/is_core.tag=../is-core/doxygen/html") +# set(DOXYGEN_HTML_DIR "${PROJECT_BINARY_DIR}/doxygen/html") + +# # Configure doxygen +# configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) + +# # Doxygen command +# add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} +# COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} +# DEPENDS ${HPP_FILES} +# MAIN_DEPENDENCY ${DOXYFILE_OUT} ${DOXYFILE_IN} +# COMMENT "Generating doxygen documentation") + +# # Generate API reference +# add_custom_target(doxygen-${PROJECT_NAME} ALL +# DEPENDS ${DOXYGEN_INDEX_FILE} +# COMMENT "Generated API documentation with doxygen" VERBATIM) +# add_dependencies(doxygen-${PROJECT_NAME} doc-dirs) + +# # Install doxygen generated XML files +# install(DIRECTORY ${PROJECT_BINARY_DIR}/doxygen/xml +# DESTINATION doxygen +# COMPONENT doxygen-${PROJECT_NAME}) +# set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "ROS2 System Handle doxygen") +# set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION +# "eProsima ROS2 System Handle doxygen documentation") +# set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) +# endif() \ No newline at end of file diff --git a/ros2/dynamic/CMakeLists.txt.user b/ros2/dynamic/CMakeLists.txt.user new file mode 100644 index 0000000..02dae41 --- /dev/null +++ b/ros2/dynamic/CMakeLists.txt.user @@ -0,0 +1,371 @@ + + + + + + EnvironmentId + {856bbe81-b67f-4809-8a78-3bbf3a9d4c31} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + UTF-8 + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 0 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {c88d0898-f70a-4fb4-8177-c0f98e0905b8} + 0 + 0 + 0 + + + /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Default + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Default + Default + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=Debug + + /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Debug + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Debug + Debug + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=Release + + /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Release + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Release + Release + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=RelWithDebInfo + + /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Release with Debug Information + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Release with Debug Information + Release with Debug Information + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=MinSizeRel + + /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Minimum Size Release + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + all + + true + CMake Build + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Minimum Size Release + Minimum Size Release + CMakeProjectManager.CMakeBuildConfiguration + + 5 + + + 0 + Deploy + + ProjectExplorer.BuildSteps.Deploy + + 1 + Deploy locally + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + true + true + /usr/bin/valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + -1 + + + + %{buildDir} + Custom Executable + + ProjectExplorer.CustomExecutableRunConfiguration + 3768 + false + true + false + false + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 18 + + + Version + 18 + + diff --git a/ros2/dynamic/include/is/sh/ros2/Conversion.hpp b/ros2/dynamic/include/is/sh/ros2/Conversion.hpp new file mode 100644 index 0000000..81e9caf --- /dev/null +++ b/ros2/dynamic/include/is/sh/ros2/Conversion.hpp @@ -0,0 +1,233 @@ +/* + * Copyright 2019 - present 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 _IS_SH_FASTDDS__INTERNAL__CONVERSION_HPP_ +#define _IS_SH_FASTDDS__INTERNAL__CONVERSION_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace fastdds = eprosima::fastdds; +namespace xtypes = eprosima::xtypes; +using namespace eprosima::fastrtps::types; + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +/** + * TODO(jamoralp): Remove this workaround once get_union_id() is added to DynamicData class. + * PR: https://github.com/eProsima/Fast-DDS/pull/1903 + */ +class UnionDynamicData : public DynamicData +{ +public: + + UnionDynamicData() = default; + + MemberId get_union_id() const + { + return union_id_; + } + +}; + +struct NavigationNode +{ + std::string member_name; + std::string type_name; + std::map > member_node; + std::weak_ptr parent_node; + + std::string get_path(); + + static std::string get_type( + std::map > map_nodes, + const std::string& full_path); + + static std::shared_ptr fill_root_node( + std::shared_ptr root, + const ::xtypes::DynamicType& type, + const std::string& full_path); + + /** + * @brief get_discriminator Introspects data until it finds an active member of type contained in member_types. + * This is useful for discriminate Union types in request/reply types. + */ + static std::shared_ptr get_discriminator( + const std::map >& member_map, + const ::xtypes::DynamicData& data, + const std::vector& member_types); + +private: + + static std::string get_type( + std::shared_ptr root, + const std::string& full_path); + + static void fill_member_node( + std::shared_ptr node, + const ::xtypes::DynamicType& type, + const std::string& full_path); + + static std::shared_ptr get_discriminator( + std::shared_ptr node, + ::xtypes::ReadableDynamicDataRef data, + const std::vector& member_types); + + static utils::Logger logger_; +}; + +struct Conversion +{ + static bool xtypes_to_fastdds( + const ::xtypes::DynamicData& input, + DynamicData* output); + + static bool fastdds_to_xtypes( + const DynamicData* input, + ::xtypes::DynamicData& output); + + static ::xtypes::DynamicData dynamic_data( + const std::string& type_name); + + static void register_type( + const std::string& type_name, + DynamicPubSubType* type) + { + registered_types_.emplace(type_name, type); + } + + static DynamicTypeBuilder* create_builder( + const xtypes::DynamicType& type, + const std::string& type_name); + + // This function patches the problem of dynamic types, which do not admit '/' in their type name. + static std::string convert_type_name( + const std::string& message_type); + + static const xtypes::DynamicType& resolve_discriminator_type( + const ::xtypes::DynamicType& service_type, + const std::string& discriminator); + + static ::xtypes::WritableDynamicDataRef access_member_data( + ::xtypes::WritableDynamicDataRef membered_data, + const std::string& path); + +private: + + ~Conversion() = default; + static std::map types_; + static std::map registered_types_; + static std::map builders_; + + static const xtypes::DynamicType& resolve_type( + const xtypes::DynamicType& type); + + static TypeKind resolve_type( + const DynamicType_ptr type); + + static DynamicTypeBuilder_ptr get_builder( + const xtypes::DynamicType& type); + + static void get_array_specs( + const xtypes::ArrayType& array, + std::pair, DynamicTypeBuilder_ptr>& result); + + // xtypes Dynamic Data -> FastDDS Dynamic Data + static void set_primitive_data( + xtypes::ReadableDynamicDataRef from, + DynamicData* to, + eprosima::fastrtps::types::MemberId id); + + // xtypes Dynamic Data -> FastDDS Dynamic Data + static void set_sequence_data( + ::xtypes::ReadableDynamicDataRef from, + DynamicData* to); + + // xtypes Dynamic Data -> FastDDS Dynamic Data + static void set_map_data( + ::xtypes::ReadableDynamicDataRef from, + DynamicData* to); + + // xtypes Dynamic Data -> FastDDS Dynamic Data + static void set_array_data( + xtypes::ReadableDynamicDataRef from, + DynamicData* to, + const std::vector& indexes); + + // xtypes Dynamic Data -> FastDDS Dynamic Data + static bool set_struct_data( + ::xtypes::ReadableDynamicDataRef input, + DynamicData* output); + + // xtypes Dynamic Data -> FastDDS Dynamic Data + static bool set_union_data( + ::xtypes::ReadableDynamicDataRef input, + DynamicData* output); + + // FastDDS Dynamic Data -> xtypes Dynamic Data + static void set_sequence_data( + const DynamicData* from, + ::xtypes::WritableDynamicDataRef to); + + // FastDDS Dynamic Data -> xtypes Dynamic Data + static void set_map_data( + const DynamicData* from, + ::xtypes::WritableDynamicDataRef to); + + // FastDDS Dynamic Data -> xtypes Dynamic Data + static void set_array_data( + const DynamicData* from, + ::xtypes::WritableDynamicDataRef to, + const std::vector& indexes); + + // FastDDS Dynamic Data -> xtypes Dynamic Data + static bool set_struct_data( + const DynamicData* input, + ::xtypes::WritableDynamicDataRef output); + + // FastDDS Dynamic Data -> xtypes Dynamic Data + static bool set_union_data( + const DynamicData* input, + ::xtypes::WritableDynamicDataRef output); + + static ::xtypes::WritableDynamicDataRef access_member_data( + ::xtypes::WritableDynamicDataRef membered_data, + const std::vector& tokens, + size_t index); + + static utils::Logger logger_; +}; + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima + +#endif // _IS_SH_FASTDDS__INTERNAL__CONVERSION_HPP_ + diff --git a/ros2/dynamic/include/is/sh/ros2/Participant.hpp b/ros2/dynamic/include/is/sh/ros2/Participant.hpp new file mode 100644 index 0000000..37d5a0f --- /dev/null +++ b/ros2/dynamic/include/is/sh/ros2/Participant.hpp @@ -0,0 +1,216 @@ +/* + * Copyright 2019 - present 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 _IS_SH_FASTDDS__INTERNAL__PARTICIPANT_HPP_ +#define _IS_SH_FASTDDS__INTERNAL__PARTICIPANT_HPP_ + +#include "ROS2MiddlewareException.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace fastdds = eprosima::fastdds; + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +/** + * @class Participant + * This class represents a ROS2 Dynamic Node within the *Integration Service* framework. + * + * @details It includes a mapping of the topic names to their corresponding + * + * Dynamic Type representation, and also mappings to identify each topic with its type. + * + * This class inherits from + * + * Fast DDS DomainParticipantListener class to scan for state changes on the DDS + * participant created by this *Integration Service* is::SystemHandle. + */ +class Participant +{ +public: + + /** + * @brief Construct a new Participant, with default values. + * + * @throws ROS2MiddlewareException If the *DomainParticipant* could not be created. + */ + Participant(); + + /** + * @brief Construct a new Participant object with the user-provided parameters + * in the *YAML* configuration file. + * + * @param[in] config The configuration provided by the user. + * It must contain two keys in the *YAML* map: + * + * - `file_path`: Specifies the path to the XML profile that will be used to configure the + * *DomainParticipant*. More information on how to write these XML profiles can be found + * + * here. + * + * - `profile_name`: Provide a name to search for within the profiles defined in the XML + * that corresponds to the configuration profile that we want this Participant + * to be configured with. + * + * @throws ROS2MiddlewareException If the XML profile was incorrect and, thus, the + * *DomainParticipant* could not be created. + */ + Participant( + const YAML::Node& config); + + /** + * @brief Destroy the Participant object. + */ + virtual ~Participant(); + + /** + * @brief Construct a *Fast DDS DomainParticipant*, given its DDS domain ID. + * + * @param[in] domain_id The DDS domain ID for this participant. + * + * @throws ROS2MiddlewareException If the *DomainParticipant* could not be created. + */ + void build_participant( + const ::fastdds::dds::DomainId_t& domain_id = 0, + const std::string& participant_name = "default_IS-ROS2-Dynamic-SH_participant"); + + /** + * @brief Get the associate *FastDDS DomainParticipant* attribute. + * + * @returns The DDS participant. + */ + ::fastdds::dds::DomainParticipant* get_dds_participant() const; + + /** + * @brief Register a *Dynamic Type* within the types map. Also, register the + * associated DDS topic. + * + * @param[in] topic_name The topic name to be associated to the *Dynamic Type*. + * + * @param[in] type_name The type name to be registered in the factory. + * + * @param[in] builder A class that represents a builder for the desired *Dynamic Type*. + * + * @throws ROS2MiddlewareException If the type could not be registered. + */ + void register_dynamic_type( + const std::string& topic_name, + const std::string& type_name, + fastrtps::types::DynamicTypeBuilder* builder); + + /** + * @brief Create an empty dynamic data object for the specified topic. + * + * @param[in] topic_name The topic name. + * + * @returns The empty DynamicData for the required topic. + * + * @throws ROS2MiddlewareException if the topic was not found + * or the type was not registered previously. + */ + fastrtps::types::DynamicData* create_dynamic_data( + const std::string& topic_name) const; + + /** + * @brief Delete a certain dynamic data from the *DomainParticipant* database. + * + * @param[in] data The dynamic data to be deleted. + */ + void delete_dynamic_data( + fastrtps::types::DynamicData* data) const; + + /** + * @brief Get the dynamic type pointer associated to a certain key. + * + * @param[in] name The key to find within the types map. + * + * @returns The pointer to the dynamic type if found, or `nullptr` otherwise. + */ + const fastrtps::types::DynamicType* get_dynamic_type( + const std::string& name) const; + + /** + * @brief Get the type name associated to a certain topic. + * + * @param[in] topic The topic whose type is wanted to be retrieved. + * + * @returns A const reference to the topic type's name. + */ + const std::string& get_topic_type( + const std::string& topic) const; + + /** + * @brief Register a topic into the topics map. + * + * @note This method is a workaround until `fastdds::dds::DomainParticipant::find_topic` gets implemented. + * + * @param[in] topic The name of the topic to register. + * + * @param[in] entity A pointer to the entity to be registered. + */ + + void associate_topic_to_dds_entity( + ::fastdds::dds::Topic* topic, + ::fastdds::dds::DomainEntity* entity); + + /** + * @brief Unregister a topic from the topics map. + * + * @note This method is a workaround until `fastdds::dds::DomainParticipant::find_topic` gets implemented. + * + * @param[in] topic The name of the topic to unregister. + * + * @param[in] entity A pointer to the entity to be unregistered. + */ + + bool dissociate_topic_from_dds_entity( + ::fastdds::dds::Topic* topic, + ::fastdds::dds::DomainEntity* entity); + +private: + + /** + * Class members. + */ + ::fastdds::dds::DomainParticipant* dds_participant_; + + std::map types_; + std::map topic_to_type_; + std::map<::fastdds::dds::Topic*, std::set<::fastdds::dds::DomainEntity*> > topic_to_entities_; + std::mutex topic_to_entities_mtx_; + + is::utils::Logger logger_; +}; + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima + +#endif // _IS_SH_FASTDDS__INTERNAL__PARTICIPANT_HPP_ diff --git a/ros2/dynamic/include/is/sh/ros2/Publisher.hpp b/ros2/dynamic/include/is/sh/ros2/Publisher.hpp new file mode 100644 index 0000000..aa684ce --- /dev/null +++ b/ros2/dynamic/include/is/sh/ros2/Publisher.hpp @@ -0,0 +1,152 @@ +/* + * Copyright 2019 - present 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 _IS_SH_FASTDDS__INTERNAL__PUBLISHER_HPP_ +#define _IS_SH_FASTDDS__INTERNAL__PUBLISHER_HPP_ + +#include "ROS2MiddlewareException.hpp" +#include "Participant.hpp" + +#include +#include +#include + +#include + +namespace fastdds = eprosima::fastdds; + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +/** + * @brief Forward declaration. + */ +class Participant; + +/** + * @class Publisher + * This class represents a + * Fast DDS Publisher within the *Integration Service* framework. + * + * Its topic type definition and data instances are represented by means + * of the + * Fast DDS Dynamic Types API, which allows to get rid of + * TypeSupport for each used type and eases users the task of defining and using their own custom + * types on the go, by means of a valid *IDL* definition. + * + * This class inherits from + * Fast DDS Data Writer Listener for reacting to datawriter events, + * such as matching with subscribers. + */ +class Publisher + : public virtual is::TopicPublisher + , private ::fastdds::dds::DataWriterListener +{ +public: + + /** + * @brief Construct a new Publisher object. + * + * @param[in] participant The associated *Integration Service* Participant, + * that holds this Publisher. + * + * @param[in] topic_name The topic that this DDS publisher will send data to. + * + * @param[in] message_type A dynamic type definition of the topic's type. + * + * @param[in] config Specific configuration regarding this publisher, in *YAML* format. + * Allowed fields are: + * - `service_instance_name`: Specify the DDS RPC service instance name property. + * + * @throws ROS2MiddlewareException if some error occurs while creating the *Fast DDS* publisher. + */ + Publisher( + Participant* participant, + const std::string& topic_name, + const xtypes::DynamicType& message_type, + const YAML::Node& config); + + // TODO(@jamoralp): Create publisher based on XML profiles? + + /** + * @brief Destroy the Publisher object. + */ + virtual ~Publisher() override; + + /** + * @brief Publisher shall not be copy constructible. + */ + Publisher( + const Publisher& /*rhs*/) = delete; + + /** + * @brief Publisher shall not be copy assignable. + */ + Publisher& operator = ( + const Publisher& /*rhs*/) = delete; + + /** + * @brief Publisher shall not be move constructible. + */ + Publisher( + Publisher&& /*rhs*/) = delete; + + /** + * @brief Publisher shall not be move assignable. + */ + Publisher& operator = ( + Publisher&& /*rhs*/) = delete; + + /** + * @brief Inherited from TopicPublisher. + */ + bool publish( + const xtypes::DynamicData& message) override; + +private: + + /** + * @brief Inherited from *DataWriterListener*. + */ + void on_publication_matched( + ::fastdds::dds::DataWriter* /*writer*/, + const ::fastdds::dds::PublicationMatchedStatus& info) override; + + /** + * Class members. + */ + Participant* participant_; + ::fastdds::dds::Publisher* dds_publisher_; + ::fastdds::dds::Topic* dds_topic_; + ::fastdds::dds::DataWriter* dds_datawriter_; + + fastrtps::types::DynamicData* dynamic_data_; + std::mutex data_mtx_; + + const std::string topic_name_; + + utils::Logger logger_; +}; + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima + +#endif // _IS_SH_FASTDDS__INTERNAL__PARTICIPANT_HPP_ diff --git a/ros2/dynamic/include/is/sh/ros2/ROS2MiddlewareException.hpp b/ros2/dynamic/include/is/sh/ros2/ROS2MiddlewareException.hpp new file mode 100644 index 0000000..7d7e30c --- /dev/null +++ b/ros2/dynamic/include/is/sh/ros2/ROS2MiddlewareException.hpp @@ -0,0 +1,62 @@ +/* + * Copyright 2019 - present 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 _IS_SH_FASTDDS__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ +#define _IS_SH_FASTDDS__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ + +#include + +#include + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +/** + * @class ROS2MiddlewareException + * Launches a runtime error every time an unexpected behavior occurs + * related to *Fast DDS* middleware, when configuring or using this is::SystemHandle. + */ +class ROS2MiddlewareException : public std::runtime_error +{ +public: + + /** + * @brief Construct a new ROS2MiddlewareException object. + * + * @param[in] logger The logging tool. + * + * @param[in] message The message to throw the runtime error with. + */ + ROS2MiddlewareException( + const utils::Logger& logger, + const std::string& message) + : std::runtime_error(message) + , from_logger(logger) + { + } + + utils::Logger from_logger; +}; + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima + +#endif // _IS_SH_FASTDDS__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ diff --git a/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp b/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp new file mode 100644 index 0000000..01aef0c --- /dev/null +++ b/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp @@ -0,0 +1,175 @@ +/* + * Copyright 2019 - present 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 _IS_SH_FASTDDS__INTERNAL__SUBSCRIBER_HPP_ +#define _IS_SH_FASTDDS__INTERNAL__SUBSCRIBER_HPP_ + +#include "ROS2MiddlewareException.hpp" +#include "Participant.hpp" + +#include +#include + +#include + +#include +#include + +namespace fastdds = eprosima::fastdds; + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +/** + * @brief Forward declaration. + */ +class Participant; + +/** + * @class Subscriber + * This class represents a + * Fast DDS Subscriber within the *Integration Service* framework. + * + * Its topic type definition and data instances are represented by means + * of the + * Fast DDS Dynamic Types API, which allows to get rid of + * TypeSupport for each used type and eases users the task of defining and using their own custom + * types on the go, by means of a valid *IDL* definition. + * + * This class inherits from + * Fast DDS Data Reader Listener for reacting to datareader events, + * such as matching with publishers or receiving new data from them. + */ +class Subscriber : private ::fastdds::dds::DataReaderListener +{ +public: + + /** + * @brief Construct a new Subscriber object. + * + * @param[in] participant The associated *Integration Service* Participant, + * which holds this Subscriber. + * + * @param[in] topic_name The topic that this DDS subscriber will attach to. + * + * @param[in] message_type A dynamic type definition of the topic's type. + * + * @param[in] is_callback Callback function signature defined by the *Integration Service*, + * triggered each time a new data arrives to the DDS Subscriber. + * + * @throws ROS2MiddlewareException if some error occurs while creating the *Fast DDS* subscriber. + */ + Subscriber( + Participant* participant, + const std::string& topic_name, + const xtypes::DynamicType& message_type, + TopicSubscriberSystem::SubscriptionCallback* is_callback); + + // TODO(@jamoralp): Create subscriber based on XML profiles? + + /** + * @brief Destroy the Subscriber object. + */ + virtual ~Subscriber(); + + /** + * @brief Subscriber shall not be copy constructible. + */ + Subscriber( + const Subscriber& /*rhs*/) = delete; + + /** + * @brief Subscriber shall not be copy assignable. + */ + Subscriber& operator = ( + const Subscriber& /*rhs*/) = delete; + + /** + * @brief Subscriber shall not be move constructible. + */ + Subscriber( + Subscriber&& /*rhs*/) = delete; + + /** + * @brief Subscriber shall not be move assignable. + */ + Subscriber& operator = ( + Subscriber&& /*rhs*/) = delete; + + /** + * @brief Handle the receiving of a new message from the DDS dataspace. + * + * @param[in] dds_message The incoming message. + */ + void receive( + const fastrtps::types::DynamicData* dds_message); + +private: + + /** + * @brief Inherited from *DataReaderListener*. + */ + void on_data_available( + ::fastdds::dds::DataReader* /*reader*/) override; + + /** + * @brief Inherited from *DataReaderListener*. + */ + void on_subscription_matched( + ::fastdds::dds::DataReader* reader, + const ::fastdds::dds::SubscriptionMatchedStatus& info) override; + + /** + * @brief Function to look for threads that have already finished and delete them from the + * reception threads database. + */ + void cleaner_function(); + + /** + * Class members. + */ + Participant* participant_; + ::fastdds::dds::Subscriber* dds_subscriber_; + ::fastdds::dds::Topic* dds_topic_; + ::fastdds::dds::DataReader* dds_datareader_; + + fastrtps::types::DynamicData* dynamic_data_; + std::mutex data_mtx_; + + const std::string topic_name_; + const xtypes::DynamicType& message_type_; + + TopicSubscriberSystem::SubscriptionCallback* is_callback_; + + std::map reception_threads_; + bool stop_cleaner_; + std::vector finished_threads_; + std::mutex cleaner_mtx_; + std::condition_variable cleaner_cv_; + std::thread cleaner_thread_; + + utils::Logger logger_; +}; + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima + +#endif // SOSS__DDS__INTERNAL__SUBSCRIBER_HPP diff --git a/ros2/dynamic/resources/CMakeLists.txt b/ros2/dynamic/resources/CMakeLists.txt new file mode 100644 index 0000000..74f4d68 --- /dev/null +++ b/ros2/dynamic/resources/CMakeLists.txt @@ -0,0 +1,42 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# Copyright (C) 2020 - present 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. +# +# library and integration-service executable CMake project + +################################################################################## +# CMake build rules for the Integration Service ROS 2 SystemHandle library +################################################################################## +cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) + +# IS_PACKAGE_NAME = Project name + +project(${IS_PACKAGE_NAME}) + +foreach(PKG ${PACKAGE_DEPENDENCIES}) + find_package(${PKG} REQUIRED) +endforeach() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +file(GLOB_RECURSE IDL_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "*.idl") + +rosidl_generate_interfaces(${IS_PACKAGE_NAME} + ${IDL_FILES} + DEPENDENCIES ${PACKAGE_DEPENDENCIES} + "" +) + +ament_package() \ No newline at end of file diff --git a/ros2/dynamic/resources/generator.bash b/ros2/dynamic/resources/generator.bash new file mode 100755 index 0000000..ac224fd --- /dev/null +++ b/ros2/dynamic/resources/generator.bash @@ -0,0 +1,34 @@ +package_name="" +dependencies="" +install_path="" + +while [[ "$#" -gt 0 ]]; do + case $1 in + -p|--package_name) package_name="$2"; shift ;; + -d|--dependencies) dependencies="$2"; shift ;; + -i|--install_path) install_path="$2"; shift ;; + *) echo "Unknown parameter passed: $1" ;; + esac + shift +done + +echo "Generating Type Support for package $package_name with dependencies $dependencies"; + +cp /tmp/CMakeLists.txt /tmp/$package_name/ + +sed "s#\([^<][^<]*\)#$package_name#" /tmp/package.xml > /tmp/$package_name/package.xml + +cd /tmp/$package_name + +echo "Installing the generated Type Support in $install_path"; + +cmake . -DIS_PACKAGE_NAME=$package_name -DPACKAGE_DEPENDENCIES=$dependencies -DCMAKE_INSTALL_PREFIX:PATH=$install_path + +cmake --build . --target install + +if [ $? -ne 0 ] +then + exit 1 +fi + +. $install_path/setup.bash \ No newline at end of file diff --git a/ros2/dynamic/resources/package.xml b/ros2/dynamic/resources/package.xml new file mode 100644 index 0000000..71cf7f8 --- /dev/null +++ b/ros2/dynamic/resources/package.xml @@ -0,0 +1,20 @@ + + + + is_ros2_msgs + 0.0.0 + Integration Service ROS 2 SystemHandle messages. + Eprosima + Apache License 2.0 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/ros2/dynamic/src/Conversion.cpp b/ros2/dynamic/src/Conversion.cpp new file mode 100644 index 0000000..374723f --- /dev/null +++ b/ros2/dynamic/src/Conversion.cpp @@ -0,0 +1,2681 @@ +/* + * Copyright 2019 - present 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 + +using eprosima::fastrtps::types::MemberId; + +// TODO(@jamoralp): Add more debug traces here. + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +namespace xtypes = eprosima::xtypes; +using namespace eprosima::fastrtps; +using namespace eprosima::fastrtps::types; + +std::map Conversion::types_; +std::map Conversion::registered_types_; +std::map Conversion::builders_; + +// Static member initialization +utils::Logger NavigationNode::logger_("is::sh::ROS2 Dynamic::Conversion::NavigationNode"); + +std::string NavigationNode::get_path() +{ + using wt = std::weak_ptr; + if (!parent_node.owner_before(wt{}) && !wt{} + .owner_before(parent_node)) // parent_node == nullptr + { + return type_name; + } + return parent_node.lock()->get_path() + "." + member_name; +} + +std::string NavigationNode::get_type( + std::map > map_nodes, + const std::string& full_path) +{ + if (full_path.find(".") == std::string::npos) + { + if (map_nodes.count(full_path) > 0) + { + return map_nodes[full_path]->type_name; + } + return full_path; + } + + std::string root = full_path.substr(0, full_path.find(".")); + + if (map_nodes.count(root) > 0) + { + return get_type(map_nodes[root], full_path.substr(full_path.find(".") + 1)); + } + + return ""; +} + +std::string NavigationNode::get_type( + std::shared_ptr root, + const std::string& full_path) +{ + if (full_path.empty() || full_path.find(".") == std::string::npos) + { + return root->type_name; + } + + std::string member = full_path.substr(0, full_path.find(".")); + + if (root->member_node.count(member) > 0) + { + std::string path = full_path.substr(full_path.find(".") + 1); + if (path.find(".") == std::string::npos) + { + return root->member_node[member]->type_name; + } + else + { + return get_type(root->member_node[member], path); + } + } + return ""; +} + +std::shared_ptr NavigationNode::fill_root_node( + std::shared_ptr root, + const ::xtypes::DynamicType& type, + const std::string& full_path) +{ + std::string type_name; + std::string path; + if (full_path.find(".") == std::string::npos) + { + type_name = full_path; + path = ""; + } + else + { + type_name = full_path.substr(0, full_path.find(".")); + path = full_path.substr(full_path.find(".") + 1); + } + + if (type_name != type.name()) + { + logger_ << utils::Logger::Level::ERROR + << "Attempting to create a root navigation node from a type with different name." + << std::endl; + + return root; + } + + if (!path.empty()) + { + std::string member; + + if (path.find(".") == std::string::npos) + { + member = path; + path = ""; + } + else + { + member = path.substr(0, path.find(".")); + path = path.substr(path.find(".") + 1); + } + + if (!type.is_aggregation_type()) + { + logger_ << utils::Logger::Level::ERROR + << "Attempting to create a root navigation node from a non-aggregated type." + << std::endl; + + return root; + } + + const ::xtypes::AggregationType& aggr_type = static_cast(type); + + if (!aggr_type.has_member(member)) + { + logger_ << utils::Logger::Level::ERROR + << "Type \"" << type_name << "\" doesn't have a member named \"" + << member << "\"." << std::endl; + + return root; + } + + std::shared_ptr member_node; + const ::xtypes::Member& type_member = aggr_type.member(member); + if (root->member_node.count(member) > 0) + { + member_node = root->member_node[member]; + } + else + { + member_node = std::make_shared(); + member_node->member_name = member; + member_node->type_name = type_member.type().name(); + member_node->parent_node = root; + root->member_node[member] = member_node; + } + + if (!path.empty()) + { + fill_member_node(member_node, type_member.type(), path); + } + } + + root->type_name = type.name(); + return root; +} + +void NavigationNode::fill_member_node( + std::shared_ptr node, + const ::xtypes::DynamicType& type, + const std::string& full_path) +{ + std::string member; + std::string path; + + if (!type.is_aggregation_type()) + { + logger_ << utils::Logger::Level::ERROR + << "Member " << node->member_name << " isn't an aggregated type." << std::endl; + + return; + } + + const ::xtypes::AggregationType& aggr_type = static_cast(type); + + if (full_path.find(".") == std::string::npos) + { + member = full_path; + path = ""; + } + else + { + member = full_path.substr(0, full_path.find(".")); + path = full_path.substr(full_path.find(".") + 1); + } + + if (!aggr_type.has_member(member)) + { + logger_ << utils::Logger::Level::ERROR + << "Member \"" << node->member_name << "\" of type \"" << node->type_name + << "\" doesn't have a member named \"" << member << "\"." << std::endl; + + return; + } + + std::shared_ptr member_node; + const ::xtypes::Member& type_member = aggr_type.member(member); + + if (node->member_node.count(member) > 0) + { + member_node = node->member_node[member]; + } + else + { + member_node = std::make_shared(); + member_node->member_name = member; + member_node->type_name = type_member.type().name(); + member_node->parent_node = node; + node->member_node[member] = member_node; + } + + if (!path.empty()) + { + fill_member_node(member_node, type_member.type(), path); + } +} + +std::shared_ptr NavigationNode::get_discriminator( + const std::map >& member_map, + const ::xtypes::DynamicData& data, + const std::vector& member_types) +{ + const std::string& type_name = data.type().name(); + + if (member_map.count(type_name) == 0) + { + auto result = std::make_shared(); + result->type_name = type_name; + return result; + } + + if (std::find(member_types.begin(), member_types.end(), type_name) != member_types.end()) + { + return member_map.at(type_name); + } + + return get_discriminator(member_map.at(type_name), data.ref(), member_types); +} + +std::shared_ptr NavigationNode::get_discriminator( + std::shared_ptr node, + ::xtypes::ReadableDynamicDataRef data, + const std::vector& member_types) +{ + const std::string& type_name = data.type().name(); + if (std::find(member_types.begin(), member_types.end(), type_name) != member_types.end()) + { + return node; + } + + if (data.type().is_aggregation_type()) + { + const ::xtypes::AggregationType& aggr = static_cast(data.type()); + std::shared_ptr result; + if (aggr.kind() == ::xtypes::TypeKind::UNION_TYPE) + { + std::string member = data.current_case().name(); + if (node->member_node.count(member) > 0) + { + result = + get_discriminator(node->member_node[member], data[member], member_types); + } + } + else + { + for (auto& n : node->member_node) + { + if (aggr.has_member(n.first)) + { + result = get_discriminator(n.second, data[n.first], member_types); + if (result) + { + break; + } + } + } + } + return result; + } + + return nullptr; +} + +// Static member initialization +utils::Logger Conversion::logger_("is::sh::ROS2 Dynamic::Conversion"); + +// This function patches the problem of dynamic types, which do not admit '/' in their type name. +std::string Conversion::convert_type_name( + const std::string& message_type) +{ + std::string type = message_type; + + //TODO(Ricardo) Remove when old dyntypes support namespaces. + size_t npos = type.rfind(":"); + if (std::string::npos != npos) + { + type = type.substr(npos + 1); + } + + for (size_t i = type.find('/'); i != std::string::npos; i = type.find('/', i)) + { + type.replace(i, 1, "__"); + } + + return type; +} + +const xtypes::DynamicType& Conversion::resolve_type( + const xtypes::DynamicType& type) +{ + if (type.kind() == ::xtypes::TypeKind::ALIAS_TYPE) + { + return static_cast(type).rget(); + } + return type; +} + +void Conversion::set_primitive_data( + ::xtypes::ReadableDynamicDataRef from, + DynamicData* to, + MemberId id) +{ + switch (resolve_type(from.type()).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + to->set_bool_value(from.value() ? true : false, id); + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + to->set_char8_value(from.value(), id); + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + to->set_char16_value(from.value(), id); + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + to->set_uint8_value(from.value(), id); + break; + case ::xtypes::TypeKind::INT_8_TYPE: + to->set_int8_value(from.value(), id); + break; + case ::xtypes::TypeKind::INT_16_TYPE: + to->set_int16_value(from.value(), id); + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + to->set_uint16_value(from.value(), id); + break; + case ::xtypes::TypeKind::INT_32_TYPE: + to->set_int32_value(from.value(), id); + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + to->set_uint32_value(from.value(), id); + break; + case ::xtypes::TypeKind::INT_64_TYPE: + to->set_int64_value(from.value(), id); + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + to->set_uint64_value(from.value(), id); + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + to->set_float32_value(from.value(), id); + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + to->set_float64_value(from.value(), id); + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + to->set_float128_value(from.value(), id); + break; + case ::xtypes::TypeKind::STRING_TYPE: + to->set_string_value(from.value(), id); + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + to->set_wstring_value(from.value(), id); + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + to->set_enum_value(from.value(), id); + break; + default: + { + logger_ << utils::Logger::Level::ERROR + << "Expected primitive data, but found '" + << from.type().name() << "'" << std::endl; + } + } +} + +void Conversion::set_array_data( + xtypes::ReadableDynamicDataRef from, + DynamicData* to, + const std::vector& indexes) +{ + const ::xtypes::ArrayType& type = static_cast(from.type()); + const ::xtypes::DynamicType& inner_type = resolve_type(type.content_type()); + DynamicDataFactory* factory = DynamicDataFactory::get_instance(); + MemberId id; + + for (uint32_t idx = 0; idx < from.size(); ++idx) + { + std::vector new_indexes = indexes; + new_indexes.push_back(idx); + switch (inner_type.kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + id = to->get_array_index(new_indexes); + to->set_bool_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + id = to->get_array_index(new_indexes); + to->set_char8_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + id = to->get_array_index(new_indexes); + to->set_char16_value(static_cast(from[idx].value()), id); + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + id = to->get_array_index(new_indexes); + to->set_uint8_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::INT_8_TYPE: + id = to->get_array_index(new_indexes); + to->set_int8_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::INT_16_TYPE: + id = to->get_array_index(new_indexes); + to->set_int16_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + id = to->get_array_index(new_indexes); + to->set_uint16_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::INT_32_TYPE: + id = to->get_array_index(new_indexes); + to->set_int32_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + id = to->get_array_index(new_indexes); + to->set_uint32_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::INT_64_TYPE: + id = to->get_array_index(new_indexes); + to->set_int64_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + id = to->get_array_index(new_indexes); + to->set_uint64_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + id = to->get_array_index(new_indexes); + to->set_float32_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + id = to->get_array_index(new_indexes); + to->set_float64_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + id = to->get_array_index(new_indexes); + to->set_float128_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::STRING_TYPE: + id = to->get_array_index(new_indexes); + to->set_string_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + id = to->get_array_index(new_indexes); + to->set_wstring_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + id = to->get_array_index(new_indexes); + to->set_enum_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::ARRAY_TYPE: + { + set_array_data(from[idx], to, new_indexes); + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + id = to->get_array_index(new_indexes); + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner sequence builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* seq_data = factory->create_data(builder_ptr->build()); + set_sequence_data(from[idx], seq_data); + to->set_complex_value(seq_data, id); + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + id = to->get_array_index(new_indexes); + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner map builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* seq_data = factory->create_data(builder_ptr->build()); + set_map_data(from[idx], seq_data); + to->set_complex_value(seq_data, id); + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + id = to->get_array_index(new_indexes); + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner struct builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* st_data = factory->create_data(builder_ptr->build()); + set_struct_data(from[idx], st_data); + to->set_complex_value(st_data, id); + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + id = to->get_array_index(new_indexes); + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner struct builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* st_data = factory->create_data(builder_ptr->build()); + set_union_data(from[idx], st_data); + to->set_complex_value(st_data, id); + break; + } + default: + { + logger_ << utils::Logger::Level::ERROR + << "Unexpected data type: '" << from.type().name() << "'" << std::endl; + } + } + } +} + +void Conversion::set_sequence_data( + ::xtypes::ReadableDynamicDataRef from, + DynamicData* to) +{ + const ::xtypes::SequenceType& type = static_cast(from.type()); + MemberId id; + DynamicDataFactory* factory = DynamicDataFactory::get_instance(); + to->clear_all_values(); + for (uint32_t idx = 0; idx < from.size(); ++idx) + { + to->insert_sequence_data(id); + switch (resolve_type(type.content_type()).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + to->set_bool_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + to->set_char8_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + to->set_char16_value(static_cast(from[idx].value()), id); + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + //to->set_uint8_value(from[idx].value(), id); + to->set_byte_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::INT_8_TYPE: + //to->set_int8_value(from[idx].value(), id); + to->set_byte_value(static_cast(from[idx].value()), id); + break; + case ::xtypes::TypeKind::INT_16_TYPE: + to->set_int16_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + to->set_uint16_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::INT_32_TYPE: + to->set_int32_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + to->set_uint32_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::INT_64_TYPE: + to->set_int64_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + to->set_uint64_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + to->set_float32_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + to->set_float64_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + to->set_float128_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::STRING_TYPE: + to->set_string_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + to->set_wstring_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + to->set_enum_value(from[idx].value(), id); + break; + case ::xtypes::TypeKind::ARRAY_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner array builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* array_data = factory->create_data(builder_ptr->build()); + set_array_data(from[idx], array_data, std::vector()); + to->set_complex_value(array_data, id); + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner sequence builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* seq_data = factory->create_data(builder_ptr->build()); + set_sequence_data(from[idx], seq_data); + to->set_complex_value(seq_data, id); + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner map builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* seq_data = factory->create_data(builder_ptr->build()); + set_map_data(from[idx], seq_data); + to->set_complex_value(seq_data, id); + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner struct builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* st_data = factory->create_data(builder_ptr->build()); + set_struct_data(from[idx], st_data); + to->set_complex_value(st_data, id); + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(from[idx].type()); // The inner union builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* st_data = factory->create_data(builder_ptr->build()); + set_union_data(from[idx], st_data); + to->set_complex_value(st_data, id); + break; + } + default: + { + logger_ << utils::Logger::Level::ERROR + << "Unexpected data type: '" << from.type().name() << "'" << std::endl; + } + } + } +} + +void Conversion::set_map_data( + ::xtypes::ReadableDynamicDataRef from, + DynamicData* to) +{ + const ::xtypes::MapType& type = static_cast(from.type()); + const ::xtypes::PairType& pair_type = static_cast(type.content_type()); + MemberId id_key; + MemberId id_value; + DynamicDataFactory* factory = DynamicDataFactory::get_instance(); + to->clear_all_values(); + + DynamicTypeBuilder_ptr key_builder = get_builder(pair_type.first()); + DynamicTypeBuilder_ptr value_builder = get_builder(pair_type.second()); + + for (::xtypes::ReadableDynamicDataRef pair : from) + { + // Convert key + DynamicData* key_data = factory->create_data(key_builder->build()); + ::xtypes::ReadableDynamicDataRef key = pair[0]; + MemberId id = MEMBER_ID_INVALID; + + switch (resolve_type(pair_type.first()).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + key_data->set_bool_value(key, id); + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + key_data->set_char8_value(key, id); + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + key_data->set_char16_value(key, id); + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + //key_data->set_uint8_value(from[key].value(), id); + key_data->set_byte_value(key, id); + break; + case ::xtypes::TypeKind::INT_8_TYPE: + //key_data->set_int8_value(from[key].value(), id); + key_data->set_byte_value(key, id); + break; + case ::xtypes::TypeKind::INT_16_TYPE: + key_data->set_int16_value(key, id); + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + key_data->set_uint16_value(key, id); + break; + case ::xtypes::TypeKind::INT_32_TYPE: + key_data->set_int32_value(key, id); + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + key_data->set_uint32_value(key, id); + break; + case ::xtypes::TypeKind::INT_64_TYPE: + key_data->set_int64_value(key, id); + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + key_data->set_uint64_value(key, id); + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + key_data->set_float32_value(key, id); + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + key_data->set_float64_value(key, id); + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + key_data->set_float128_value(key, id); + break; + case ::xtypes::TypeKind::STRING_TYPE: + key_data->set_string_value(key, id); + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + key_data->set_wstring_value(key, id); + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + key_data->set_enum_value(key.value(), id); + break; + case ::xtypes::TypeKind::ARRAY_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(key.type()); // The inner array builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* array_data = factory->create_data(builder_ptr->build()); + set_array_data(key, array_data, std::vector()); + key_data->set_complex_value(array_data, id); + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(key.type()); // The inner map builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* seq_data = factory->create_data(builder_ptr->build()); + set_map_data(key, seq_data); + key_data->set_complex_value(seq_data, id); + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(key.type()); // The inner sequence builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* seq_data = factory->create_data(builder_ptr->build()); + set_sequence_data(key, seq_data); + key_data->set_complex_value(seq_data, id); + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(key.type()); // The inner struct builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* st_data = factory->create_data(builder_ptr->build()); + set_struct_data(key, st_data); + key_data->set_complex_value(st_data, id); + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + DynamicTypeBuilder_ptr builder = get_builder(key.type()); // The inner struct builder + DynamicTypeBuilder* builder_ptr = static_cast(builder.get()); + DynamicData* st_data = factory->create_data(builder_ptr->build()); + set_union_data(key, st_data); + key_data->set_complex_value(st_data, id); + break; + } + default: + { + logger_ << utils::Logger::Level::ERROR + << "Unexpected data type: '" << from.type().name() << "'" << std::endl; + } + } + + // Convert data + id = MEMBER_ID_INVALID; + DynamicData* value_data = factory->create_data(value_builder->build()); + ::xtypes::ReadableDynamicDataRef value = pair[1]; + + switch (resolve_type(pair_type.second()).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + value_data->set_bool_value(value, id); + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + value_data->set_char8_value(value, id); + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + value_data->set_char16_value(value, id); + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + //value_data->set_uint8_value(value, id); + value_data->set_byte_value(value, id); + break; + case ::xtypes::TypeKind::INT_8_TYPE: + //value_data->set_int8_value(value, id); + value_data->set_byte_value(value, id); + break; + case ::xtypes::TypeKind::INT_16_TYPE: + value_data->set_int16_value(value, id); + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + value_data->set_uint16_value(value, id); + break; + case ::xtypes::TypeKind::INT_32_TYPE: + value_data->set_int32_value(value, id); + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + value_data->set_uint32_value(value, id); + break; + case ::xtypes::TypeKind::INT_64_TYPE: + value_data->set_int64_value(value, id); + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + value_data->set_uint64_value(value, id); + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + value_data->set_float32_value(value, id); + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + value_data->set_float64_value(value, id); + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + value_data->set_float128_value(value, id); + break; + case ::xtypes::TypeKind::STRING_TYPE: + value_data->set_string_value(value, id); + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + value_data->set_wstring_value(value, id); + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + value_data->set_enum_value(value.value(), id); + break; + case ::xtypes::TypeKind::ARRAY_TYPE: + { + set_array_data(value, value_data, std::vector()); + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + set_map_data(value, value_data); + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + set_sequence_data(value, value_data); + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + set_struct_data(value, value_data); + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + set_union_data(value, value_data); + break; + } + default: + { + logger_ << utils::Logger::Level::ERROR + << "Unexpected data type: '" << from.type().name() << "'" << std::endl; + } + } + + // Add key value + to->insert_map_data(key_data, value_data, id_key, id_value); + } +} + +bool Conversion::xtypes_to_fastdds( + const ::xtypes::DynamicData& input, + DynamicData* output) +{ + if (input.type().kind() == ::xtypes::TypeKind::STRUCTURE_TYPE) + { + return set_struct_data(input, output); + } + else if (input.type().kind() == ::xtypes::TypeKind::UNION_TYPE) + { + return set_union_data(input, output); + } + + logger_ << utils::Logger::Level::ERROR + << "Unsupported data to convert (expected Structure or Union)." << std::endl; + + return false; +} + +bool Conversion::set_struct_data( + ::xtypes::ReadableDynamicDataRef input, + DynamicData* output) +{ + std::stringstream ss; + + const ::xtypes::StructType& type = static_cast(input.type()); + + for (const ::xtypes::Member& member : type.members()) + { + MemberId id = output->get_member_id_by_name(member.name()); + switch (resolve_type(member.type()).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + case ::xtypes::TypeKind::CHAR_8_TYPE: + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + case ::xtypes::TypeKind::UINT_8_TYPE: + case ::xtypes::TypeKind::INT_8_TYPE: + case ::xtypes::TypeKind::INT_16_TYPE: + case ::xtypes::TypeKind::UINT_16_TYPE: + case ::xtypes::TypeKind::INT_32_TYPE: + case ::xtypes::TypeKind::UINT_32_TYPE: + case ::xtypes::TypeKind::INT_64_TYPE: + case ::xtypes::TypeKind::UINT_64_TYPE: + case ::xtypes::TypeKind::FLOAT_32_TYPE: + case ::xtypes::TypeKind::FLOAT_64_TYPE: + case ::xtypes::TypeKind::FLOAT_128_TYPE: + case ::xtypes::TypeKind::STRING_TYPE: + case ::xtypes::TypeKind::WSTRING_TYPE: + case ::xtypes::TypeKind::ENUMERATION_TYPE: + { + set_primitive_data(input[member.name()], output, id); + break; + } + case ::xtypes::TypeKind::ARRAY_TYPE: + { + DynamicData* array_data = output->loan_value(id); + set_array_data(input[member.name()], array_data, std::vector()); + output->return_loaned_value(array_data); + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + DynamicData* seq_data = output->loan_value(id); + set_sequence_data(input[member.name()], seq_data); + output->return_loaned_value(seq_data); + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + DynamicData* seq_data = output->loan_value(id); + set_map_data(input[member.name()], seq_data); + output->return_loaned_value(seq_data); + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + DynamicData* st_data = output->loan_value(id); + set_struct_data(input[member.name()], st_data); + output->return_loaned_value(st_data); + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + DynamicData* st_data = output->loan_value(id); + set_union_data(input[member.name()], st_data); + output->return_loaned_value(st_data); + break; + } + default: + logger_ << utils::Logger::Level::ERROR + << "Unsupported type: '" << member.type().name() << "'" << std::endl; + + } + } + return true; +} + +bool Conversion::set_union_data( + ::xtypes::ReadableDynamicDataRef input, + DynamicData* output) +{ + std::stringstream ss; + + // Discriminator + switch (resolve_type(input.d().type()).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + output->set_discriminator_value(input.d().value()); + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + output->set_discriminator_value(static_cast(input.d().value())); + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + output->set_discriminator_value(static_cast(input.d().value())); + break; + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + output->set_discriminator_value(static_cast(input.d().value())); + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + output->set_discriminator_value(input.d().value()); + break; + case ::xtypes::TypeKind::INT_8_TYPE: + output->set_discriminator_value(static_cast(input.d().value())); + break; + case ::xtypes::TypeKind::INT_16_TYPE: + output->set_discriminator_value(static_cast(input.d().value())); + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + output->set_discriminator_value(input.d().value()); + break; + case ::xtypes::TypeKind::INT_32_TYPE: + output->set_discriminator_value(static_cast(input.d().value())); + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + output->set_discriminator_value(input.d().value()); + break; + case ::xtypes::TypeKind::INT_64_TYPE: + output->set_discriminator_value(static_cast(input.d().value())); + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + output->set_discriminator_value(input.d().value()); + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + output->set_discriminator_value(input.d().value()); + break; + default: + break; + } + + // Active member + const ::xtypes::Member& member = input.current_case(); + MemberId id = output->get_member_id_by_name(member.name()); + switch (resolve_type(member.type()).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + case ::xtypes::TypeKind::CHAR_8_TYPE: + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + case ::xtypes::TypeKind::UINT_8_TYPE: + case ::xtypes::TypeKind::INT_8_TYPE: + case ::xtypes::TypeKind::INT_16_TYPE: + case ::xtypes::TypeKind::UINT_16_TYPE: + case ::xtypes::TypeKind::INT_32_TYPE: + case ::xtypes::TypeKind::UINT_32_TYPE: + case ::xtypes::TypeKind::INT_64_TYPE: + case ::xtypes::TypeKind::UINT_64_TYPE: + case ::xtypes::TypeKind::FLOAT_32_TYPE: + case ::xtypes::TypeKind::FLOAT_64_TYPE: + case ::xtypes::TypeKind::FLOAT_128_TYPE: + case ::xtypes::TypeKind::STRING_TYPE: + case ::xtypes::TypeKind::WSTRING_TYPE: + case ::xtypes::TypeKind::ENUMERATION_TYPE: + { + set_primitive_data(input[member.name()], output, id); + break; + } + case ::xtypes::TypeKind::ARRAY_TYPE: + { + DynamicData* array_data = output->loan_value(id); + set_array_data(input[member.name()], array_data, std::vector()); + output->return_loaned_value(array_data); + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + DynamicData* seq_data = output->loan_value(id); + set_sequence_data(input[member.name()], seq_data); + output->return_loaned_value(seq_data); + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + DynamicData* st_data = output->loan_value(id); + set_struct_data(input[member.name()], st_data); + output->return_loaned_value(st_data); + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + DynamicData* st_data = output->loan_value(id); + set_map_data(input[member.name()], st_data); + output->return_loaned_value(st_data); + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + DynamicData* st_data = output->loan_value(id); + set_union_data(input[member.name()], st_data); + output->return_loaned_value(st_data); + break; + } + default: + logger_ << utils::Logger::Level::ERROR + << "Unsupported type: '" << member.type().name() << "'" << std::endl; + } + return true; +} + +void Conversion::set_sequence_data( + const DynamicData* c_from, + ::xtypes::WritableDynamicDataRef to) +{ + const ::xtypes::SequenceType& type = static_cast(to.type()); + DynamicData* from = const_cast(c_from); + + for (uint32_t idx = 0; idx < c_from->get_item_count(); ++idx) + { + MemberId id = idx; + ResponseCode ret = ResponseCode::RETCODE_ERROR; + + switch (resolve_type(type.content_type()).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + { + bool value; + ret = from->get_bool_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + { + char value; + ret = from->get_char8_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + { + wchar_t value; + ret = from->get_char16_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + { + uint8_t value; + ret = from->get_uint8_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::INT_8_TYPE: + { + int8_t value; + ret = from->get_int8_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::INT_16_TYPE: + { + int16_t value; + ret = from->get_int16_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + { + uint16_t value; + ret = from->get_uint16_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::INT_32_TYPE: + { + int32_t value; + ret = from->get_int32_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + { + uint32_t value; + ret = from->get_uint32_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::INT_64_TYPE: + { + int64_t value; + ret = from->get_int64_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + { + uint64_t value; + ret = from->get_uint64_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + { + float value; + ret = from->get_float32_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + { + double value; + ret = from->get_float64_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + { + long double value; + ret = from->get_float128_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::STRING_TYPE: + { + std::string value; + ret = from->get_string_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + { + std::wstring value; + ret = from->get_wstring_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + { + uint32_t value; + ret = from->get_enum_value(value, id); + to.push(value); + } + break; + case ::xtypes::TypeKind::ARRAY_TYPE: + { + DynamicData* array = from->loan_value(id); + ::xtypes::DynamicData xtypes_array(type.content_type()); + set_array_data(array, xtypes_array.ref(), std::vector()); + from->return_loaned_value(array); + to.push(xtypes_array); + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + DynamicData* seq = from->loan_value(id); + ::xtypes::DynamicData xtypes_seq(type.content_type()); + set_sequence_data(seq, xtypes_seq.ref()); + from->return_loaned_value(seq); + to.push(xtypes_seq); + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + DynamicData* seq = from->loan_value(id); + ::xtypes::DynamicData xtypes_map(type.content_type()); + set_map_data(seq, xtypes_map.ref()); + from->return_loaned_value(seq); + to.push(xtypes_map); + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + DynamicData* st = from->loan_value(id); + ::xtypes::DynamicData xtypes_st(type.content_type()); + set_struct_data(st, xtypes_st.ref()); + from->return_loaned_value(st); + to.push(xtypes_st); + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + DynamicData* st = from->loan_value(id); + ::xtypes::DynamicData xtypes_union(type.content_type()); + set_union_data(st, xtypes_union.ref()); + from->return_loaned_value(st); + to.push(xtypes_union); + ret = ResponseCode::RETCODE_OK; + break; + } + default: + { + logger_ << utils::Logger::Level::ERROR + << "Unexpected data type: '" << to.type().name() << "'" << std::endl; + } + } + + if (ret != ResponseCode::RETCODE_OK) + { + logger_ << utils::Logger::Level::ERROR + << "Error parsing from dynamic type '" << to.type().name() << "'" << std::endl; + } + } +} + +void Conversion::set_map_data( + const DynamicData* c_from, + ::xtypes::WritableDynamicDataRef to) +{ + const ::xtypes::MapType& map_type = static_cast(to.type()); + const ::xtypes::PairType& pair_type = static_cast(map_type.content_type()); + const ::xtypes::DynamicType& key_type = pair_type.first(); + const ::xtypes::DynamicType& value_type = pair_type.second(); + DynamicData* from = const_cast(c_from); + + for (uint32_t idx = 0; idx < c_from->get_item_count(); ++idx) + { + MemberId key_id = idx * 2; + MemberId value_id = key_id + 1; + ResponseCode ret = ResponseCode::RETCODE_ERROR; + + ::xtypes::DynamicData key_data(key_type); + ::xtypes::DynamicData value_data(value_type); + + // Key + switch (resolve_type(key_type).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + { + bool value; + ret = from->get_bool_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + { + char value; + ret = from->get_char8_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + { + wchar_t value; + ret = from->get_char16_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + { + uint8_t value; + ret = from->get_uint8_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::INT_8_TYPE: + { + int8_t value; + ret = from->get_int8_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::INT_16_TYPE: + { + int16_t value; + ret = from->get_int16_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + { + uint16_t value; + ret = from->get_uint16_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::INT_32_TYPE: + { + int32_t value; + ret = from->get_int32_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + { + uint32_t value; + ret = from->get_uint32_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::INT_64_TYPE: + { + int64_t value; + ret = from->get_int64_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + { + uint64_t value; + ret = from->get_uint64_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + { + float value; + ret = from->get_float32_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + { + double value; + ret = from->get_float64_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + { + long double value; + ret = from->get_float128_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::STRING_TYPE: + { + std::string value; + ret = from->get_string_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + { + std::wstring value; + ret = from->get_wstring_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + { + uint32_t value; + ret = from->get_enum_value(value, key_id); + key_data = value; + } + break; + case ::xtypes::TypeKind::ARRAY_TYPE: + { + DynamicData* array = from->loan_value(key_id); + ::xtypes::DynamicData xtypes_array(key_type); + set_array_data(array, xtypes_array.ref(), std::vector()); + from->return_loaned_value(array); + key_data = xtypes_array; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + DynamicData* seq = from->loan_value(key_id); + ::xtypes::DynamicData xtypes_seq(key_type); + set_sequence_data(seq, xtypes_seq.ref()); + from->return_loaned_value(seq); + key_data = xtypes_seq; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + DynamicData* seq = from->loan_value(key_id); + ::xtypes::DynamicData xtypes_map(key_type); + set_map_data(seq, xtypes_map.ref()); + from->return_loaned_value(seq); + key_data = xtypes_map; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + DynamicData* st = from->loan_value(key_id); + ::xtypes::DynamicData xtypes_st(key_type); + set_struct_data(st, xtypes_st.ref()); + from->return_loaned_value(st); + key_data = xtypes_st; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + DynamicData* st = from->loan_value(key_id); + ::xtypes::DynamicData xtypes_union(key_type); + set_union_data(st, xtypes_union.ref()); + from->return_loaned_value(st); + key_data = xtypes_union; + ret = ResponseCode::RETCODE_OK; + break; + } + default: + { + logger_ << utils::Logger::Level::ERROR + << "Unexpected data type: '" << key_type.name() << "'" << std::endl; + } + } + + // Value + switch (resolve_type(value_type).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + { + bool value; + ret = from->get_bool_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + { + char value; + ret = from->get_char8_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + { + wchar_t value; + ret = from->get_char16_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + { + uint8_t value; + ret = from->get_uint8_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::INT_8_TYPE: + { + int8_t value; + ret = from->get_int8_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::INT_16_TYPE: + { + int16_t value; + ret = from->get_int16_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + { + uint16_t value; + ret = from->get_uint16_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::INT_32_TYPE: + { + int32_t value; + ret = from->get_int32_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + { + uint32_t value; + ret = from->get_uint32_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::INT_64_TYPE: + { + int64_t value; + ret = from->get_int64_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + { + uint64_t value; + ret = from->get_uint64_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + { + float value; + ret = from->get_float32_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + { + double value; + ret = from->get_float64_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + { + long double value; + ret = from->get_float128_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::STRING_TYPE: + { + std::string value; + ret = from->get_string_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + { + std::wstring value; + ret = from->get_wstring_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + { + uint32_t value; + ret = from->get_enum_value(value, value_id); + value_data = value; + } + break; + case ::xtypes::TypeKind::ARRAY_TYPE: + { + DynamicData* array = from->loan_value(value_id); + ::xtypes::DynamicData xtypes_array(value_type); + set_array_data(array, xtypes_array.ref(), std::vector()); + from->return_loaned_value(array); + value_data = xtypes_array; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + DynamicData* seq = from->loan_value(value_id); + ::xtypes::DynamicData xtypes_seq(value_type); + set_sequence_data(seq, xtypes_seq.ref()); + from->return_loaned_value(seq); + value_data = xtypes_seq; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + DynamicData* seq = from->loan_value(value_id); + ::xtypes::DynamicData xtypes_map(value_type); + set_map_data(seq, xtypes_map.ref()); + from->return_loaned_value(seq); + value_data = xtypes_map; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + DynamicData* st = from->loan_value(value_id); + ::xtypes::DynamicData xtypes_st(value_type); + set_struct_data(st, xtypes_st.ref()); + from->return_loaned_value(st); + value_data = xtypes_st; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + DynamicData* st = from->loan_value(value_id); + ::xtypes::DynamicData xtypes_union(value_type); + set_union_data(st, xtypes_union.ref()); + from->return_loaned_value(st); + value_data = xtypes_union; + ret = ResponseCode::RETCODE_OK; + break; + } + default: + { + logger_ << utils::Logger::Level::ERROR + << "Unexpected data type: '" << value_type.name() << "'" << std::endl; + } + } + + if (ret != ResponseCode::RETCODE_OK) + { + logger_ << utils::Logger::Level::ERROR + << "Error parsing from dynamic type '" << to.type().name() << "'" << std::endl; + } + + // Add entry + to[key_data] = value_data; + } +} + +void Conversion::set_array_data( + const DynamicData* c_from, + ::xtypes::WritableDynamicDataRef to, + const std::vector& indexes) +{ + const ::xtypes::ArrayType& type = static_cast(to.type()); + const ::xtypes::DynamicType& inner_type = type.content_type(); + DynamicData* from = const_cast(c_from); + MemberId id; + + for (uint32_t idx = 0; idx < type.dimension(); ++idx) + { + std::vector new_indexes = indexes; + new_indexes.push_back(idx); + ResponseCode ret = ResponseCode::RETCODE_ERROR; + switch (resolve_type(inner_type).kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + { + id = from->get_array_index(new_indexes); + bool value; + ret = from->get_bool_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + { + id = from->get_array_index(new_indexes); + char value; + ret = from->get_char8_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + { + id = from->get_array_index(new_indexes); + wchar_t value; + ret = from->get_char16_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::UINT_8_TYPE: + { + id = from->get_array_index(new_indexes); + uint8_t value; + ret = from->get_uint8_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::INT_8_TYPE: + { + id = from->get_array_index(new_indexes); + int8_t value; + ret = from->get_int8_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::INT_16_TYPE: + { + id = from->get_array_index(new_indexes); + int16_t value; + ret = from->get_int16_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::UINT_16_TYPE: + { + id = from->get_array_index(new_indexes); + uint16_t value; + ret = from->get_uint16_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::INT_32_TYPE: + { + id = from->get_array_index(new_indexes); + int32_t value; + ret = from->get_int32_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::UINT_32_TYPE: + { + id = from->get_array_index(new_indexes); + uint32_t value; + ret = from->get_uint32_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::INT_64_TYPE: + { + id = from->get_array_index(new_indexes); + int64_t value; + ret = from->get_int64_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::UINT_64_TYPE: + { + id = from->get_array_index(new_indexes); + uint64_t value; + ret = from->get_uint64_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::FLOAT_32_TYPE: + { + id = from->get_array_index(new_indexes); + float value; + ret = from->get_float32_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::FLOAT_64_TYPE: + { + id = from->get_array_index(new_indexes); + double value; + ret = from->get_float64_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::FLOAT_128_TYPE: + { + id = from->get_array_index(new_indexes); + long double value; + ret = from->get_float128_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::STRING_TYPE: + { + id = from->get_array_index(new_indexes); + std::string value; + ret = from->get_string_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::WSTRING_TYPE: + { + id = from->get_array_index(new_indexes); + std::wstring value; + ret = from->get_wstring_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::ENUMERATION_TYPE: + { + id = from->get_array_index(new_indexes); + uint32_t value; + ret = from->get_enum_value(value, id); + to[idx].value(value); + } + break; + case ::xtypes::TypeKind::ARRAY_TYPE: + { + ::xtypes::DynamicData xtypes_array(type.content_type()); + set_array_data(from, xtypes_array.ref(), new_indexes); + to[idx] = xtypes_array; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + id = from->get_array_index(new_indexes); + DynamicData* seq = from->loan_value(id); + ::xtypes::DynamicData xtypes_seq(type.content_type()); + set_sequence_data(seq, xtypes_seq.ref()); + from->return_loaned_value(seq); + to[idx] = xtypes_seq; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::MAP_TYPE: + { + id = from->get_array_index(new_indexes); + DynamicData* seq = from->loan_value(id); + ::xtypes::DynamicData xtypes_map(type.content_type()); + set_map_data(seq, xtypes_map.ref()); + from->return_loaned_value(seq); + to[idx] = xtypes_map; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + id = from->get_array_index(new_indexes); + DynamicData* st = from->loan_value(id); + ::xtypes::DynamicData xtypes_st(type.content_type()); + set_struct_data(st, xtypes_st.ref()); + from->return_loaned_value(st); + to[idx] = xtypes_st; + ret = ResponseCode::RETCODE_OK; + break; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + id = from->get_array_index(new_indexes); + DynamicData* st = from->loan_value(id); + ::xtypes::DynamicData xtypes_union(type.content_type()); + set_union_data(st, xtypes_union.ref()); + from->return_loaned_value(st); + to[idx] = xtypes_union; + ret = ResponseCode::RETCODE_OK; + break; + } + default: + { + logger_ << utils::Logger::Level::ERROR + << "Unexpected data type: '" << to.type().name() << "'" << std::endl; + } + } + + if (ret != ResponseCode::RETCODE_OK) + { + logger_ << utils::Logger::Level::ERROR + << "Error parsing from dynamic type '" << to.type().name() << "'" << std::endl; + } + } + +} + +// TODO: Can we receive a type without members as root? +bool Conversion::fastdds_to_xtypes( + const DynamicData* c_input, + ::xtypes::DynamicData& output) +{ + if (output.type().kind() == ::xtypes::TypeKind::STRUCTURE_TYPE) + { + return set_struct_data(c_input, output.ref()); + } + else if (output.type().kind() == ::xtypes::TypeKind::UNION_TYPE) + { + return set_union_data(c_input, output.ref()); + } + + logger_ << utils::Logger::Level::ERROR + << "Unsupported data to convert (expected Structure or Union)." << std::endl; + + return false; +} + +TypeKind Conversion::resolve_type( + const DynamicType_ptr type) +{ + TypeDescriptor* desc_type = type->get_descriptor(); + while (desc_type->get_kind() == types::TK_ALIAS) + { + desc_type = desc_type->get_base_type()->get_descriptor(); + } + return desc_type->get_kind(); +} + +bool Conversion::set_struct_data( + const DynamicData* c_input, + ::xtypes::WritableDynamicDataRef output) +{ + std::stringstream ss; + uint32_t id = 0; + uint32_t i = 0; + MemberDescriptor descriptor; + + // We promise to not modify it, but we need it non-const, so we can call loan_value freely. + DynamicData* input = const_cast(c_input); + + while (id != MEMBER_ID_INVALID) + { + id = input->get_member_id_at_index(i); + ResponseCode ret = ResponseCode::RETCODE_ERROR; + + if (id != MEMBER_ID_INVALID) + { + ret = input->get_descriptor(descriptor, id); + + if (ret == ResponseCode::RETCODE_OK) + { + switch (resolve_type(descriptor.get_type())) + { + case types::TK_BOOLEAN: + { + bool value; + ret = input->get_bool_value(value, id); + output[descriptor.get_name()].value(value ? true : false); + break; + } + case types::TK_BYTE: + { + uint8_t value; + ret = input->get_byte_value(value, id); + switch (output[descriptor.get_name()].type().kind()) + { + case ::xtypes::TypeKind::UINT_8_TYPE: + output[descriptor.get_name()].value(value); + break; + case ::xtypes::TypeKind::CHAR_8_TYPE: + output[descriptor.get_name()].value(static_cast(value)); + break; + case ::xtypes::TypeKind::INT_8_TYPE: + output[descriptor.get_name()].value(static_cast(value)); + break; + default: + logger_ << utils::Logger::Level::ERROR + << "Error parsing from dynamic type '" + << input->get_name() << "'" << std::endl; + } + break; + } + case types::TK_INT16: + { + int16_t value; + ret = input->get_int16_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_INT32: + { + int32_t value; + ret = input->get_int32_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_INT64: + { + int64_t value; + ret = input->get_int64_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_UINT16: + { + uint16_t value; + ret = input->get_uint16_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_UINT32: + { + uint32_t value; + ret = input->get_uint32_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_UINT64: + { + uint64_t value; + ret = input->get_uint64_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_FLOAT32: + { + float value; + ret = input->get_float32_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_FLOAT64: + { + double value; + ret = input->get_float64_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_FLOAT128: + { + long double value; + ret = input->get_float128_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_CHAR8: + { + char value; + ret = input->get_char8_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_CHAR16: + { + wchar_t value; + ret = input->get_char16_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_STRING8: + { + std::string value; + ret = input->get_string_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_STRING16: + { + std::wstring value; + ret = input->get_wstring_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_ENUM: + { + uint32_t value; + ret = input->get_enum_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_ARRAY: + { + DynamicData* array = input->loan_value(id); + set_array_data(array, output[descriptor.get_name()], std::vector()); + input->return_loaned_value(array); + break; + } + case types::TK_SEQUENCE: + { + DynamicData* seq = input->loan_value(id); + set_sequence_data(seq, output[descriptor.get_name()]); + input->return_loaned_value(seq); + break; + } + case types::TK_MAP: + { + DynamicData* seq = input->loan_value(id); + set_map_data(seq, output[descriptor.get_name()]); + input->return_loaned_value(seq); + break; + } + case types::TK_STRUCTURE: + { + DynamicData* nested_msg_dds = input->loan_value(id); + + if (nested_msg_dds != nullptr) + { + if (set_struct_data(nested_msg_dds, output[descriptor.get_name()])) + { + ret = ResponseCode::RETCODE_OK; + } + input->return_loaned_value(nested_msg_dds); + } + break; + } + case types::TK_UNION: + { + DynamicData* nested_msg_dds = input->loan_value(id); + + if (nested_msg_dds != nullptr) + { + if (set_union_data(nested_msg_dds, output[descriptor.get_name()])) + { + ret = ResponseCode::RETCODE_OK; + } + input->return_loaned_value(nested_msg_dds); + } + break; + } + default: + { + ret = ResponseCode::RETCODE_ERROR; + break; + } + } + + i++; + } + + if (ret != ResponseCode::RETCODE_OK) + { + logger_ << utils::Logger::Level::ERROR + << "Error parsing from dynamic type '" + << input->get_name() << "'" << std::endl; + } + } + } + + return true; +} + +bool Conversion::set_union_data( + const DynamicData* c_input, + ::xtypes::WritableDynamicDataRef output) +{ + std::stringstream ss; + MemberDescriptor descriptor; + + // We promise to not modify it, but we need it non-const, so we can call loan_value freely. + DynamicData* input = const_cast(c_input); + + // Discriminator is set automatically when the operator[] is used. + + // Active member + UnionDynamicData* u_input = static_cast(input); + MemberId id = u_input->get_union_id(); + ResponseCode ret = ResponseCode::RETCODE_ERROR; + + ret = input->get_descriptor(descriptor, id); + + if (ret == ResponseCode::RETCODE_OK) + { + switch (resolve_type(descriptor.get_type())) + { + case types::TK_BOOLEAN: + { + bool value; + ret = input->get_bool_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_BYTE: + { + uint8_t value; + ret = input->get_byte_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_INT16: + { + int16_t value; + ret = input->get_int16_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_INT32: + { + int32_t value; + ret = input->get_int32_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_INT64: + { + int64_t value; + ret = input->get_int64_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_UINT16: + { + uint16_t value; + ret = input->get_uint16_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_UINT32: + { + uint32_t value; + ret = input->get_uint32_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_UINT64: + { + uint64_t value; + ret = input->get_uint64_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_FLOAT32: + { + float value; + ret = input->get_float32_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_FLOAT64: + { + double value; + ret = input->get_float64_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_FLOAT128: + { + long double value; + ret = input->get_float128_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_CHAR8: + { + char value; + ret = input->get_char8_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_CHAR16: + { + wchar_t value; + ret = input->get_char16_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_STRING8: + { + std::string value; + ret = input->get_string_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_STRING16: + { + std::wstring value; + ret = input->get_wstring_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_ENUM: + { + uint32_t value; + ret = input->get_enum_value(value, id); + output[descriptor.get_name()].value(value); + break; + } + case types::TK_ARRAY: + { + DynamicData* array = input->loan_value(id); + set_array_data(array, output[descriptor.get_name()], std::vector()); + input->return_loaned_value(array); + break; + } + case types::TK_SEQUENCE: + { + DynamicData* seq = input->loan_value(id); + set_sequence_data(seq, output[descriptor.get_name()]); + input->return_loaned_value(seq); + break; + } + case types::TK_MAP: + { + DynamicData* seq = input->loan_value(id); + set_map_data(seq, output[descriptor.get_name()]); + input->return_loaned_value(seq); + break; + } + case types::TK_STRUCTURE: + { + DynamicData* nested_msg_dds = input->loan_value(id); + + if (nested_msg_dds != nullptr) + { + if (set_struct_data(nested_msg_dds, output[descriptor.get_name()])) + { + ret = ResponseCode::RETCODE_OK; + } + input->return_loaned_value(nested_msg_dds); + } + break; + } + case types::TK_UNION: + { + DynamicData* nested_msg_dds = input->loan_value(id); + + if (nested_msg_dds != nullptr) + { + if (set_union_data(nested_msg_dds, output[descriptor.get_name()])) + { + ret = ResponseCode::RETCODE_OK; + } + input->return_loaned_value(nested_msg_dds); + } + break; + } + default: + { + ret = ResponseCode::RETCODE_ERROR; + break; + } + } + } + + if (ret != ResponseCode::RETCODE_OK) + { + logger_ << utils::Logger::Level::ERROR + << "Error parsing from dynamic type '" << input->get_name() << "'" << std::endl; + } + + return true; +} + +::xtypes::DynamicData Conversion::dynamic_data( + const std::string& type_name) +{ + auto it = types_.find(type_name); + + if (it == types_.end()) + { + logger_ << utils::Logger::Level::ERROR + << "Error getting data from dynamic type '" + << type_name << "' (not registered)" << std::endl; + } + + return ::xtypes::DynamicData(*it->second.get()); +} + +DynamicTypeBuilder* Conversion::create_builder( + const ::xtypes::DynamicType& type, + const std::string& type_name) +{ + if (builders_.count(type_name) > 0) + { + return static_cast(builders_[type_name].get()); + } + + DynamicTypeBuilder_ptr builder = get_builder(type); + if (builder == nullptr) + { + return nullptr; + } + DynamicTypeBuilder* result = static_cast(builder.get()); + result->set_name(convert_type_name(type_name)); + builders_.emplace(type_name, std::move(builder)); + return result; +} + +DynamicTypeBuilder_ptr Conversion::get_builder( + const ::xtypes::DynamicType& type) +{ + DynamicTypeBuilderFactory* factory = DynamicTypeBuilderFactory::get_instance(); + switch (type.kind()) + { + case ::xtypes::TypeKind::BOOLEAN_TYPE: + { + return factory->create_bool_builder(); + } + case ::xtypes::TypeKind::INT_8_TYPE: + case ::xtypes::TypeKind::UINT_8_TYPE: + { + return factory->create_byte_builder(); + } + case ::xtypes::TypeKind::INT_16_TYPE: + { + return factory->create_int16_builder(); + } + case ::xtypes::TypeKind::UINT_16_TYPE: + { + return factory->create_uint16_builder(); + } + case ::xtypes::TypeKind::INT_32_TYPE: + { + return factory->create_int32_builder(); + } + case ::xtypes::TypeKind::UINT_32_TYPE: + { + return factory->create_uint32_builder(); + } + case ::xtypes::TypeKind::INT_64_TYPE: + { + return factory->create_int64_builder(); + } + case ::xtypes::TypeKind::UINT_64_TYPE: + { + return factory->create_uint64_builder(); + } + case ::xtypes::TypeKind::FLOAT_32_TYPE: + { + return factory->create_float32_builder(); + } + case ::xtypes::TypeKind::FLOAT_64_TYPE: + { + return factory->create_float64_builder(); + } + case ::xtypes::TypeKind::FLOAT_128_TYPE: + { + return factory->create_float128_builder(); + } + case ::xtypes::TypeKind::CHAR_8_TYPE: + { + return factory->create_char8_builder(); + } + case ::xtypes::TypeKind::CHAR_16_TYPE: + case ::xtypes::TypeKind::WIDE_CHAR_TYPE: + { + return factory->create_char16_builder(); + } + case ::xtypes::TypeKind::ENUMERATION_TYPE: + { + DynamicTypeBuilder* builder = factory->create_enum_builder(); + builder->set_name(convert_type_name(type.name())); + const ::xtypes::EnumerationType& enum_type = + static_cast&>(type); + const std::map& enumerators = enum_type.enumerators(); + for (auto pair : enumerators) + { + builder->add_empty_member(pair.second, pair.first); + } + return builder; + } + case ::xtypes::TypeKind::BITSET_TYPE: + { + // TODO + break; + } + case ::xtypes::TypeKind::ALIAS_TYPE: + { + /* Real alias... doesn't returns a builder, but a type + DynamicTypeBuilder_ptr content = get_builder(static_cast(type).get()); + DynamicTypeBuilder* ptr = content.get(); + return factory->create_alias_type(ptr, type.name()); + */ + return get_builder(static_cast(type).rget()); // Resolve alias + } + /* + case ::xtypes::TypeKind::BITMASK_TYPE: + { + // TODO + } + */ + case ::xtypes::TypeKind::ARRAY_TYPE: + { + const ::xtypes::ArrayType& c_type = static_cast(type); + std::pair, DynamicTypeBuilder_ptr> pair; + get_array_specs(c_type, pair); + DynamicTypeBuilder* builder = static_cast(pair.second.get()); + DynamicTypeBuilder_ptr result = factory->create_array_builder(builder, pair.first); + return result; + } + case ::xtypes::TypeKind::SEQUENCE_TYPE: + { + const ::xtypes::SequenceType& c_type = static_cast(type); + DynamicTypeBuilder_ptr content = get_builder(c_type.content_type()); + DynamicTypeBuilder* builder = static_cast(content.get()); + DynamicTypeBuilder_ptr result = factory->create_sequence_builder(builder, c_type.bounds()); + return result; + } + case ::xtypes::TypeKind::STRING_TYPE: + { + const ::xtypes::StringType& c_type = static_cast(type); + return factory->create_string_builder(c_type.bounds()); + } + case ::xtypes::TypeKind::WSTRING_TYPE: + { + const ::xtypes::WStringType& c_type = static_cast(type); + return factory->create_wstring_builder(c_type.bounds()); + } + case ::xtypes::TypeKind::MAP_TYPE: + { + const ::xtypes::MapType& c_type = static_cast(type); + const ::xtypes::PairType& content_type = static_cast(c_type.content_type()); + DynamicTypeBuilder_ptr key = get_builder(content_type.first()); + DynamicTypeBuilder_ptr value = get_builder(content_type.second()); + DynamicTypeBuilder_ptr result = factory->create_map_builder(key.get(), value.get(), c_type.bounds()); + return result; + } + case ::xtypes::TypeKind::UNION_TYPE: + { + const ::xtypes::UnionType& c_type = static_cast(type); + DynamicTypeBuilder_ptr disc_type = get_builder(c_type.discriminator()); + DynamicTypeBuilder_ptr result = factory->create_union_builder(disc_type.get()); + + MemberId idx = 0; + for (const std::string& member_name : c_type.get_case_members()) + { + bool is_default = c_type.is_default(member_name); + const ::xtypes::Member& member = c_type.member(member_name); + std::vector labels_original = c_type.get_labels(member_name); + std::vector labels; + for (int64_t label : labels_original) + { + labels.push_back(static_cast(label)); + } + DynamicTypeBuilder_ptr member_builder = get_builder(member.type()); + result->add_member(idx++, member_name, member_builder.get(), "", labels, is_default); + } + return result; + } + case ::xtypes::TypeKind::STRUCTURE_TYPE: + { + DynamicTypeBuilder_ptr result = factory->create_struct_builder(); + const ::xtypes::StructType& from = static_cast(type); + + for (size_t idx = 0; idx < from.members().size(); ++idx) + { + const ::xtypes::Member& member = from.member(idx); + DynamicTypeBuilder_ptr member_builder = get_builder(member.type()); + DynamicTypeBuilder* builder = static_cast(member_builder.get()); + DynamicTypeBuilder* result_ptr = static_cast(result.get()); + result_ptr->add_member(static_cast(idx), member.name(), builder); + } + return result; + } + default: + break; + } + return nullptr; +} + +void Conversion::get_array_specs( + const ::xtypes::ArrayType& array, + std::pair, DynamicTypeBuilder_ptr>& result) +{ + result.first.push_back(array.dimension()); + if (array.content_type().kind() == ::xtypes::TypeKind::ARRAY_TYPE) + { + get_array_specs(static_cast(array.content_type()), result); + } + else + { + result.second = get_builder(array.content_type()); + } +} + +const xtypes::DynamicType& Conversion::resolve_discriminator_type( + const ::xtypes::DynamicType& service_type, + const std::string& discriminator) +{ + if (discriminator.find(".") == std::string::npos) + { + return service_type; + } + + std::string path = discriminator; + const xtypes::DynamicType* type_ptr; + std::string type = path.substr(0, path.find(".")); + std::string member; + type_ptr = &service_type; + while (path.find(".") != std::string::npos) + { + path = path.substr(path.find(".") + 1); + member = path.substr(0, path.find(".")); + if (type_ptr->is_aggregation_type()) + { + const xtypes::AggregationType& aggregation = static_cast(*type_ptr); + type_ptr = &aggregation.member(member).type(); + } + } + + return *type_ptr; +} + +::xtypes::WritableDynamicDataRef Conversion::access_member_data( + ::xtypes::WritableDynamicDataRef membered_data, + const std::string& path) +{ + std::vector nodes; + std::string token; + std::istringstream iss(path); + + // Split the path + while (std::getline(iss, token, '.')) + { + nodes.push_back(token); + } + + // Navigate + return access_member_data(membered_data, nodes, 1); +} + +::xtypes::WritableDynamicDataRef Conversion::access_member_data( + ::xtypes::WritableDynamicDataRef membered_data, + const std::vector& tokens, + size_t index) +{ + if (tokens.empty() || index == tokens.size()) + { + return membered_data; + } + else + { + return access_member_data(membered_data[tokens[index]], tokens, index + 1); + } +} + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima diff --git a/ros2/dynamic/src/Participant.cpp b/ros2/dynamic/src/Participant.cpp new file mode 100644 index 0000000..186de5a --- /dev/null +++ b/ros2/dynamic/src/Participant.cpp @@ -0,0 +1,294 @@ +/* + * Copyright 2019 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 + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +Participant::Participant() + : dds_participant_(nullptr) + , logger_("is::sh::ROS2 Dynamic::Participant") +{ + build_participant(); +} + +Participant::Participant( + const YAML::Node& config) + : dds_participant_(nullptr) + , logger_("is::sh::ROS2 Dynamic::Participant") +{ + ::fastdds::dds::DomainId_t domain_id = 0; + std::string node_name = "default_IS-ROS2-Dynamic-SH_participant"; + + if (config["domain"]) + { + domain_id = config["domain"].as(); + } + if (config["node_name"]) + { + node_name = config["node_name"].as(); + } + + build_participant(domain_id, node_name); +} + +Participant::~Participant() +{ + if (!dds_participant_->has_active_entities()) + { + dds_participant_->set_listener(nullptr); + + if (fastrtps::types::ReturnCode_t::RETCODE_OK != + ::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant(dds_participant_)) + { + logger_ << utils::Logger::Level::ERROR + << "Cannot delete ROS2 Dynamic node yet: it has active entities" << std::endl; + } + } +} + +void Participant::build_participant( + const ::fastdds::dds::DomainId_t& domain_id, + const std::string& participant_name) +{ + ::fastdds::dds::DomainParticipantQos participant_qos = ::fastdds::dds::PARTICIPANT_QOS_DEFAULT; + participant_qos.name(participant_name); + + // By default use UDPv4 due to communication failures between dockers sharing the network with the host + // When it is solved in Fast-DDS delete the following lines and use the default builtin transport. + participant_qos.transport().use_builtin_transports = false; + auto udp_transport = std::make_shared<::fastdds::rtps::UDPv4TransportDescriptor>(); + participant_qos.transport().user_transports.push_back(udp_transport); + + dds_participant_ = ::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( + domain_id, participant_qos); + + if (dds_participant_) + { + logger_ << utils::Logger::Level::INFO + << "Created ROS 2 Dynamic node '" << participant_qos.name() + << "' with default QoS attributes and Domain ID: " + << domain_id << std::endl; + } + else + { + std::ostringstream err; + err << "Error while creating ROS2 Dynamic node '" << participant_qos.name() + << "' with default QoS attributes and Domain ID: " << domain_id; + + throw ROS2MiddlewareException(logger_, err.str()); + } +} + +::fastdds::dds::DomainParticipant* Participant::get_dds_participant() const +{ + return dds_participant_; +} + +void Participant::register_dynamic_type( + const std::string& topic_name, + const std::string& type_name, + fastrtps::types::DynamicTypeBuilder* builder) +{ + auto topic_to_type_it = topic_to_type_.find(topic_name); + if (topic_to_type_it != topic_to_type_.end()) + { + return; // Already registered. + } + + auto types_it = types_.find(type_name); + if (types_.end() != types_it) + { + // Type known, add the entry in the map topic->type + topic_to_type_.emplace(topic_name, type_name); + + logger_ << utils::Logger::Level::DEBUG + << "Adding type '" << type_name << "' to topic '" + << topic_name << "'" << std::endl; + + return; + } + + fastrtps::types::DynamicType_ptr dtptr = builder->build(); + + if (dtptr != nullptr) + { + auto pair = types_.emplace(type_name, fastrtps::types::DynamicPubSubType(dtptr)); + fastrtps::types::DynamicPubSubType& dynamic_type_support = pair.first->second; + + topic_to_type_.emplace(topic_name, type_name); + + // Check if already registered + ::fastdds::dds::TypeSupport p_type = dds_participant_->find_type(type_name); + + if (nullptr == p_type) + { + dynamic_type_support.setName(type_name.c_str()); + + /** + * The following lines are added here so that a bug with UnionType in + * Fast DDS Dynamic Types is bypassed. This is a workaround and SHOULD + * be removed once this bug is solved. + * Until that moment, the Fast DDS SystemHandle will not be compatible with + * Fast DDS Dynamic Type Discovery mechanism. + * + * More information here: https://eprosima.easyredmine.com/issues/11349 + */ + // WORKAROUND START + dynamic_type_support.auto_fill_type_information(false); + dynamic_type_support.auto_fill_type_object(false); + // WORKAROUND END + + // Register it within the DomainParticipant + if (pair.second && !dds_participant_->register_type(dynamic_type_support)) + { + std::ostringstream err; + err << "Dynamic type '" << type_name << "' registration failed"; + + throw ROS2MiddlewareException(logger_, err.str()); + } + } + + if (pair.second) + { + logger_ << utils::Logger::Level::DEBUG + << "Registered type '" << type_name << "' in topic '" + << topic_name << "'" << std::endl; + + Conversion::register_type(topic_name, &dynamic_type_support); + } + else + { + logger_ << utils::Logger::Level::WARN + << "Failed registering type '" << type_name << "' in topic '" + << topic_name << "'" << std::endl; + } + } + else + { + std::ostringstream err; + err << "Dynamic type '" << type_name << "' for topic '" + << topic_name << "' was not correctly built"; + + throw ROS2MiddlewareException(logger_, err.str()); + } +} + +fastrtps::types::DynamicData* Participant::create_dynamic_data( + const std::string& topic_name) const +{ + auto topic_to_type_it = topic_to_type_.find(topic_name); + if (topic_to_type_.end() == topic_to_type_it) + { + std::ostringstream err; + err << "Creating dynamic data for topic '" << topic_name + << "' failed because the topic was not registered"; + + throw ROS2MiddlewareException(logger_, err.str()); + } + + auto types_it = types_.find(topic_to_type_it->second); + if (types_.end() == types_it) + { + std::ostringstream err; + err << "Creating dynamic data: dynamic type '" << types_it->first << "' not defined"; + + throw ROS2MiddlewareException(logger_, err.str()); + } + + const fastrtps::types::DynamicType_ptr& dynamic_type_ = types_it->second.GetDynamicType(); + return fastrtps::types::DynamicDataFactory::get_instance()->create_data(dynamic_type_); +} + +void Participant::delete_dynamic_data( + fastrtps::types::DynamicData* data) const +{ + DynamicDataFactory::get_instance()->delete_data(data); +} + +const fastrtps::types::DynamicType* Participant::get_dynamic_type( + const std::string& name) const +{ + auto it = types_.find(name); + if (it == types_.end()) + { + return nullptr; + } + + return static_cast(it->second.GetDynamicType().get()); +} + +const std::string& Participant::get_topic_type( + const std::string& topic) const +{ + return topic_to_type_.at(topic); +} + +void Participant::associate_topic_to_dds_entity( + ::fastdds::dds::Topic* topic, + ::fastdds::dds::DomainEntity* entity) +{ + std::unique_lock lock(topic_to_entities_mtx_); + + if (topic_to_entities_.end() == topic_to_entities_.find(topic)) + { + std::set<::fastdds::dds::DomainEntity*> entities; + entities.emplace(entity); + + topic_to_entities_[topic] = std::move(entities); + } + else + { + topic_to_entities_.at(topic).emplace(entity); + } +} + +bool Participant::dissociate_topic_from_dds_entity( + ::fastdds::dds::Topic* topic, + ::fastdds::dds::DomainEntity* entity) +{ + // std::unique_lock lock(topic_to_entities_mtx_); + + if (1 == topic_to_entities_.at(topic).size()) + { + // Only one entity remains in the map + topic_to_entities_.erase(topic); + return true; + } + else // Size should not be 0 at this point + { + topic_to_entities_.at(topic).erase(entity); + return false; + } +} + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima diff --git a/ros2/dynamic/src/Publisher.cpp b/ros2/dynamic/src/Publisher.cpp new file mode 100644 index 0000000..3611d65 --- /dev/null +++ b/ros2/dynamic/src/Publisher.cpp @@ -0,0 +1,209 @@ +/* + * Copyright 2019 - present 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 + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +Publisher::Publisher( + Participant* participant, + const std::string& topic_name, + const xtypes::DynamicType& message_type, + const YAML::Node& config) + : participant_(participant) + , topic_name_(topic_name) + , logger_("is::sh::ROS2 Dynamic::Publisher") +{ + + // Adds the type name mangling + std::string type_name = message_type.name(); + std::size_t found = type_name.find_last_of("::"); + + if (found == std::string::npos) + { + logger_ << utils::Logger::Level::ERROR + << "The type must follow the ROS2 naming convention" << std::endl; + return; + } + else + { + type_name.insert(found + 1, "dds_::"); + type_name.append("_"); + } + + fastrtps::types::DynamicTypeBuilder* builder = Conversion::create_builder(message_type, type_name); + + if (builder != nullptr) + { + participant->register_dynamic_type(topic_name, type_name, builder); + } + else + + { + throw ROS2MiddlewareException( + logger_, "Cannot create builder for type " + type_name); + } + + dynamic_data_ = participant->create_dynamic_data(topic_name); + + // Retrieve DDS participant + ::fastdds::dds::DomainParticipant* dds_participant = participant->get_dds_participant(); + if (!dds_participant) + { + throw ROS2MiddlewareException( + logger_, "Trying to create a publisher without a DDS participant!"); + } + + // Create DDS publisher with default publisher QoS + dds_publisher_ = dds_participant->create_publisher(::fastdds::dds::PUBLISHER_QOS_DEFAULT); + if (dds_publisher_) + { + logger_ << utils::Logger::Level::DEBUG + << "Created ROS2 Dynamic publisher for topic '" << topic_name << "'" << std::endl; + } + else + { + std::ostringstream err; + err << "ROS2 Dynamic publisher for topic '" << topic_name << "' was not created"; + + throw ROS2MiddlewareException(logger_, err.str()); + } + + // Create DDS topic + auto topic_description = dds_participant->lookup_topicdescription(topic_name); + if (!topic_description) + { + dds_topic_ = dds_participant->create_topic( + topic_name, type_name, ::fastdds::dds::TOPIC_QOS_DEFAULT); + if (dds_topic_) + { + logger_ << utils::Logger::Level::DEBUG + << "Created ROS2 Dynamic topic '" << topic_name << "' with type '" + << type_name << "'" << std::endl; + } + else + { + std::ostringstream err; + err << "ROS2 Dynamic topic '" << topic_name << "' with type '" + << type_name << "' was not created"; + + throw ROS2MiddlewareException(logger_, err.str()); + } + } + else + { + dds_topic_ = static_cast<::fastdds::dds::Topic*>(topic_description); + } + + // Create DDS datawriter + ::fastdds::dds::DataWriterQos datawriter_qos = ::fastdds::dds::DATAWRITER_QOS_DEFAULT; + if (config["service_instance_name"]) + { + fastrtps::rtps::Property instance_property; + instance_property.name("dds.rpc.service_instance_name"); + instance_property.value(config["service_instance_name"].as()); + datawriter_qos.properties().properties().emplace_back(std::move(instance_property)); + } + + dds_datawriter_ = dds_publisher_->create_datawriter(dds_topic_, datawriter_qos, this); + if (dds_datawriter_) + { + logger_ << utils::Logger::Level::DEBUG + << "Created ROS2 Dynamic datawriter for topic '" << topic_name << "'" << std::endl; + participant_->associate_topic_to_dds_entity(dds_topic_, dds_datawriter_); + } + else + { + std::ostringstream err; + err << "ROS2 Dynamic datawriter for topic '" << topic_name << "' was not created"; + + throw ROS2MiddlewareException(logger_, err.str()); + } +} + +Publisher::~Publisher() +{ + std::unique_lock lock(data_mtx_); + participant_->delete_dynamic_data(dynamic_data_); + + bool delete_topic = participant_->dissociate_topic_from_dds_entity(dds_topic_, dds_datawriter_); + + dds_datawriter_->set_listener(nullptr); + dds_publisher_->delete_datawriter(dds_datawriter_); + participant_->get_dds_participant()->delete_publisher(dds_publisher_); + + if (delete_topic) + { + participant_->get_dds_participant()->delete_topic(dds_topic_); + } +} + +bool Publisher::publish( + const ::xtypes::DynamicData& message) +{ + std::unique_lock lock(data_mtx_); + + logger_ << utils::Logger::Level::INFO + << "Sending message from Integration Service to DDS for topic '" << topic_name_ << "': " + << "[[ " << message << " ]]" << std::endl; + + bool success = Conversion::xtypes_to_fastdds(message, dynamic_data_); + if (success) + { + success = dds_datawriter_->write(static_cast(dynamic_data_)); + } + else + { + logger_ << utils::Logger::Level::ERROR + << "Failed to convert message from Integration Service to DDS for topic '" + << topic_name_ << "': [[ " << message << " ]]" << std::endl; + } + + return success; +} + +void Publisher::on_publication_matched( + ::fastdds::dds::DataWriter* /*writer*/, + const ::fastdds::dds::PublicationMatchedStatus& info) +{ + if (1 == info.current_count_change) + { + logger_ << utils::Logger::Level::INFO + << "Publisher for topic '" << topic_name_ << "' matched" << std::endl; + } + else if (-1 == info.current_count_change) + { + logger_ << utils::Logger::Level::INFO + << "Publisher for topic '" << topic_name_ << "' unmatched" << std::endl; + } +} + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima diff --git a/ros2/dynamic/src/Subscriber.cpp b/ros2/dynamic/src/Subscriber.cpp new file mode 100644 index 0000000..39df884 --- /dev/null +++ b/ros2/dynamic/src/Subscriber.cpp @@ -0,0 +1,333 @@ +/* + * Copyright 2019 - present 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 +#if FASTRTPS_VERSION_MINOR >= 2 +#include +#endif // if FASTRTPS_VERSION_MINOR >= 2 + +#include +#include + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +Subscriber::Subscriber( + Participant* participant, + const std::string& topic_name, + const xtypes::DynamicType& message_type, + TopicSubscriberSystem::SubscriptionCallback* is_callback) + : participant_(participant) + , dds_subscriber_(nullptr) + , dynamic_data_(nullptr) + , topic_name_(topic_name) + , message_type_(message_type) + , is_callback_(is_callback) + , reception_threads_() + , stop_cleaner_(false) + , cleaner_thread_(&Subscriber::cleaner_function, this) + , logger_("is::sh::ROS2 Dynamic::Subscriber") +{ + // Adds the type name mangling + std::string type_name = message_type.name(); + std::size_t found = type_name.find_last_of("::"); + + if (found == std::string::npos) + { + logger_ << utils::Logger::Level::ERROR + << "The type must follow the ROS2 naming convention" << std::endl; + return; + } + else + { + type_name.insert(found + 1, "dds_::"); + type_name.append("_"); + } + + DynamicTypeBuilder* builder = Conversion::create_builder(message_type, type_name); + if (builder != nullptr) + { + participant->register_dynamic_type(topic_name, type_name, builder); + } + else + { + throw ROS2MiddlewareException( + logger_, "Cannot create builder for type " + type_name); + } + + dynamic_data_ = participant->create_dynamic_data(topic_name); + + // Retrieve DDS participant + ::fastdds::dds::DomainParticipant* dds_participant = participant->get_dds_participant(); + if (!dds_participant) + { + throw ROS2MiddlewareException( + logger_, "Trying to create a subscriber without a DDS participant!"); + } + + // Create DDS subscriber with default subscriber QoS + dds_subscriber_ = dds_participant->create_subscriber(::fastdds::dds::SUBSCRIBER_QOS_DEFAULT); + + if (dds_subscriber_) + { + logger_ << utils::Logger::Level::DEBUG + << "Created ROS2 Dynamic subscriber for topic '" << topic_name << "'" << std::endl; + } + else + { + std::ostringstream err; + err << "ROS2 Dynamic subscriber for topic '" << topic_name << "' was not created"; + + throw ROS2MiddlewareException(logger_, err.str()); + } + + // Create DDS topic + auto topic_description = dds_participant->lookup_topicdescription(topic_name); + if (!topic_description) + { + dds_topic_ = dds_participant->create_topic( + topic_name, type_name, ::fastdds::dds::TOPIC_QOS_DEFAULT); + if (dds_topic_) + { + logger_ << utils::Logger::Level::DEBUG + << "Created ROS2 Dynamic topic '" << topic_name << "' with type '" + << type_name << "'" << std::endl; + } + else + { + std::ostringstream err; + err << "ROS2 Dynamic topic '" << topic_name << "' with type '" + << type_name << "' was not created"; + + throw ROS2MiddlewareException(logger_, err.str()); + } + } + else + { + dds_topic_ = static_cast<::fastdds::dds::Topic*>(topic_description); + } + + // Create DDS datareader + ::fastdds::dds::DataReaderQos datareader_qos = ::fastdds::dds::DATAREADER_QOS_DEFAULT; + ::fastdds::dds::ReliabilityQosPolicy rel_policy; + rel_policy.kind = ::fastdds::dds::RELIABLE_RELIABILITY_QOS; + datareader_qos.reliability(rel_policy); + + dds_datareader_ = dds_subscriber_->create_datareader(dds_topic_, datareader_qos, this); + if (dds_datareader_) + { + logger_ << utils::Logger::Level::DEBUG + << "Created ROS2 Dynamic datareader for topic '" << topic_name << "'" << std::endl; + + participant_->associate_topic_to_dds_entity(dds_topic_, dds_datareader_); + } + else + { + std::ostringstream err; + err << "ROS2 Dynamic datareader for topic '" << topic_name << "' was not created"; + + throw ROS2MiddlewareException(logger_, err.str()); + } +} + +Subscriber::~Subscriber() +{ + logger_ << utils::Logger::Level::INFO + << "Waiting for current processing messages before quitting" << std::endl; + + { + std::unique_lock lock(cleaner_mtx_); + stop_cleaner_ = true; + cleaner_cv_.notify_one(); + } + + if (cleaner_thread_.joinable()) + { + cleaner_thread_.join(); + } + + logger_ << utils::Logger::Level::INFO + << "All messages were processed. Quitting now..." << std::endl; + + std::unique_lock lock(data_mtx_); + participant_->delete_dynamic_data(dynamic_data_); + + bool delete_topic = participant_->dissociate_topic_from_dds_entity(dds_topic_, dds_datareader_); + + dds_datareader_->set_listener(nullptr); + dds_subscriber_->delete_datareader(dds_datareader_); + participant_->get_dds_participant()->delete_subscriber(dds_subscriber_); + + if (delete_topic) + { + participant_->get_dds_participant()->delete_topic(dds_topic_); + } +} + +void Subscriber::receive( + const fastrtps::types::DynamicData* dds_message) +{ + logger_ << utils::Logger::Level::INFO + << "Receiving message from DDS for topic '" << topic_name_ << "'" << std::endl; + + ::xtypes::DynamicData is_message(message_type_); + bool success = Conversion::fastdds_to_xtypes(dds_message, is_message); + data_mtx_.unlock(); + + if (success) + { + logger_ << utils::Logger::Level::INFO + << "Received message: [[ " << is_message << " ]]" << std::endl; + + (*is_callback_)(is_message); + } + else + { + logger_ << utils::Logger::Level::ERROR + << "Failed to convert message from DDS to Integration Service for topic '" + << topic_name_ << "'" << std::endl; + } + + // Notify that we have ended + std::unique_lock lock(cleaner_mtx_); + finished_threads_.push_back(std::this_thread::get_id()); + cleaner_cv_.notify_one(); +} + +void Subscriber::on_data_available( + ::fastdds::dds::DataReader* /*reader*/) +{ + using namespace std::placeholders; + + ::fastdds::dds::SampleInfo info; + std::unique_lock lock(cleaner_mtx_); + data_mtx_.lock(); + + if (!stop_cleaner_ && fastrtps::types::ReturnCode_t::RETCODE_OK + == dds_datareader_->take_next_sample(dynamic_data_, &info)) + { +#if FASTRTPS_VERSION_MINOR < 2 + if (::fastdds::dds::InstanceStateKind::ALIVE == info.instance_state) +#else + if (::fastdds::dds::InstanceStateKind::ALIVE_INSTANCE_STATE == info.instance_state) +#endif // if FASTRTPS_VERSION_MINOR < 2 + { + logger_ << utils::Logger::Level::DEBUG + << "Processing incoming data available for topic '" + << topic_name_ << "'" << std::endl; + + std::thread* thread = new std::thread(&Subscriber::receive, this, dynamic_data_); + reception_threads_.emplace(thread->get_id(), thread); + } + else + { + data_mtx_.unlock(); + } + } + else + { + data_mtx_.unlock(); + } +} + +void Subscriber::on_subscription_matched( + ::fastdds::dds::DataReader* /*reader*/, + const ::fastdds::dds::SubscriptionMatchedStatus& info) +{ + if (1 == info.current_count_change) + { + logger_ << utils::Logger::Level::INFO + << "Subscriber for topic '" << topic_name_ << "' matched" << std::endl; + } + else if (-1 == info.current_count_change) + { + logger_ << utils::Logger::Level::INFO + << "Subscriber for topic '" << topic_name_ << "' unmatched" << std::endl; + } +} + +void Subscriber::cleaner_function() +{ + using namespace std::chrono_literals; + std::unique_lock lock(cleaner_mtx_); + std::vector stopped; + + while (!stop_cleaner_) + { + cleaner_cv_.wait( + lock, + [this]() + { + return stop_cleaner_ || !finished_threads_.empty(); + }); + + for (std::thread::id id : finished_threads_) + { + // Some threads may end too quickly, wait until the next iteration + if (reception_threads_.count(id) > 0) + { + std::thread* thread = reception_threads_.at(id); + reception_threads_.erase(id); + + if (thread->joinable()) + { + thread->join(); + } + delete thread; + stopped.push_back(id); + } + } + + for (std::thread::id id : stopped) + { + auto it = std::find(finished_threads_.begin(), finished_threads_.end(), id); + finished_threads_.erase(it); + } + stopped.clear(); + + // Free the CPU just a moment + lock.unlock(); + std::this_thread::sleep_for(10ms); + lock.lock(); + } + + // Wait for the rest of threads + for (auto& pair : reception_threads_) + { + std::thread* thread = pair.second; + if (thread->joinable()) + { + thread->join(); + } + delete thread; + } +} + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp new file mode 100644 index 0000000..99842ab --- /dev/null +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -0,0 +1,632 @@ +/* + * Copyright 2019 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 +#include +#include + +namespace eprosima { +namespace is { +namespace sh { +namespace ros2 { + +/** + * @class SystemHandle + * This class represents a full *Integration Service* system handle or plugin for the *ROS2* + * middleware, using eProsima's implementation, + * Fast DDS. + * + * This class inherits from is::FullSystem, so to implement publisher, subscriber + * operations for the *Integration Service*. + * + */ +class SystemHandle : public virtual FullSystem +{ +public: + + SystemHandle() + : FullSystem() + , logger_("is::sh::ROS2 Dynamic") + { + } + + ~SystemHandle() + { + std::filesystem::path tmp = std::filesystem::temp_directory_path(); + for (auto pkg_name: package_names) + { + std::filesystem::remove_all(tmp / pkg_name); + } + } + + bool configure( + const core::RequiredTypes& /*types*/, + const YAML::Node& configuration, + TypeRegistry& /*type_registry*/) override + { + /* + * The Fast-DDS sh doesn't define new types. + * Needed types will be defined in the 'types' section of the YAML file, and hence, + * already registered in the 'TypeRegistry' by the *Integration Service core*. + */ + + if(configuration["namespace"]) + { + namespace_ = configuration["namespace"].as(); + } + + try + { + participant_ = std::make_unique(configuration); + } + catch (ROS2MiddlewareException& e) + { + e.from_logger << utils::Logger::Level::ERROR << e.what() << std::endl; + return false; + } + + logger_ << utils::Logger::Level::INFO << "Configured!" << std::endl; + + return true; + } + + bool okay() const override + { + return (nullptr != participant_->get_dds_participant()); + } + + bool spin_once() override + { + using namespace std::chrono_literals; + std::this_thread::sleep_for(100ms); + return okay(); + } + + bool subscribe( + const std::string& topic_name, + const xtypes::DynamicType& message_type, + SubscriptionCallback* callback, + const YAML::Node& /* configuration */) override + { + try + { + std::string topic_name_mangling = get_ros2_topic_name(topic_name); + auto subscriber = std::make_shared( + participant_.get(), topic_name_mangling, message_type, callback); + + subscribers_.emplace_back(std::move(subscriber)); + + logger_ << utils::Logger::Level::INFO + << "Subscriber created for topic '" << topic_name_mangling << "', with type '" + << message_type.name() << "'" << std::endl; + + return true; + } + catch (ROS2MiddlewareException& e) + { + e.from_logger << utils::Logger::Level::ERROR << e.what() << std::endl; + return false; + } + } + + std::shared_ptr advertise( + const std::string& topic_name, + const xtypes::DynamicType& message_type, + const YAML::Node& configuration) override + { + try + { + std::string topic_name_mangling = get_ros2_topic_name(topic_name); + auto publisher = std::make_shared( + participant_.get(), topic_name_mangling, message_type, configuration); + publishers_.emplace_back(std::move(publisher)); + + logger_ << utils::Logger::Level::INFO + << "Publisher created for topic '" << topic_name_mangling << "', with type '" + << message_type.name() << "'" << std::endl; + + return publishers_.back(); + } + catch (ROS2MiddlewareException& e) + { + e.from_logger << utils::Logger::Level::ERROR << e.what() << std::endl; + return std::shared_ptr(); + } + } + + void replace_all_string( + std::string& str, + const std::string& from, + const std::string& to) const + { + size_t froms = from.size(); + size_t tos = to.size(); + size_t pos = str.find(from); + while (pos != std::string::npos) + { + str.replace(pos, froms, to); + pos = str.find(from, pos + tos); + } + } + + bool preprocess_types( + const YAML::Node& types_node) override + { + std::vector include_paths; + if (types_node["paths"]) + { + // Check if there are paths to custom idls + std::regex reg("/opt/ros/([a-z])+/share/*"); + for (auto& path : types_node["paths"]) + { + if (!std::regex_match (path.as(), reg)) + { + logger_ << utils::Logger::Level::DEBUG + << "Added path for custom IDL." << std::endl; + } + include_paths.push_back(path.as()); + } + } + + if(types_node["idls"]) + { + for (auto& entry: types_node["idls"]) + { + logger_ << utils::Logger::Level::DEBUG + << "IDL: " << Dump(entry) << std::endl; + + std::set ros2_modules; + std::vector custom_include_paths; + bool custom_includes = false; + // Retrieve the include clauses from the IDL + std::regex incl_reg("#\\s*include\\s+[<\"][^>\"]*[>\"]"); + std::string idl = entry.as(); + replace_all_string(idl, "#include", "\n#include"); + std::sregex_iterator iter(idl.begin(), idl.end(), incl_reg); + std::sregex_iterator end; + + logger_ << utils::Logger::Level::DEBUG + << "Searching include clauses..." << std::endl; + + while (iter != end) + { + for(unsigned i = 0; i < iter->size(); ++i) + { + // On each include extracts the package_name + std::string incl = (*iter)[i]; + logger_ << utils::Logger::Level::DEBUG + << "Include Clause: " << incl << std::endl; + + std::regex pkg_reg("[<\"][^>\"/]*[/]"); + std::cmatch cm; + std::regex_search(incl.c_str(), cm, pkg_reg); + std::string pkg = cm[0]; + + if (!pkg.empty()) + { + pkg = pkg.substr(1, pkg.length() - 2); + logger_ << utils::Logger::Level::DEBUG + << "Include Package: " << pkg << std::endl; + + // Check if the package has a corresponding folder within ros installation + struct stat sb; + std::string dir = "/opt/ros/" + ROS2_DISTRO + "/include/" + pkg; + if (stat(dir.c_str(), &sb) == 0 && S_ISDIR(sb.st_mode)) + { + ros2_modules.insert(pkg); + } + else + { + custom_includes = true; + } + } + else + { + custom_includes = true; + } + + if (custom_includes) + { + std::regex path_reg("[<\"][^>\"]*[\">]"); + std::cmatch cm1; + std::regex_search(incl.c_str(), cm1, path_reg); + std::string in_path = cm1[0]; + in_path = in_path.substr(1, in_path.length() - 2); + for (auto& path: include_paths) + { + logger_ << utils::Logger::Level::DEBUG + << "Looking for file " << in_path << " within path " << path << std::endl; + if (std::filesystem::exists(path + in_path)) + { + logger_ << utils::Logger::Level::DEBUG + << "IDL file found in: " << path + in_path << std::endl; + custom_include_paths.push_back(path + in_path); + } + } + } + } + ++iter; + } + + // Obtain the xtypes representation of the IDL + logger_ << utils::Logger::Level::INFO + << "Parsing IDL" << std::endl; + + eprosima::xtypes::idl::Context context; + context.allow_keyword_identifiers = true; + if (!include_paths.empty()) + { + context.include_paths = include_paths; + } + + eprosima::xtypes::idl::parse(entry.as(), context); + + if (context.success) + { + size_t modules = context.module().submodule_size() - ros2_modules.size(); + logger_ << utils::Logger::Level::DEBUG + << "Number of modules: " << modules << std::endl; + + if (modules == 0) + { + logger_ << utils::Logger::Level::ERROR + << "The type doesn't have module. Please follow the ROS2 naming convention." + << std::endl; + + return false; + } + else if (modules > 1) + { + logger_ << utils::Logger::Level::ERROR + << "There can only be one module per idl, add" + << " another entry in the YAML idls tag." << std::endl; + + return false; + } + else + { + bool is_success = true; + context.module().for_each_submodule([&] (const xtypes::idl::Module& mod) + { + if (ros2_modules.find(mod.name()) != ros2_modules.end()) + { + return; + } + + // Check that the package_name follows the ROS2 naming convention + std::regex reg("[a-z]+(([a-z0-9]*)_?[a-z0-9]+)+"); + if (!std::regex_match(mod.name(), reg) || mod.name().find("ros") != std::string::npos) + { + logger_ << utils::Logger::Level::ERROR + << "The package name [" << mod.name() + << "] doesn't follow the ROS2 naming convention." + << std::endl; + is_success = false; + return; + } + + // Check whether the following submodule is msg or srv + if (!mod.has_submodule("msg") && !mod.has_submodule("srv")) + { + logger_ << utils::Logger::Level::ERROR + << "The type must have a module msg or srv." + << std::endl; + is_success = false; + return; + } + + std::filesystem::path tmp = std::filesystem::temp_directory_path(); + //TODO: Change when the services are implemented + std::filesystem::create_directories(tmp / mod.name() / "msg"); + + package_names.insert(mod.name()); + + std::string aux; + std::size_t found = idl.rfind("struct"); + aux = idl.substr(found + 7); + found = aux.find("{"); + aux = aux.substr(0, found); + aux.erase(remove_if(aux.begin(), aux.end(), isspace), aux.end()); + + // Get the type corresponding to the last struct inside the current module + auto type = context.module().submodule(mod.name()).get()->type(aux, true); + + // Check if it is a Structure + if (type.get()->kind() != xtypes::TypeKind::STRUCTURE_TYPE) + { + logger_ << utils::Logger::Level::ERROR + << "[" << aux + << "] is not a structure." + << std::endl; + is_success = false; + return; + } + + // Check that the message name follows the ROS2 naming convention + std::regex type_reg("[A-Z]([a-zA-Z0-9])+"); + if (!std::regex_match(aux, type_reg)) + { + logger_ << utils::Logger::Level::ERROR + << "The message name [" << aux + << "] doesn't follow the ROS2 naming convention." + << std::endl; + is_success = false; + return; + } + + //TODO: Change when the services are implemented + std::ofstream idlfile ("/tmp/" + mod.name() + "/msg/" + aux + ".idl"); + idlfile << idl << std::endl; + idlfile.close(); + + for (auto& mv_idl: custom_include_paths) + { + std::string filename = std::filesystem::path(mv_idl).filename(); + std::string path = std::filesystem::path(tmp / mod.name() / "msg" / filename); + logger_ << utils::Logger::Level::DEBUG + << "The file " << filename << " has been copied to " << path + << std::endl; + if (!std::filesystem::exists(path)) + { + std::filesystem::copy(mv_idl, path); + } + } + + std::string package_name = "--package_name " + mod.name(); + std::string path = "--install_path /opt/ros/" + ROS2_DISTRO; + + logger_ << is::utils::Logger::Level::INFO + << "Generating ROS2 Type Support for package: " << package_name + << std::endl; + + std::string command = "exec bash /tmp/generator.bash " + package_name + " " + path; + + if (ros2_modules.size() != 0) + { + std::ostringstream stream; + std::copy(ros2_modules.begin(), ros2_modules.end(), std::ostream_iterator(stream, ";")); + std::string depends = "--depedencies \"" + stream.str() + "\""; + + logger_ << is::utils::Logger::Level::DEBUG + << "ROS2 Type Support Dependencies [" << depends + << "]" << std::endl; + + command += " " + depends; + } + + FILE* pipe = popen(command.c_str(), "r"); + if (!pipe) + { + logger_ << utils::Logger::Level::ERROR + << " Failed to execute command: " << command + << std::endl; + is_success = false; + return; + } + + char buffer[128]; + std::string output = ""; + while(!feof(pipe)) { + if(fgets(buffer, 128, pipe) != NULL) + output += buffer; + } + + logger_ << is::utils::Logger::Level::DEBUG + << output << std::endl; + + int st = pclose(pipe); + if (WIFEXITED(st)) + { + if (1 == WEXITSTATUS(st)) + { + logger_ << is::utils::Logger::Level::ERROR + << "Failed to generate the Type Support for package " << package_name + << ". Make sure you follow all the ROS2 naming and structure convention" + << " rules" << std::endl; + + if (logger_.get_level() != is::utils::Logger::Level::DEBUG) + { + logger_ << is::utils::Logger::Level::ERROR + << output << std::endl; + } + + is_success = false; + return; + } + else if (0 == WEXITSTATUS(st)) + { + logger_ << is::utils::Logger::Level::INFO + << "ROS2 Type Supports generation finished. Installed in path " + << path << std::endl; + } + + } + + }, false); + + return is_success; + } + } + } + } + + return false; + } + + // bool create_client_proxy( + // const std::string& service_name, + // const xtypes::DynamicType& type, + // RequestCallback* callback, + // const YAML::Node& configuration) override + // { + // return create_client_proxy(service_name, type, type, callback, configuration); + // } + + // bool create_client_proxy( + // const std::string& service_name, + // const xtypes::DynamicType& request_type, + // const xtypes::DynamicType& reply_type, + // RequestCallback* callback, + // const YAML::Node& configuration) override + // { + // if (clients_.count(service_name) == 0) + // { + // try + // { + // auto client = std::make_shared( + // participant_.get(), + // service_name, + // request_type, + // reply_type, + // callback, + // configuration); + + // clients_[service_name] = std::move(client); + + // logger_ << utils::Logger::Level::INFO + // << "Client created for service '" << service_name + // << "', with request_type '" << request_type.name() + // << "' and reply_type '" << reply_type.name() << "'" << std::endl; + + // return true; + + // } + // catch (ROS2MiddlewareException& e) + // { + // e.from_logger << utils::Logger::Level::ERROR << e.what() << std::endl; + // return false; + // } + // } + + // return clients_[service_name]->add_config(configuration, callback); + // } + + // std::shared_ptr create_service_proxy( + // const std::string& service_name, + // const xtypes::DynamicType& type, + // const YAML::Node& configuration) override + // { + // return create_service_proxy(service_name, type, type, configuration); + // } + + // std::shared_ptr create_service_proxy( + // const std::string& service_name, + // const xtypes::DynamicType& request_type, + // const xtypes::DynamicType& reply_type, + // const YAML::Node& configuration) override + // { + // if (servers_.count(service_name) == 0) + // { + // try + // { + // auto server = std::make_shared( + // participant_.get(), + // service_name, + // request_type, + // reply_type, + // configuration); + + // servers_[service_name] = std::move(server); + + // logger_ << utils::Logger::Level::INFO + // << "Server created for service '" << service_name + // << "', with request_type '" << request_type.name() + // << "' and reply_type '" << reply_type.name() << "'" << std::endl; + + // return servers_[service_name]; + // } + // catch (ROS2MiddlewareException& e) + // { + // e.from_logger << utils::Logger::Level::ERROR << e.what() << std::endl; + // return nullptr; + // } + // } + + // if (servers_[service_name]->add_config(configuration)) + // { + // return servers_[service_name]; + // } + // else + // { + // return nullptr; + // } + // } + +private: + + std::string get_ros2_topic_name( + const std::string& topic_name) + { + std::string topic_namespace = "/"; + if (!namespace_.empty()) + { + if (namespace_.front() == '/') + { + topic_namespace = namespace_; + } + else + { + topic_namespace += namespace_; + } + + if (namespace_.back() != '/') + { + topic_namespace += "/"; + } + } + std::string topic_name_mangling = "rt" + topic_namespace + topic_name; + + return topic_name_mangling; + } + + std::unique_ptr participant_; + std::vector > publishers_; + std::vector > subscribers_; + //std::map > clients_; + //std::map > servers_; + + std::string namespace_; + + std::set package_names; + + utils::Logger logger_; +}; + + + +} // namespace ros2 +} // namespace sh +} // namespace is +} // namespace eprosima + +IS_REGISTER_SYSTEM("ros2_dynamic", eprosima::is::sh::ros2::SystemHandle) diff --git a/ros2/CHANGELOG.rst b/ros2/static/CHANGELOG.rst similarity index 100% rename from ros2/CHANGELOG.rst rename to ros2/static/CHANGELOG.rst diff --git a/ros2/static/CMakeLists.txt b/ros2/static/CMakeLists.txt new file mode 100644 index 0000000..3dd7e15 --- /dev/null +++ b/ros2/static/CMakeLists.txt @@ -0,0 +1,200 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# Copyright (C) 2020 - present 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. +# +# library and integration-service executable CMake project + +################################################################################## +# CMake build rules for the Integration Service ROS 2 SystemHandle library +################################################################################## +cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) + +################################################################################### +# Configure options +################################################################################### +option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) + +################################################################################### +# Load external CMake Modules. +################################################################################### +if(BUILD_LIBRARY) + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) + + find_package(Sanitizers QUIET) + + if(SANITIZE_ADDRESS) + message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") + endif() +endif() + +################################################################################### +# External dependencies for the Integration Service ROS 2 SystemHandle library +################################################################################### +if(BUILD_LIBRARY) + find_package(is-core REQUIRED) + find_package(rclcpp REQUIRED) +endif() + +################################################################################### +# Configure the Integration Service ROS 2 SystemHandle library +################################################################################### +if(BUILD_LIBRARY) + add_library(${PROJECT_NAME} + SHARED + src/Factory.cpp + $,src/SystemHandle__foxy.cpp,src/SystemHandle.cpp> + src/MetaPublisher.cpp + ) + + if (Sanitizers_FOUND) + add_sanitizers(${PROJECT_NAME}) + endif() + + set_target_properties(${PROJECT_NAME} PROPERTIES + VERSION + ${PROJECT_VERSION} + SOVERSION + ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + CXX_STANDARD + 17 + CXX_STANDARD_REQUIRED + YES + ) + + target_compile_options(${PROJECT_NAME} + PRIVATE + $<$:-pedantic> + $<$:-fstrict-aliasing> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wcast-align> + $<$:-Wshadow> + $<$:/W4> + $<$:/wd4700> + $<$:/wd4996> + $<$:/wd4820> + $<$:/wd4255> + $<$:/wd4668> + ) + + include(GNUInstallDirs) + message(STATUS "Configuring [${PROJECT_NAME}]...") + + target_link_libraries(${PROJECT_NAME} + PUBLIC + is::core + ${rclcpp_LIBRARIES} + ) + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${rclcpp_INCLUDE_DIRS} + ) + + is_generate_export_header(ros2) + + if (NOT rosidl_runtime_cpp_FOUND) + target_compile_definitions(${PROJECT_NAME} + PUBLIC + "IS_SH_ROS2__ROSIDL_GENERATOR_CPP" + ) + endif() +endif() + +################################################################################### +# Install the Integration Service ROS 2 SystemHandle library +################################################################################### +if(BUILD_LIBRARY) + is_install_middleware_plugin( + MIDDLEWARE + ros2 + TARGET + ${PROJECT_NAME} + DEPENDENCIES + rclcpp + ) + + file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/include/ + DESTINATION + ${CMAKE_INSTALL_PREFIX}/include/ + ) +endif() + +################################################################################### +# Integration Service ROS 2 SystemHandle tests +################################################################################### +if(BUILD_LIBRARY) + if(BUILD_TESTS OR BUILD_ROS2_TESTS) + add_subdirectory(test) + endif() +endif() + +################################################################################### +# Integration Service ROS 2 SystemHandle API Reference +################################################################################### +if(BUILD_API_REFERENCE) + if(NOT BUILD_LIBRARY) + find_file(IS_DOXYGEN_CONFIG_FILE doxygen-config.in PATHS ${CMAKE_INSTALL_PREFIX}/../is-core) + endif() + find_package(Doxygen REQUIRED) + # Create doxygen directories + add_custom_target(doc-dirs + COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen + COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen/html + COMMENT "Creating documentation directories" VERBATIM) + + set(IS_ROS2_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/include/is/sh/ros2") + set(IS_ROS2_SOURCE_DIR "${CMAKE_SOURCE_DIR}/src") + file(GLOB_RECURSE HPP_FILES + "${IS_ROS2_INCLUDE_DIR}/*.h*" + "${IS_ROS2_SOURCE_DIR}/*.h*") + + # Doxygen related variables + set(DOXYGEN_INPUT_DIR "${IS_ROS2_INCLUDE_DIR} ${IS_ROS2_SOURCE_DIR}") + set(DOXYGEN_OUTPUT_DIR "${PROJECT_BINARY_DIR}/doxygen") + set(DOXYGEN_INDEX_FILE "${PROJECT_BINARY_DIR}/doxygen/xml/index.xml") + set(DOXYFILE_IN "${IS_DOXYGEN_CONFIG_FILE}") + set(DOXYFILE_OUT ${PROJECT_BINARY_DIR}/doxygen-config) + set(DOXYGEN_TAGFILES "../is-core/is_core.tag=../is-core/doxygen/html") + set(DOXYGEN_HTML_DIR "${PROJECT_BINARY_DIR}/doxygen/html") + + # Configure doxygen + configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) + + # Doxygen command + add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} + COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} + DEPENDS ${HPP_FILES} + MAIN_DEPENDENCY ${DOXYFILE_OUT} ${DOXYFILE_IN} + COMMENT "Generating doxygen documentation") + + # Generate API reference + add_custom_target(doxygen-${PROJECT_NAME} ALL + DEPENDS ${DOXYGEN_INDEX_FILE} + COMMENT "Generated API documentation with doxygen" VERBATIM) + add_dependencies(doxygen-${PROJECT_NAME} doc-dirs) + + # Install doxygen generated XML files + install(DIRECTORY ${PROJECT_BINARY_DIR}/doxygen/xml + DESTINATION doxygen + COMPONENT doxygen-${PROJECT_NAME}) + set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "ROS2 System Handle doxygen") + set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION + "eProsima ROS2 System Handle doxygen documentation") + set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) +endif() \ No newline at end of file diff --git a/ros2/docs/class.puml b/ros2/static/docs/class.puml similarity index 100% rename from ros2/docs/class.puml rename to ros2/static/docs/class.puml diff --git a/ros2/docs/design.md b/ros2/static/docs/design.md similarity index 100% rename from ros2/docs/design.md rename to ros2/static/docs/design.md diff --git a/ros2/docs/type-inference.png b/ros2/static/docs/type-inference.png similarity index 100% rename from ros2/docs/type-inference.png rename to ros2/static/docs/type-inference.png diff --git a/ros2/include/is/sh/ros2/Factory.hpp b/ros2/static/include/is/sh/ros2/Factory.hpp similarity index 100% rename from ros2/include/is/sh/ros2/Factory.hpp rename to ros2/static/include/is/sh/ros2/Factory.hpp diff --git a/ros2/src/Factory.cpp b/ros2/static/src/Factory.cpp similarity index 100% rename from ros2/src/Factory.cpp rename to ros2/static/src/Factory.cpp diff --git a/ros2/src/MetaPublisher.cpp b/ros2/static/src/MetaPublisher.cpp similarity index 100% rename from ros2/src/MetaPublisher.cpp rename to ros2/static/src/MetaPublisher.cpp diff --git a/ros2/src/MetaPublisher.hpp b/ros2/static/src/MetaPublisher.hpp similarity index 100% rename from ros2/src/MetaPublisher.hpp rename to ros2/static/src/MetaPublisher.hpp diff --git a/ros2/src/SystemHandle.cpp b/ros2/static/src/SystemHandle.cpp similarity index 100% rename from ros2/src/SystemHandle.cpp rename to ros2/static/src/SystemHandle.cpp diff --git a/ros2/src/SystemHandle.hpp b/ros2/static/src/SystemHandle.hpp similarity index 100% rename from ros2/src/SystemHandle.hpp rename to ros2/static/src/SystemHandle.hpp diff --git a/ros2/src/SystemHandle__foxy.cpp b/ros2/static/src/SystemHandle__foxy.cpp similarity index 100% rename from ros2/src/SystemHandle__foxy.cpp rename to ros2/static/src/SystemHandle__foxy.cpp diff --git a/ros2/test/CHANGELOG.rst b/ros2/static/test/CHANGELOG.rst similarity index 100% rename from ros2/test/CHANGELOG.rst rename to ros2/static/test/CHANGELOG.rst diff --git a/ros2/test/CMakeLists.txt b/ros2/static/test/CMakeLists.txt similarity index 100% rename from ros2/test/CMakeLists.txt rename to ros2/static/test/CMakeLists.txt diff --git a/ros2/test/integration/ros2__geometry_msgs.cpp b/ros2/static/test/integration/ros2__geometry_msgs.cpp similarity index 100% rename from ros2/test/integration/ros2__geometry_msgs.cpp rename to ros2/static/test/integration/ros2__geometry_msgs.cpp diff --git a/ros2/test/integration/ros2__primitives_msgs.cpp b/ros2/static/test/integration/ros2__primitives_msgs.cpp similarity index 100% rename from ros2/test/integration/ros2__primitives_msgs.cpp rename to ros2/static/test/integration/ros2__primitives_msgs.cpp diff --git a/ros2/test/integration/ros2__test_domain.cpp b/ros2/static/test/integration/ros2__test_domain.cpp similarity index 100% rename from ros2/test/integration/ros2__test_domain.cpp rename to ros2/static/test/integration/ros2__test_domain.cpp diff --git a/ros2/test/integration/ros2__test_domain__foxy.cpp b/ros2/static/test/integration/ros2__test_domain__foxy.cpp similarity index 100% rename from ros2/test/integration/ros2__test_domain__foxy.cpp rename to ros2/static/test/integration/ros2__test_domain__foxy.cpp diff --git a/ros2/test/resources/ros2__domain_change.yaml b/ros2/static/test/resources/ros2__domain_change.yaml similarity index 100% rename from ros2/test/resources/ros2__domain_change.yaml rename to ros2/static/test/resources/ros2__domain_change.yaml diff --git a/ros2/test/resources/ros2__websocket__test.yaml b/ros2/static/test/resources/ros2__websocket__test.yaml similarity index 100% rename from ros2/test/resources/ros2__websocket__test.yaml rename to ros2/static/test/resources/ros2__websocket__test.yaml diff --git a/utils/ros2-mix-generator/CMakeLists.txt b/utils/ros2-mix-generator/CMakeLists.txt index b4b5cb2..df79186 100644 --- a/utils/ros2-mix-generator/CMakeLists.txt +++ b/utils/ros2-mix-generator/CMakeLists.txt @@ -26,11 +26,19 @@ project(is-ros2-mix-generator) # Configure options ################################################################################### option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) +set(IS_ROS2_SH_MODE "Static" CACHE STRING "Select the ROS 2 SystemHandle Mode") if(NOT BUILD_LIBRARY) return() endif() +set(IS_ROS2_SH_MODE_LOWERCASE "" CACHE STRING "Build mode to lowercase") +string(TOLOWER "${IS_ROS2_SH_MODE}" IS_ROS2_SH_MODE_LOWERCASE) + +if("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "dynamic") + return() +endif() + # Get dependencies find_package(is-core REQUIRED) find_package(rosidl_parser REQUIRED) From f269f54db7fb1278231af1c8bc627fcd14ef1b33 Mon Sep 17 00:00:00 2001 From: Jose Antonio Moral Date: Wed, 16 Jun 2021 09:32:04 +0200 Subject: [PATCH 03/34] Add static and dynamic SHs to CI matrix Signed-off-by: Jose Antonio Moral Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- .github/workflows/ci.yml | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 2e4d57d..e9c134e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -20,10 +20,11 @@ jobs: ros2-sh_CI: strategy: matrix: - node: [foxy, galactic] + ros2_version: [foxy, galactic] + sh_mode: [static, dynamic] runs-on: ubuntu-20.04 - container: ros:${{ matrix.node }} + container: ros:${{ matrix.ros2_version }} steps: - uses: actions/checkout@v2 @@ -33,7 +34,7 @@ jobs: - name: Download required dependencies run: | apt update - DEBIAN_FRONTEND=noninteractive apt install -y cmake gcc g++ git libboost-dev libboost-program-options-dev libyaml-cpp-dev ros-${{ matrix.node }}-rmw-fastrtps-cpp + DEBIAN_FRONTEND=noninteractive apt install -y cmake gcc g++ git libboost-dev libboost-program-options-dev libyaml-cpp-dev ros-${{ matrix.ros2_version }}-rmw-fastrtps-cpp - name: Download the Integration Service run: | @@ -41,10 +42,10 @@ jobs: - name: Build run: | - . /opt/ros/${{ matrix.node }}/setup.sh - colcon build --cmake-args -DCMAKE_BUILD_TYPE=DEBUG -DBUILD_ROS2_TESTS=ON --event-handlers console_direct+ + . /opt/ros/${{ matrix.ros2_version }}/setup.sh + colcon build --cmake-args -DCMAKE_BUILD_TYPE=DEBUG -DBUILD_ROS2_TESTS=ON -DIS_ROS2_SH_MODE=${{ matrix.sh_mode }} --event-handlers console_direct+ - name: Test run: | - . /opt/ros/${{ matrix.node }}/setup.sh && . install/local_setup.sh && RMW_IMPLEMENTATION=rmw_fastrtps_cpp colcon test --packages-select is-ros2 --event-handlers console_direct+ + . /opt/ros/${{ matrix.ros2_version }}/setup.sh && . install/local_setup.sh && RMW_IMPLEMENTATION=rmw_fastrtps_cpp colcon test --packages-select is-ros2 --event-handlers console_direct+ colcon test-result From f032a5fe5d8d8bc90fe84993bbba2e38fc2ec1a9 Mon Sep 17 00:00:00 2001 From: laura <52071480+laura-eprosima@users.noreply.github.com> Date: Wed, 16 Jun 2021 09:58:35 +0200 Subject: [PATCH 04/34] Change Integration Service core branch Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e9c134e..fe2dfdb 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -38,7 +38,7 @@ jobs: - name: Download the Integration Service run: | - git clone --recursive https://github.com/eProsima/Integration-Service src/integration-service + git clone --recursive https://github.com/eProsima/Integration-Service -b feature/ros2_dynamic src/integration-service - name: Build run: | From d6e8f1949163213a25e1c3ef555d1d02645a7dd5 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Wed, 16 Jun 2021 11:23:24 +0200 Subject: [PATCH 05/34] Requested Changes Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/CMakeLists.txt | 2 + ros2/dynamic/CMakeLists.txt | 211 +++++----- ros2/dynamic/CMakeLists.txt.user | 371 ------------------ .../dynamic/include/is/sh/ros2/Conversion.hpp | 6 +- .../include/is/sh/ros2/Participant.hpp | 8 +- ros2/dynamic/include/is/sh/ros2/Publisher.hpp | 8 +- .../is/sh/ros2/ROS2MiddlewareException.hpp | 8 +- .../dynamic/include/is/sh/ros2/Subscriber.hpp | 8 +- ros2/dynamic/src/Conversion.cpp | 4 +- ros2/dynamic/src/Participant.cpp | 4 +- ros2/dynamic/src/Publisher.cpp | 13 +- ros2/dynamic/src/Subscriber.cpp | 6 +- ros2/dynamic/src/SystemHandle.cpp | 42 +- 13 files changed, 149 insertions(+), 542 deletions(-) delete mode 100644 ros2/dynamic/CMakeLists.txt.user diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index 4fb1d4c..8d2e143 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -65,5 +65,7 @@ if(BUILD_LIBRARY) add_subdirectory(static) elseif("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "dynamic") add_subdirectory(dynamic) + else + message(WARNING "Invalid mode selected. Please choose between 'static' or 'dynamic'") endif() endif() \ No newline at end of file diff --git a/ros2/dynamic/CMakeLists.txt b/ros2/dynamic/CMakeLists.txt index 4dc4da1..2fa7f7d 100644 --- a/ros2/dynamic/CMakeLists.txt +++ b/ros2/dynamic/CMakeLists.txt @@ -20,152 +20,133 @@ ################################################################################## cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) -################################################################################### -# Configure options -################################################################################### -option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) - ################################################################################### # Load external CMake Modules. ################################################################################### -if(BUILD_LIBRARY) - set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) - find_package(Sanitizers QUIET) +find_package(Sanitizers QUIET) - if(SANITIZE_ADDRESS) - message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") - endif() +if(SANITIZE_ADDRESS) + message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") endif() ################################################################################### # External dependencies for the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - # Check if the user specified which ROS 2 version he/she is using. If not, warn and try to automatically get it - if(NOT IS_ROS2_DISTRO) - - message(WARNING " - The variable 'IS_ROS2_DISTRO' was not used. You might want to set it to - specify which ROS 2 version should be used to compile the ROS 2 System Handle. - By default, a ROS 2 version from the sourced environment will be retrieved automatically.") - - if ("$ENV{ROS_VERSION}" STREQUAL "1") - message(FATAL_ERROR " - A ROS 1 version was sourced last in your build environment. - Please use the 'IS_ROS2_VERSION' variable!") - else() - set(IS_ROS2_DISTRO $ENV{ROS_DISTRO}) - endif() +# Check if the user specified which ROS 2 version he/she is using. If not, warn and try to automatically get it +if(NOT IS_ROS2_DISTRO) + + message(WARNING " + The variable 'IS_ROS2_DISTRO' was not used. You might want to set it to + specify which ROS 2 version should be used to compile the ROS 2 System Handle. + By default, a ROS 2 version from the sourced environment will be retrieved automatically.") + + if ("$ENV{ROS_VERSION}" STREQUAL "1") + message(FATAL_ERROR " + A ROS 1 version was sourced last in your build environment. + Please use the 'IS_ROS2_VERSION' variable!") + else() + set(IS_ROS2_DISTRO $ENV{ROS_DISTRO}) + endif() - endif() +endif() - message(STATUS "Compiling the ${PROJECT_NAME} System Handle for ROS 2 ${IS_ROS2_DISTRO}...") +message(STATUS "Compiling the ${PROJECT_NAME} System Handle for ROS 2 ${IS_ROS2_DISTRO}...") - find_package(is-core REQUIRED) - find_package(fastrtps REQUIRED) -endif() +find_package(is-core REQUIRED) +find_package(fastrtps REQUIRED) ################################################################################### # Configure the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - add_library(${PROJECT_NAME} - SHARED - src/Conversion.cpp - src/Participant.cpp - src/Publisher.cpp - src/Subscriber.cpp - src/SystemHandle.cpp - #$,src/SystemHandle__foxy.cpp,src/SystemHandle.cpp> +add_library(${PROJECT_NAME} + SHARED + src/Conversion.cpp + src/Participant.cpp + src/Publisher.cpp + src/Subscriber.cpp + src/SystemHandle.cpp + #$,src/SystemHandle__foxy.cpp,src/SystemHandle.cpp> +) + +if (Sanitizers_FOUND) + add_sanitizers(${PROJECT_NAME}) +endif() + +set_target_properties(${PROJECT_NAME} PROPERTIES + VERSION + ${PROJECT_VERSION} + SOVERSION + ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + CXX_STANDARD + 17 + CXX_STANDARD_REQUIRED + YES ) - if (Sanitizers_FOUND) - add_sanitizers(${PROJECT_NAME}) - endif() - - set_target_properties(${PROJECT_NAME} PROPERTIES - VERSION - ${PROJECT_VERSION} - SOVERSION - ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} - CXX_STANDARD - 17 - CXX_STANDARD_REQUIRED - YES - ) - - target_compile_options(${PROJECT_NAME} - PRIVATE - $<$:-pedantic> - $<$:-fstrict-aliasing> - $<$:-Wall> - $<$:-Wextra> - $<$:-Wcast-align> - $<$:-Wshadow> - $<$:/W4> - $<$:/wd4700> - $<$:/wd4996> - $<$:/wd4820> - $<$:/wd4255> - $<$:/wd4668> - ) - - include(GNUInstallDirs) - message(STATUS "Configuring [${PROJECT_NAME}]...") - - target_link_libraries(${PROJECT_NAME} - PUBLIC - is::core - fastrtps - ) - - target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ - $ - $ - ) - - #is_generate_export_header(ros2) +target_compile_options(${PROJECT_NAME} + PRIVATE + $<$:-pedantic> + $<$:-fstrict-aliasing> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wcast-align> + $<$:-Wshadow> + $<$:/W4> + $<$:/wd4700> + $<$:/wd4996> + $<$:/wd4820> + $<$:/wd4255> + $<$:/wd4668> + ) -endif() +include(GNUInstallDirs) +message(STATUS "Configuring [${PROJECT_NAME}]...") + +target_link_libraries(${PROJECT_NAME} + PUBLIC + is::core + fastrtps + ) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + $ + ) ################################################################################### # Install the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - is_install_middleware_plugin( - MIDDLEWARE - ros2_dynamic - TARGET - ${PROJECT_NAME} - ) - - file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/include/ - DESTINATION - ${CMAKE_INSTALL_PREFIX}/include/ +is_install_middleware_plugin( + MIDDLEWARE + ros2_dynamic + TARGET + ${PROJECT_NAME} ) - file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/resources/ - DESTINATION - /tmp/ - ) +file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/include/ + DESTINATION + ${CMAKE_INSTALL_PREFIX}/include/ +) -endif() +file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/resources/ + DESTINATION + /tmp/ +) ################################################################################### # Integration Service ROS 2 SystemHandle tests ################################################################################### -# if(BUILD_LIBRARY) -# if(BUILD_TESTS OR BUILD_ROS2_TESTS) -# add_subdirectory(test) -# endif() +# if(BUILD_TESTS OR BUILD_ROS2_TESTS) +# add_subdirectory(test) # endif() ################################################################################### diff --git a/ros2/dynamic/CMakeLists.txt.user b/ros2/dynamic/CMakeLists.txt.user deleted file mode 100644 index 02dae41..0000000 --- a/ros2/dynamic/CMakeLists.txt.user +++ /dev/null @@ -1,371 +0,0 @@ - - - - - - EnvironmentId - {856bbe81-b67f-4809-8a78-3bbf3a9d4c31} - - - ProjectExplorer.Project.ActiveTarget - 0 - - - ProjectExplorer.Project.EditorSettings - - true - false - true - - Cpp - - CppGlobal - - - - QmlJS - - QmlJSGlobal - - - 2 - UTF-8 - false - 4 - false - 80 - true - true - 1 - true - false - 0 - true - true - 0 - 8 - true - 1 - true - true - true - false - - - - ProjectExplorer.Project.PluginSettings - - - - ProjectExplorer.Project.Target.0 - - Desktop - Desktop - {c88d0898-f70a-4fb4-8177-c0f98e0905b8} - 0 - 0 - 0 - - - /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Default - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Default - Default - CMakeProjectManager.CMakeBuildConfiguration - - - - CMAKE_BUILD_TYPE:STRING=Debug - - /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Debug - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Debug - Debug - CMakeProjectManager.CMakeBuildConfiguration - - - - CMAKE_BUILD_TYPE:STRING=Release - - /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Release - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Release - Release - CMakeProjectManager.CMakeBuildConfiguration - - - - CMAKE_BUILD_TYPE:STRING=RelWithDebInfo - - /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Release with Debug Information - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Release with Debug Information - Release with Debug Information - CMakeProjectManager.CMakeBuildConfiguration - - - - CMAKE_BUILD_TYPE:STRING=MinSizeRel - - /home/lauramartin/projects/ROS2-SH/ros2/build-dynamic-Desktop-Minimum Size Release - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Minimum Size Release - Minimum Size Release - CMakeProjectManager.CMakeBuildConfiguration - - 5 - - - 0 - Deploy - - ProjectExplorer.BuildSteps.Deploy - - 1 - Deploy locally - - ProjectExplorer.DefaultDeployConfiguration - - 1 - - - false - false - 1000 - - true - - false - false - false - false - true - 0.01 - 10 - true - 1 - 25 - - 1 - true - true - true - /usr/bin/valgrind - - 0 - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - 12 - 13 - 14 - - -1 - - - - %{buildDir} - Custom Executable - - ProjectExplorer.CustomExecutableRunConfiguration - 3768 - false - true - false - false - true - - 1 - - - - ProjectExplorer.Project.TargetCount - 1 - - - ProjectExplorer.Project.Updater.FileVersion - 18 - - - Version - 18 - - diff --git a/ros2/dynamic/include/is/sh/ros2/Conversion.hpp b/ros2/dynamic/include/is/sh/ros2/Conversion.hpp index 81e9caf..c52b374 100644 --- a/ros2/dynamic/include/is/sh/ros2/Conversion.hpp +++ b/ros2/dynamic/include/is/sh/ros2/Conversion.hpp @@ -15,8 +15,8 @@ * */ -#ifndef _IS_SH_FASTDDS__INTERNAL__CONVERSION_HPP_ -#define _IS_SH_FASTDDS__INTERNAL__CONVERSION_HPP_ +#ifndef _IS_SH_ROS2_DYNAMIC__INTERNAL__CONVERSION_HPP_ +#define _IS_SH_ROS2_DYNAMIC__INTERNAL__CONVERSION_HPP_ #include #include @@ -229,5 +229,5 @@ struct Conversion } // namespace is } // namespace eprosima -#endif // _IS_SH_FASTDDS__INTERNAL__CONVERSION_HPP_ +#endif // _IS_SH_ROS2_DYNAMIC__INTERNAL__CONVERSION_HPP_ diff --git a/ros2/dynamic/include/is/sh/ros2/Participant.hpp b/ros2/dynamic/include/is/sh/ros2/Participant.hpp index 37d5a0f..13d9e3f 100644 --- a/ros2/dynamic/include/is/sh/ros2/Participant.hpp +++ b/ros2/dynamic/include/is/sh/ros2/Participant.hpp @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef _IS_SH_FASTDDS__INTERNAL__PARTICIPANT_HPP_ -#define _IS_SH_FASTDDS__INTERNAL__PARTICIPANT_HPP_ +#ifndef _IS_SH_ROS2_DYNAMIC__INTERNAL__PARTICIPANT_HPP_ +#define _IS_SH_ROS2_DYNAMIC__INTERNAL__PARTICIPANT_HPP_ #include "ROS2MiddlewareException.hpp" @@ -40,7 +40,7 @@ namespace ros2 { /** * @class Participant - * This class represents a ROS2 Dynamic Node within the *Integration Service* framework. + * This class represents a ROS 2 Dynamic Node within the *Integration Service* framework. * * @details It includes a mapping of the topic names to their corresponding * @@ -213,4 +213,4 @@ class Participant } // namespace is } // namespace eprosima -#endif // _IS_SH_FASTDDS__INTERNAL__PARTICIPANT_HPP_ +#endif // _IS_SH_ROS2_DYNAMIC__INTERNAL__PARTICIPANT_HPP_ diff --git a/ros2/dynamic/include/is/sh/ros2/Publisher.hpp b/ros2/dynamic/include/is/sh/ros2/Publisher.hpp index aa684ce..ea54b4f 100644 --- a/ros2/dynamic/include/is/sh/ros2/Publisher.hpp +++ b/ros2/dynamic/include/is/sh/ros2/Publisher.hpp @@ -15,8 +15,8 @@ * */ -#ifndef _IS_SH_FASTDDS__INTERNAL__PUBLISHER_HPP_ -#define _IS_SH_FASTDDS__INTERNAL__PUBLISHER_HPP_ +#ifndef _IS_SH_ROS2_DYNAMIC__INTERNAL__PUBLISHER_HPP_ +#define _IS_SH_ROS2_DYNAMIC__INTERNAL__PUBLISHER_HPP_ #include "ROS2MiddlewareException.hpp" #include "Participant.hpp" @@ -42,7 +42,7 @@ class Participant; /** * @class Publisher * This class represents a - * Fast DDS Publisher within the *Integration Service* framework. + * ROS 2 Publisher implemented using FastDDS within the *Integration Service* framework. * * Its topic type definition and data instances are represented by means * of the @@ -149,4 +149,4 @@ class Publisher } // namespace is } // namespace eprosima -#endif // _IS_SH_FASTDDS__INTERNAL__PARTICIPANT_HPP_ +#endif // _IS_SH_ROS2_DYNAMIC__INTERNAL__PARTICIPANT_HPP_ diff --git a/ros2/dynamic/include/is/sh/ros2/ROS2MiddlewareException.hpp b/ros2/dynamic/include/is/sh/ros2/ROS2MiddlewareException.hpp index 7d7e30c..c5c0f01 100644 --- a/ros2/dynamic/include/is/sh/ros2/ROS2MiddlewareException.hpp +++ b/ros2/dynamic/include/is/sh/ros2/ROS2MiddlewareException.hpp @@ -15,8 +15,8 @@ * */ -#ifndef _IS_SH_FASTDDS__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ -#define _IS_SH_FASTDDS__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ +#ifndef _IS_SH_ROS2_DYNAMIC__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ +#define _IS_SH_ROS2_DYNAMIC__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ #include @@ -30,7 +30,7 @@ namespace ros2 { /** * @class ROS2MiddlewareException * Launches a runtime error every time an unexpected behavior occurs - * related to *Fast DDS* middleware, when configuring or using this is::SystemHandle. + * related to *ROS 2 Dynamic* middleware, when configuring or using this is::SystemHandle. */ class ROS2MiddlewareException : public std::runtime_error { @@ -59,4 +59,4 @@ class ROS2MiddlewareException : public std::runtime_error } // namespace is } // namespace eprosima -#endif // _IS_SH_FASTDDS__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ +#endif // _IS_SH_ROS2_DYNAMIC__INTERNAL__ROS2MIDDLEWAREEXCEPTION_HPP_ diff --git a/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp b/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp index 01aef0c..c9423cb 100644 --- a/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp +++ b/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp @@ -15,8 +15,8 @@ * */ -#ifndef _IS_SH_FASTDDS__INTERNAL__SUBSCRIBER_HPP_ -#define _IS_SH_FASTDDS__INTERNAL__SUBSCRIBER_HPP_ +#ifndef _IS_SH_ROS2_DYNAMIC__INTERNAL__SUBSCRIBER_HPP_ +#define _IS_SH_ROS2_DYNAMIC__INTERNAL__SUBSCRIBER_HPP_ #include "ROS2MiddlewareException.hpp" #include "Participant.hpp" @@ -44,7 +44,7 @@ class Participant; /** * @class Subscriber * This class represents a - * Fast DDS Subscriber within the *Integration Service* framework. + * ROS 2 Subscriber implemented using FastDDS within the *Integration Service* framework. * * Its topic type definition and data instances are represented by means * of the @@ -172,4 +172,4 @@ class Subscriber : private ::fastdds::dds::DataReaderListener } // namespace is } // namespace eprosima -#endif // SOSS__DDS__INTERNAL__SUBSCRIBER_HPP +#endif // _IS_SH_ROS2_DYNAMIC__INTERNAL__SUBSCRIBER_HPP diff --git a/ros2/dynamic/src/Conversion.cpp b/ros2/dynamic/src/Conversion.cpp index 374723f..f2daac7 100644 --- a/ros2/dynamic/src/Conversion.cpp +++ b/ros2/dynamic/src/Conversion.cpp @@ -43,7 +43,7 @@ std::map Conversion::registered_types_; std::map Conversion::builders_; // Static member initialization -utils::Logger NavigationNode::logger_("is::sh::ROS2 Dynamic::Conversion::NavigationNode"); +utils::Logger NavigationNode::logger_("is::sh::ROS2_Dynamic::Conversion::NavigationNode"); std::string NavigationNode::get_path() { @@ -319,7 +319,7 @@ std::shared_ptr NavigationNode::get_discriminator( } // Static member initialization -utils::Logger Conversion::logger_("is::sh::ROS2 Dynamic::Conversion"); +utils::Logger Conversion::logger_("is::sh::ROS2_Dynamic::Conversion"); // This function patches the problem of dynamic types, which do not admit '/' in their type name. std::string Conversion::convert_type_name( diff --git a/ros2/dynamic/src/Participant.cpp b/ros2/dynamic/src/Participant.cpp index 186de5a..e7efd6c 100644 --- a/ros2/dynamic/src/Participant.cpp +++ b/ros2/dynamic/src/Participant.cpp @@ -33,7 +33,7 @@ namespace ros2 { Participant::Participant() : dds_participant_(nullptr) - , logger_("is::sh::ROS2 Dynamic::Participant") + , logger_("is::sh::ROS2_Dynamic::Participant") { build_participant(); } @@ -41,7 +41,7 @@ Participant::Participant() Participant::Participant( const YAML::Node& config) : dds_participant_(nullptr) - , logger_("is::sh::ROS2 Dynamic::Participant") + , logger_("is::sh::ROS2_Dynamic::Participant") { ::fastdds::dds::DomainId_t domain_id = 0; std::string node_name = "default_IS-ROS2-Dynamic-SH_participant"; diff --git a/ros2/dynamic/src/Publisher.cpp b/ros2/dynamic/src/Publisher.cpp index 3611d65..0eefe79 100644 --- a/ros2/dynamic/src/Publisher.cpp +++ b/ros2/dynamic/src/Publisher.cpp @@ -37,7 +37,7 @@ Publisher::Publisher( const YAML::Node& config) : participant_(participant) , topic_name_(topic_name) - , logger_("is::sh::ROS2 Dynamic::Publisher") + , logger_("is::sh::ROS2_Dynamic::Publisher") { // Adds the type name mangling @@ -122,13 +122,6 @@ Publisher::Publisher( // Create DDS datawriter ::fastdds::dds::DataWriterQos datawriter_qos = ::fastdds::dds::DATAWRITER_QOS_DEFAULT; - if (config["service_instance_name"]) - { - fastrtps::rtps::Property instance_property; - instance_property.name("dds.rpc.service_instance_name"); - instance_property.value(config["service_instance_name"].as()); - datawriter_qos.properties().properties().emplace_back(std::move(instance_property)); - } dds_datawriter_ = dds_publisher_->create_datawriter(dds_topic_, datawriter_qos, this); if (dds_datawriter_) @@ -169,7 +162,7 @@ bool Publisher::publish( std::unique_lock lock(data_mtx_); logger_ << utils::Logger::Level::INFO - << "Sending message from Integration Service to DDS for topic '" << topic_name_ << "': " + << "Sending message from Integration Service to ROS 2 for topic '" << topic_name_ << "': " << "[[ " << message << " ]]" << std::endl; bool success = Conversion::xtypes_to_fastdds(message, dynamic_data_); @@ -180,7 +173,7 @@ bool Publisher::publish( else { logger_ << utils::Logger::Level::ERROR - << "Failed to convert message from Integration Service to DDS for topic '" + << "Failed to convert message from Integration Service to ROS 2 for topic '" << topic_name_ << "': [[ " << message << " ]]" << std::endl; } diff --git a/ros2/dynamic/src/Subscriber.cpp b/ros2/dynamic/src/Subscriber.cpp index 39df884..21ac897 100644 --- a/ros2/dynamic/src/Subscriber.cpp +++ b/ros2/dynamic/src/Subscriber.cpp @@ -50,7 +50,7 @@ Subscriber::Subscriber( , reception_threads_() , stop_cleaner_(false) , cleaner_thread_(&Subscriber::cleaner_function, this) - , logger_("is::sh::ROS2 Dynamic::Subscriber") + , logger_("is::sh::ROS2_Dynamic::Subscriber") { // Adds the type name mangling std::string type_name = message_type.name(); @@ -192,7 +192,7 @@ void Subscriber::receive( const fastrtps::types::DynamicData* dds_message) { logger_ << utils::Logger::Level::INFO - << "Receiving message from DDS for topic '" << topic_name_ << "'" << std::endl; + << "Receiving message from ROS 2 for topic '" << topic_name_ << "'" << std::endl; ::xtypes::DynamicData is_message(message_type_); bool success = Conversion::fastdds_to_xtypes(dds_message, is_message); @@ -208,7 +208,7 @@ void Subscriber::receive( else { logger_ << utils::Logger::Level::ERROR - << "Failed to convert message from DDS to Integration Service for topic '" + << "Failed to convert message from ROS 2 to Integration Service for topic '" << topic_name_ << "'" << std::endl; } diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index 99842ab..c0b6e04 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -56,7 +56,7 @@ class SystemHandle : public virtual FullSystem SystemHandle() : FullSystem() - , logger_("is::sh::ROS2 Dynamic") + , logger_("is::sh::ROS2_Dynamic") { } @@ -75,7 +75,7 @@ class SystemHandle : public virtual FullSystem TypeRegistry& /*type_registry*/) override { /* - * The Fast-DDS sh doesn't define new types. + * The ROS 2 Dynamic SH doesn't define new types. * Needed types will be defined in the 'types' section of the YAML file, and hence, * already registered in the 'TypeRegistry' by the *Integration Service core*. */ @@ -200,7 +200,7 @@ class SystemHandle : public virtual FullSystem if(types_node["idls"]) { - for (auto& entry: types_node["idls"]) + for (const auto& entry : types_node["idls"]) { logger_ << utils::Logger::Level::DEBUG << "IDL: " << Dump(entry) << std::endl; @@ -216,7 +216,7 @@ class SystemHandle : public virtual FullSystem std::sregex_iterator end; logger_ << utils::Logger::Level::DEBUG - << "Searching include clauses..." << std::endl; + << "Searching include clauses..." << std::endl; while (iter != end) { @@ -225,7 +225,7 @@ class SystemHandle : public virtual FullSystem // On each include extracts the package_name std::string incl = (*iter)[i]; logger_ << utils::Logger::Level::DEBUG - << "Include Clause: " << incl << std::endl; + << "Include Clause: " << incl << std::endl; std::regex pkg_reg("[<\"][^>\"/]*[/]"); std::cmatch cm; @@ -262,10 +262,10 @@ class SystemHandle : public virtual FullSystem std::regex_search(incl.c_str(), cm1, path_reg); std::string in_path = cm1[0]; in_path = in_path.substr(1, in_path.length() - 2); - for (auto& path: include_paths) + for (const auto& path : include_paths) { logger_ << utils::Logger::Level::DEBUG - << "Looking for file " << in_path << " within path " << path << std::endl; + << "Looking for file " << in_path << " within path " << path << std::endl; if (std::filesystem::exists(path + in_path)) { logger_ << utils::Logger::Level::DEBUG @@ -300,7 +300,7 @@ class SystemHandle : public virtual FullSystem if (modules == 0) { logger_ << utils::Logger::Level::ERROR - << "The type doesn't have module. Please follow the ROS2 naming convention." + << "The type is not declared within an IDL module. Please follow the ROS 2 naming convention." << std::endl; return false; @@ -308,8 +308,8 @@ class SystemHandle : public virtual FullSystem else if (modules > 1) { logger_ << utils::Logger::Level::ERROR - << "There can only be one module per idl, add" - << " another entry in the YAML idls tag." << std::endl; + << "There can only be one module per IDL, add" + << " another entry in the YAML `idls` tag." << std::endl; return false; } @@ -339,7 +339,8 @@ class SystemHandle : public virtual FullSystem if (!mod.has_submodule("msg") && !mod.has_submodule("srv")) { logger_ << utils::Logger::Level::ERROR - << "The type must have a module msg or srv." + << "The module '" << mod.name() + << "' must be followed by a 'msg' or 'srv' submodule prior to declaring the type." << std::endl; is_success = false; return; @@ -389,21 +390,21 @@ class SystemHandle : public virtual FullSystem idlfile << idl << std::endl; idlfile.close(); - for (auto& mv_idl: custom_include_paths) + for (const auto& mv_idl : custom_include_paths) { std::string filename = std::filesystem::path(mv_idl).filename(); std::string path = std::filesystem::path(tmp / mod.name() / "msg" / filename); logger_ << utils::Logger::Level::DEBUG - << "The file " << filename << " has been copied to " << path - << std::endl; + << "The file " << filename << " has been copied to " << path + << std::endl; if (!std::filesystem::exists(path)) { std::filesystem::copy(mv_idl, path); } } - std::string package_name = "--package_name " + mod.name(); - std::string path = "--install_path /opt/ros/" + ROS2_DISTRO; + const std::string package_name = "--package_name " + mod.name(); + const std::string path = "--install_path /opt/ros/" + ROS2_DISTRO; logger_ << is::utils::Logger::Level::INFO << "Generating ROS2 Type Support for package: " << package_name @@ -415,7 +416,7 @@ class SystemHandle : public virtual FullSystem { std::ostringstream stream; std::copy(ros2_modules.begin(), ros2_modules.end(), std::ostream_iterator(stream, ";")); - std::string depends = "--depedencies \"" + stream.str() + "\""; + const std::string depends = "--dependencies \"" + stream.str() + "\""; logger_ << is::utils::Logger::Level::DEBUG << "ROS2 Type Support Dependencies [" << depends @@ -450,8 +451,8 @@ class SystemHandle : public virtual FullSystem if (1 == WEXITSTATUS(st)) { logger_ << is::utils::Logger::Level::ERROR - << "Failed to generate the Type Support for package " << package_name - << ". Make sure you follow all the ROS2 naming and structure convention" + << "Failed to generate the Type Support for package '" << package_name + << "'. Make sure you follow all the ROS2 naming and structure convention" << " rules" << std::endl; if (logger_.get_level() != is::utils::Logger::Level::DEBUG) @@ -466,7 +467,8 @@ class SystemHandle : public virtual FullSystem else if (0 == WEXITSTATUS(st)) { logger_ << is::utils::Logger::Level::INFO - << "ROS2 Type Supports generation finished. Installed in path " + << "ROS2 Type Supports generation finishedfor package '" + << package_name << "', installedin path " << path << std::endl; } From 4a413af57bbab401910a4f848be01c4f43ac01fe Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Wed, 16 Jun 2021 11:51:45 +0200 Subject: [PATCH 06/34] Fix CMakeList Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index 8d2e143..ad494d1 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -65,7 +65,7 @@ if(BUILD_LIBRARY) add_subdirectory(static) elseif("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "dynamic") add_subdirectory(dynamic) - else + else() message(WARNING "Invalid mode selected. Please choose between 'static' or 'dynamic'") endif() endif() \ No newline at end of file From 5ccddd5c338c3671924fb94335e37ec3f0ff089f Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Wed, 16 Jun 2021 13:22:35 +0200 Subject: [PATCH 07/34] const Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/src/SystemHandle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index c0b6e04..b153ecd 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -187,7 +187,7 @@ class SystemHandle : public virtual FullSystem { // Check if there are paths to custom idls std::regex reg("/opt/ros/([a-z])+/share/*"); - for (auto& path : types_node["paths"]) + for (const auto& path : types_node["paths"]) { if (!std::regex_match (path.as(), reg)) { From 32eea9b67083e177469984c370cff98446aa5201 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Wed, 16 Jun 2021 13:27:45 +0200 Subject: [PATCH 08/34] finishedfor Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/src/SystemHandle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index b153ecd..d973f5b 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -467,7 +467,7 @@ class SystemHandle : public virtual FullSystem else if (0 == WEXITSTATUS(st)) { logger_ << is::utils::Logger::Level::INFO - << "ROS2 Type Supports generation finishedfor package '" + << "ROS2 Type Supports generation finished for package '" << package_name << "', installedin path " << path << std::endl; } From 5aa50861ea9a75523a40a9430e848eea42614dd6 Mon Sep 17 00:00:00 2001 From: Jose Antonio Moral Date: Wed, 16 Jun 2021 16:30:50 +0200 Subject: [PATCH 09/34] Add first simple test Signed-off-by: Jose Antonio Moral Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/CMakeLists.txt | 6 +- ros2/dynamic/test/CMakeLists.txt | 105 ++++++++++++ .../integration/check_ros2pkg_creation.cpp | 153 ++++++++++++++++++ .../test/resources/ros2__geometry_msgs.yaml | 17 ++ 4 files changed, 278 insertions(+), 3 deletions(-) create mode 100644 ros2/dynamic/test/CMakeLists.txt create mode 100644 ros2/dynamic/test/integration/check_ros2pkg_creation.cpp create mode 100644 ros2/dynamic/test/resources/ros2__geometry_msgs.yaml diff --git a/ros2/dynamic/CMakeLists.txt b/ros2/dynamic/CMakeLists.txt index 2fa7f7d..3102493 100644 --- a/ros2/dynamic/CMakeLists.txt +++ b/ros2/dynamic/CMakeLists.txt @@ -145,9 +145,9 @@ file( ################################################################################### # Integration Service ROS 2 SystemHandle tests ################################################################################### -# if(BUILD_TESTS OR BUILD_ROS2_TESTS) -# add_subdirectory(test) -# endif() +if(BUILD_TESTS OR BUILD_ROS2_TESTS) + add_subdirectory(test) +endif() ################################################################################### # Integration Service ROS 2 SystemHandle API Reference diff --git a/ros2/dynamic/test/CMakeLists.txt b/ros2/dynamic/test/CMakeLists.txt new file mode 100644 index 0000000..cf47d55 --- /dev/null +++ b/ros2/dynamic/test/CMakeLists.txt @@ -0,0 +1,105 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# Copyright (C) 2020 - present 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. +# +# is-ros2 test SystemHandle library and integration-service executable CMake project + +############################################################################################### +# CMake build rules for the Integration Service ROS 2 SystemHandle integration test library +############################################################################################### + +cmake_minimum_required(VERSION 3.5.0) + +# Get Integration Service dependencies +find_package(is-mock REQUIRED) + +# Get message dependencies +# find_package(geometry_msgs REQUIRED) +# find_package(nav_msgs REQUIRED) +# find_package(std_msgs REQUIRED) + +macro(compile_test) + # Parse arguments + if("${ARGV0}" STREQUAL "NAME") + set(uniValueArgs NAME) + else() + set(TEST_NAME "${ARGV0}") + endif() + set(multiValueArgs SOURCE) + cmake_parse_arguments(TEST "" "${uniValueArgs}" "${multiValueArgs}" ${ARGN}) + + add_executable(${TEST_NAME} ${TEST_SOURCE}) + + target_link_libraries(${TEST_NAME} + PUBLIC + is::mock + # ${rclcpp_LIBRARIES} + # ${geometry_msgs_LIBRARIES} + # ${nav_msgs_LIBRARIES} + # ${std_msgs_LIBRARIES} + PRIVATE + $,libgtest,gtest> + ) + + target_include_directories(${TEST_NAME} + PRIVATE + # ${geometry_msgs_INCLUDE_DIRS} + # ${nav_msgs_INCLUDE_DIRS} + # ${std_msgs_INCLUDE_DIRS} + ) + + target_compile_options(${TEST_NAME} + INTERFACE + $<$:-Wall> + $<$:-Wextra> + ) + + set_target_properties(${TEST_NAME} + PROPERTIES + CXX_STANDARD 17 + ) + + add_gtest(${TEST_NAME} SOURCES ${TEST_SOURCE}) +endmacro() + +include(CTest) +include(${IS_GTEST_CMAKE_MODULE_DIR}/gtest.cmake) +enable_testing() + +# compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msgs.cpp) +# compile_test(${PROJECT_NAME}_primitive_msgs SOURCE integration/ros2__primitives_msgs.cpp) +compile_test(${PROJECT_NAME}_check_ros2pkg_creation SOURCE integration/check_ros2pkg_creation.cpp) + +#if ("${IS_ROS2_DISTRO}" STREQUAL "foxy") +# compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) +#else() +# compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain.cpp) +#endif() + +set(test_msgs_config_file "ros2__test_msgs.yaml") + +set_property( + # TARGET ${PROJECT_NAME}_geometry_msgs ${PROJECT_NAME}_test_domain ${PROJECT_NAME}_primitive_msgs + TARGET ${PROJECT_NAME}_check_ros2pkg_creation + APPEND PROPERTY COMPILE_DEFINITIONS PRIVATE + # "ROS2__GEOMETRY_MSGS__TEST_CONFIG=\"${CMAKE_CURRENT_LIST_DIR}/resources/ros2__geometry_msgs.yaml\"" + # "ROS2__TEST_DOMAIN__TEST_CONFIG=\"${CMAKE_CURRENT_LIST_DIR}/resources/ros2__domain_change.yaml\"" + "ROS2__ROSIDL__BUILD_DIR=\"${CMAKE_BINARY_DIR}/is/rosidl/ros2/lib\"" + ) + +# Windows dll dependencies installation +if(WIN32) + find_file(MOCKDLL NAMES "is-mock.dll" PATHS "${is-mock_DIR}" PATH_SUFFIXES "lib" ) + install (FILES ${MOCKDLL} DESTINATION "${CMAKE_INSTALL_PREFIX}/../bin") +endif() diff --git a/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp b/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp new file mode 100644 index 0000000..79598bc --- /dev/null +++ b/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2020 - present 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 + +// TODO: resolve this include #include +// TODO remove this workaround +#define ROS2_DISTRO "foxy" + +#include +#include +#include + +#include + +#include + +namespace is = eprosima::is; +namespace xtypes = eprosima::xtypes; +using namespace std::chrono_literals; + +static is::utils::Logger logger("is::sh::ROS2_Dynamic::test::check_ros2pkg_creation"); + +class ROS2Dynamic : public testing::Test +{ +public: + + void SetUp() override + { + std::string yaml; + + yaml += "types:\n"; + yaml += " idls:\n"; + yaml += " - >\n"; + yaml += " #include \n\n"; + yaml += " module custom_msgs\n"; + yaml += " {\n"; + yaml += " module msg\n"; + yaml += " {\n"; + yaml += " struct Message\n"; + yaml += " {\n"; + yaml += " std_msgs::msg::String text;\n"; + yaml += " };\n"; + yaml += " };\n"; + yaml += " };\n"; + yaml += " paths: [\"/opt/ros/"; + yaml += ROS2_DISTRO; + yaml += "/share\"]\n"; + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic } \n"; + yaml += " mock: { type: mock }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'custom_msgs::msg::Message', route: ros2_to_mock }\n"; + yaml += " echo: { type: 'custom_msgs::msg::Message', route: mock_to_ros2 }\n"; + + logger << is::utils::Logger::Level::INFO << "YAML file:\n" << yaml << std::endl; + + YAML::Node config_node = YAML::Load(yaml); + + handle = std::make_unique(is::run_instance( + config_node, {ROS2__ROSIDL__BUILD_DIR})); + + ASSERT_TRUE(handle.get()); + logger << is::utils::Logger::Level::INFO + << "Integration Service launch finished" << std::endl; + } + + void TearDown() override + { + // Quit and wait for no more than a minute. We don't want the test to get + // hung here indefinitely in the case of an error. + handle->quit().wait_for(1min); + + // Require that it's no longer running. If it is still running, then it is + // probably stuck, and we should forcefully quit. + ASSERT_TRUE(!handle->running()); + ASSERT_EQ(handle->wait(), 0); + } + + void test() + { + logger << is::utils::Logger::Level::INFO << "Execute ROS2_Dynamic test" << std::endl; + + std::ostringstream command; + command << ". /opt/ros/" << ROS2_DISTRO << "/setup.sh && ros2 topic pub /test "; + command << "custom_msgs/msg/Message \"{text: {data: 'thisisatest'}}\" --once"; + + FILE* pipe = popen(command.str().c_str(), "r"); + if (!pipe) + { + logger << is::utils::Logger::Level::ERROR + << "Failed to execute command: " << command.str() << std::endl; + } + ASSERT_NE(nullptr, pipe); + + char buffer[128]; + std::string output = ""; + while (!feof(pipe)) + { + if (fgets(buffer, 128, pipe) != NULL) + { + output += buffer; + } + } + + logger << is::utils::Logger::Level::INFO << output << std::endl; + int st = pclose(pipe); + ASSERT_NE(0, WIFEXITED(st)); + ASSERT_EQ(0, WEXITSTATUS(st)); + + auto res = output.find( + "publishing #1: custom_msgs.msg.Message(text=std_msgs.msg.String(data='thisisatest'))"); + ASSERT_NE(std::string::npos, res); + } + +protected: + + std::unique_ptr handle; +}; + +TEST_F(ROS2Dynamic, Check_ros2pkg_creation) +{ + this->test(); +} + +int main( + int argc, + char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml b/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml new file mode 100644 index 0000000..000f794 --- /dev/null +++ b/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml @@ -0,0 +1,17 @@ +systems: + ros2: { type: ros2 } + mock: { type: mock, types-from: ros2 } + +routes: + mock_to_ros2: { from: mock, to: ros2 } + ros2_to_mock: { from: ros2, to: mock } + mock_srv: { server: mock, clients: ros2 } + ros2_srv: { server: ros2, clients: mock } + +topics: + transmit_pose: { type: "geometry_msgs/Pose", route: ros2_to_mock } + echo_pose: { type: "geometry_msgs/Pose", route: mock_to_ros2 } + +services: + get_plan: { type: "nav_msgs/GetPlan:request", route: ros2_srv } + echo_plan: { type: "nav_msgs/GetPlan:response", route: mock_srv } From 4a1f83a075a3a0709facbd5712d7794ef2de2e40 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Thu, 17 Jun 2021 11:50:33 +0200 Subject: [PATCH 10/34] Avoid infinite loop Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/include/is/sh/ros2/Publisher.hpp | 14 +++++++++ .../dynamic/include/is/sh/ros2/Subscriber.hpp | 6 +++- ros2/dynamic/src/Publisher.cpp | 10 +++++++ ros2/dynamic/src/Subscriber.cpp | 7 +++-- ros2/dynamic/src/SystemHandle.cpp | 30 +++++++++++++++++++ 5 files changed, 63 insertions(+), 4 deletions(-) diff --git a/ros2/dynamic/include/is/sh/ros2/Publisher.hpp b/ros2/dynamic/include/is/sh/ros2/Publisher.hpp index ea54b4f..5742dfc 100644 --- a/ros2/dynamic/include/is/sh/ros2/Publisher.hpp +++ b/ros2/dynamic/include/is/sh/ros2/Publisher.hpp @@ -119,6 +119,20 @@ class Publisher bool publish( const xtypes::DynamicData& message) override; + /** + * @brief Get the topic name where this publisher sends data to. + * + * @returns The topic name. + */ + const std::string& topic_name() const; + + /** + * @brief Get the DDS instance handle object for the associated datawriter. + * + * @returns The datawriter instance handle. + */ + const fastrtps::rtps::InstanceHandle_t get_dds_instance_handle() const; + private: /** diff --git a/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp b/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp index c9423cb..e18057a 100644 --- a/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp +++ b/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -116,9 +117,12 @@ class Subscriber : private ::fastdds::dds::DataReaderListener * @brief Handle the receiving of a new message from the DDS dataspace. * * @param[in] dds_message The incoming message. + * + * @param[in] sample_info Structure containing the relevant information regarding the incoming message. */ void receive( - const fastrtps::types::DynamicData* dds_message); + const fastrtps::types::DynamicData* dds_message, + ::fastdds::dds::SampleInfo sample_info); private: diff --git a/ros2/dynamic/src/Publisher.cpp b/ros2/dynamic/src/Publisher.cpp index 0eefe79..8e9b51a 100644 --- a/ros2/dynamic/src/Publisher.cpp +++ b/ros2/dynamic/src/Publisher.cpp @@ -180,6 +180,16 @@ bool Publisher::publish( return success; } +const std::string& Publisher::topic_name() const +{ + return topic_name_; +} + +const fastrtps::rtps::InstanceHandle_t Publisher::get_dds_instance_handle() const +{ + return dds_datawriter_->get_instance_handle(); +} + void Publisher::on_publication_matched( ::fastdds::dds::DataWriter* /*writer*/, const ::fastdds::dds::PublicationMatchedStatus& info) diff --git a/ros2/dynamic/src/Subscriber.cpp b/ros2/dynamic/src/Subscriber.cpp index 21ac897..5a968a5 100644 --- a/ros2/dynamic/src/Subscriber.cpp +++ b/ros2/dynamic/src/Subscriber.cpp @@ -189,7 +189,8 @@ Subscriber::~Subscriber() } void Subscriber::receive( - const fastrtps::types::DynamicData* dds_message) + const fastrtps::types::DynamicData* dds_message, + ::fastdds::dds::SampleInfo sample_info) { logger_ << utils::Logger::Level::INFO << "Receiving message from ROS 2 for topic '" << topic_name_ << "'" << std::endl; @@ -203,7 +204,7 @@ void Subscriber::receive( logger_ << utils::Logger::Level::INFO << "Received message: [[ " << is_message << " ]]" << std::endl; - (*is_callback_)(is_message); + (*is_callback_)(is_message, static_cast(&sample_info)); } else { @@ -240,7 +241,7 @@ void Subscriber::on_data_available( << "Processing incoming data available for topic '" << topic_name_ << "'" << std::endl; - std::thread* thread = new std::thread(&Subscriber::receive, this, dynamic_data_); + std::thread* thread = new std::thread(&Subscriber::receive, this, dynamic_data_, info); reception_threads_.emplace(thread->get_id(), thread); } else diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index d973f5b..b44744a 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -139,6 +139,36 @@ class SystemHandle : public virtual FullSystem } } + bool is_internal_message( + void* filter_handle) + { + ::fastdds::dds::SampleInfo* sample_info = static_cast<::fastdds::dds::SampleInfo*>(filter_handle); + + auto sample_writer_guid = fastrtps::rtps::iHandle2GUID(sample_info->publication_handle); + + if (sample_writer_guid.guidPrefix == participant_->get_dds_participant()->guid().guidPrefix) + { + if (utils::Logger::Level::DEBUG == logger_.get_level()) + { + for (const auto& publisher : publishers_) + { + if (sample_writer_guid == fastrtps::rtps::iHandle2GUID(publisher->get_dds_instance_handle())) + { + logger_ << utils::Logger::Level::DEBUG + << "Received internal message from publisher '" + << publisher->topic_name() << "', ignoring it..." << std::endl; + + break; + } + } + } + // This is a message published FROM Integration Service. Discard it. + return true; + } + + return false; + } + std::shared_ptr advertise( const std::string& topic_name, const xtypes::DynamicType& message_type, From 53c013e48d06118cdf95e99fa94138c67de088cb Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Thu, 17 Jun 2021 16:11:43 +0200 Subject: [PATCH 11/34] New functionality for 'using' YAML tag Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/src/Publisher.cpp | 2 +- ros2/dynamic/src/SystemHandle.cpp | 94 +++++++++++++++++++++++++++++-- 2 files changed, 91 insertions(+), 5 deletions(-) diff --git a/ros2/dynamic/src/Publisher.cpp b/ros2/dynamic/src/Publisher.cpp index 8e9b51a..81444ec 100644 --- a/ros2/dynamic/src/Publisher.cpp +++ b/ros2/dynamic/src/Publisher.cpp @@ -34,7 +34,7 @@ Publisher::Publisher( Participant* participant, const std::string& topic_name, const xtypes::DynamicType& message_type, - const YAML::Node& config) + const YAML::Node& /*config*/) : participant_(participant) , topic_name_(topic_name) , logger_("is::sh::ROS2_Dynamic::Publisher") diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index b44744a..1a8844e 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -40,6 +40,8 @@ namespace is { namespace sh { namespace ros2 { +namespace xtypes = eprosima::xtypes; + /** * @class SystemHandle * This class represents a full *Integration Service* system handle or plugin for the *ROS2* @@ -72,7 +74,7 @@ class SystemHandle : public virtual FullSystem bool configure( const core::RequiredTypes& /*types*/, const YAML::Node& configuration, - TypeRegistry& /*type_registry*/) override + TypeRegistry& type_registry) override { /* * The ROS 2 Dynamic SH doesn't define new types. @@ -80,11 +82,22 @@ class SystemHandle : public virtual FullSystem * already registered in the 'TypeRegistry' by the *Integration Service core*. */ - if(configuration["namespace"]) + if (configuration["namespace"]) { namespace_ = configuration["namespace"].as(); } + if (configuration["using"]) + { + if (!add_types_to_registry(configuration["using"], type_registry)) + { + logger_ << utils::Logger::Level::ERROR + << "Failed to register the types." << std::endl; + + return false; + } + } + try { participant_ = std::make_unique(configuration); @@ -194,6 +207,74 @@ class SystemHandle : public virtual FullSystem } } + bool add_types_to_registry( + const YAML::Node& configuration, + TypeRegistry& type_registry) + { + logger_ << utils::Logger::Level::INFO << "Using node " << Dump(configuration) << std::endl; + std::map, std::string> type_paths; //, Path + + for (auto& conf : configuration) + { + std::string type = conf.as(); + logger_ << utils::Logger::Level::INFO << "TYPE: " << type << std::endl; + std::size_t found = type.find("/"); + // The type introduced is a package. Register all the types inside that package + if (found == std::string::npos) + { + logger_ << utils::Logger::Level::INFO << "It is a package" << std::endl; + std::string path = "/opt/ros/" + ROS2_DISTRO + "/share/" + type + "/msg"; + + for (const auto & entry : std::filesystem::directory_iterator(path)) + { + if (std::filesystem::is_regular_file(entry) && entry.path().extension() == ".idl") + { + logger_ << utils::Logger::Level::INFO << "Entry in the path " << path + << " : " << entry.path() << std::endl; + type_paths.emplace(std::make_pair(type, entry.path().stem().string()), entry.path().string()); + } + } + } + else + { + logger_ << utils::Logger::Level::INFO << "It is a typename" << std::endl; + std::string pkg = type.substr(0, found); + std::string type_name = type.substr(found + 1); + std::string path = "/opt/ros/" + ROS2_DISTRO + "/share/" + pkg + "/msg/" + type_name + ".idl"; + logger_ << utils::Logger::Level::INFO << "Package " << pkg << " Type " << type_name << std::endl; + + if (std::filesystem::exists(path)) + { + type_paths.emplace(std::make_pair(pkg, type_name), path); + } + else + { + logger_ << utils::Logger::Level::INFO << "The type '" << type_name << "' doesn't exists within the " + << "package '" << pkg << "'" << std::endl; + } + } + } + + for (auto const& [type, path] : type_paths) + { + xtypes::idl::Context context; + context.allow_keyword_identifiers = true; + context.include_paths.push_back("/opt/ros/" + ROS2_DISTRO + "/share"); + xtypes::idl::parse_file(path, context); + + if (context.success) + { + xtypes::DynamicType::Ptr dtype = context.module().submodule(type.first)->type(type.second, true); + if (dtype.get()) + { + type_registry.emplace(type.first + "::msg::" + type.second, std::move(dtype)); + } + } + } + + return true; + } + void replace_all_string( std::string& str, const std::string& from, @@ -212,6 +293,11 @@ class SystemHandle : public virtual FullSystem bool preprocess_types( const YAML::Node& types_node) override { + if (!types_node) + { + return true; + } + std::vector include_paths; if (types_node["paths"]) { @@ -312,14 +398,14 @@ class SystemHandle : public virtual FullSystem logger_ << utils::Logger::Level::INFO << "Parsing IDL" << std::endl; - eprosima::xtypes::idl::Context context; + xtypes::idl::Context context; context.allow_keyword_identifiers = true; if (!include_paths.empty()) { context.include_paths = include_paths; } - eprosima::xtypes::idl::parse(entry.as(), context); + xtypes::idl::parse(entry.as(), context); if (context.success) { From 7ec885cfc4219c3e2d23c12b435b2f958aafd521 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Mon, 21 Jun 2021 10:28:34 +0200 Subject: [PATCH 12/34] Add tests to Dynamic SystemHandle Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/CMakeLists.txt | 26 +- ros2/dynamic/CMakeLists.txt | 295 ++++++++------- ros2/dynamic/src/SystemHandle.cpp | 19 +- ros2/dynamic/test/CMakeLists.txt | 39 +- .../test/integration/ros2__geometry_msgs.cpp | 352 ++++++++++++++++++ .../test/integration/ros2__test_domain.cpp | 180 +++++++++ .../integration/ros2__test_domain__foxy.cpp | 209 +++++++++++ .../test/resources/ros2__geometry_msgs.yaml | 12 +- ros2/static/test/CMakeLists.txt | 1 + .../ros2__geometry_msgs__pubsub.yaml | 0 .../ros2__geometry_msgs__services.yaml | 0 11 files changed, 935 insertions(+), 198 deletions(-) create mode 100644 ros2/dynamic/test/integration/ros2__geometry_msgs.cpp create mode 100644 ros2/dynamic/test/integration/ros2__test_domain.cpp create mode 100644 ros2/dynamic/test/integration/ros2__test_domain__foxy.cpp rename ros2/{ => static}/test/resources/ros2__geometry_msgs__pubsub.yaml (100%) rename ros2/{ => static}/test/resources/ros2__geometry_msgs__services.yaml (100%) diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index ad494d1..4eba9f9 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -56,16 +56,22 @@ if(BUILD_LIBRARY) configure_file(${PROJECT_SOURCE_DIR}/config.hpp.in ${CMAKE_INSTALL_PREFIX}/include/is/sh/ros2/config.hpp ) +endif() - # Build ROS2 Static SH or ROS2 Dynamic SH - set(IS_ROS2_SH_MODE_LOWERCASE "" CACHE STRING "Build mode to lowercase") - string(TOLOWER "${IS_ROS2_SH_MODE}" IS_ROS2_SH_MODE_LOWERCASE) +# Build ROS2 Static SH or ROS2 Dynamic SH +set(IS_ROS2_SH_MODE_LOWERCASE "" CACHE STRING "Build mode to lowercase") +string(TOLOWER "${IS_ROS2_SH_MODE}" IS_ROS2_SH_MODE_LOWERCASE) - if("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "static") - add_subdirectory(static) - elseif("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "dynamic") - add_subdirectory(dynamic) - else() - message(WARNING "Invalid mode selected. Please choose between 'static' or 'dynamic'") +if("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "static") + add_subdirectory(static) + if(BUILD_TESTS OR BUILD_ROS2_TESTS) + enable_testing() endif() -endif() \ No newline at end of file +elseif("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "dynamic") + add_subdirectory(dynamic) + if(BUILD_TESTS OR BUILD_ROS2_TESTS) + enable_testing() + endif() +else() + message(WARNING "Invalid mode selected. Please choose between 'static' or 'dynamic'") +endif() diff --git a/ros2/dynamic/CMakeLists.txt b/ros2/dynamic/CMakeLists.txt index 3102493..58fa661 100644 --- a/ros2/dynamic/CMakeLists.txt +++ b/ros2/dynamic/CMakeLists.txt @@ -20,186 +20,183 @@ ################################################################################## cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) +################################################################################### +# Configure options +################################################################################### +option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) + ################################################################################### # Load external CMake Modules. ################################################################################### -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) +if(BUILD_LIBRARY) + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) -find_package(Sanitizers QUIET) + find_package(Sanitizers QUIET) -if(SANITIZE_ADDRESS) - message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") + if(SANITIZE_ADDRESS) + message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") + endif() endif() ################################################################################### # External dependencies for the Integration Service ROS 2 SystemHandle library ################################################################################### -# Check if the user specified which ROS 2 version he/she is using. If not, warn and try to automatically get it -if(NOT IS_ROS2_DISTRO) - - message(WARNING " - The variable 'IS_ROS2_DISTRO' was not used. You might want to set it to - specify which ROS 2 version should be used to compile the ROS 2 System Handle. - By default, a ROS 2 version from the sourced environment will be retrieved automatically.") - - if ("$ENV{ROS_VERSION}" STREQUAL "1") - message(FATAL_ERROR " - A ROS 1 version was sourced last in your build environment. - Please use the 'IS_ROS2_VERSION' variable!") - else() - set(IS_ROS2_DISTRO $ENV{ROS_DISTRO}) - endif() - +if(BUILD_LIBRARY) + find_package(is-core REQUIRED) + find_package(fastrtps REQUIRED) endif() -message(STATUS "Compiling the ${PROJECT_NAME} System Handle for ROS 2 ${IS_ROS2_DISTRO}...") - -find_package(is-core REQUIRED) -find_package(fastrtps REQUIRED) - ################################################################################### # Configure the Integration Service ROS 2 SystemHandle library ################################################################################### -add_library(${PROJECT_NAME} - SHARED - src/Conversion.cpp - src/Participant.cpp - src/Publisher.cpp - src/Subscriber.cpp - src/SystemHandle.cpp - #$,src/SystemHandle__foxy.cpp,src/SystemHandle.cpp> -) - -if (Sanitizers_FOUND) - add_sanitizers(${PROJECT_NAME}) -endif() - -set_target_properties(${PROJECT_NAME} PROPERTIES - VERSION - ${PROJECT_VERSION} - SOVERSION - ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} - CXX_STANDARD - 17 - CXX_STANDARD_REQUIRED - YES - ) - -target_compile_options(${PROJECT_NAME} - PRIVATE - $<$:-pedantic> - $<$:-fstrict-aliasing> - $<$:-Wall> - $<$:-Wextra> - $<$:-Wcast-align> - $<$:-Wshadow> - $<$:/W4> - $<$:/wd4700> - $<$:/wd4996> - $<$:/wd4820> - $<$:/wd4255> - $<$:/wd4668> +if(BUILD_LIBRARY) + add_library(${PROJECT_NAME} + SHARED + src/Conversion.cpp + src/Participant.cpp + src/Publisher.cpp + src/Subscriber.cpp + src/SystemHandle.cpp ) -include(GNUInstallDirs) -message(STATUS "Configuring [${PROJECT_NAME}]...") - -target_link_libraries(${PROJECT_NAME} - PUBLIC - is::core - fastrtps - ) - -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ - $ - $ - ) + if (Sanitizers_FOUND) + add_sanitizers(${PROJECT_NAME}) + endif() + + set_target_properties(${PROJECT_NAME} PROPERTIES + VERSION + ${PROJECT_VERSION} + SOVERSION + ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + CXX_STANDARD + 17 + CXX_STANDARD_REQUIRED + YES + ) + + target_compile_options(${PROJECT_NAME} + PRIVATE + $<$:-pedantic> + $<$:-fstrict-aliasing> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wcast-align> + $<$:-Wshadow> + $<$:/W4> + $<$:/wd4700> + $<$:/wd4996> + $<$:/wd4820> + $<$:/wd4255> + $<$:/wd4668> + ) + + include(GNUInstallDirs) + message(STATUS "Configuring [${PROJECT_NAME}]...") + + target_link_libraries(${PROJECT_NAME} + PUBLIC + is::core + fastrtps + ) + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + $ + ) +endif() ################################################################################### # Install the Integration Service ROS 2 SystemHandle library ################################################################################### -is_install_middleware_plugin( - MIDDLEWARE - ros2_dynamic - TARGET - ${PROJECT_NAME} +if(BUILD_LIBRARY) + is_install_middleware_plugin( + MIDDLEWARE + ros2_dynamic + TARGET + ${PROJECT_NAME} + ) + + file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/include/ + DESTINATION + ${CMAKE_INSTALL_PREFIX}/include/ ) -file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/include/ - DESTINATION - ${CMAKE_INSTALL_PREFIX}/include/ -) - -file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/resources/ - DESTINATION - /tmp/ -) + file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/resources/ + DESTINATION + /tmp/ + ) +endif() ################################################################################### # Integration Service ROS 2 SystemHandle tests ################################################################################### -if(BUILD_TESTS OR BUILD_ROS2_TESTS) - add_subdirectory(test) +if(BUILD_LIBRARY) + if(BUILD_TESTS OR BUILD_ROS2_TESTS) + enable_testing() + add_subdirectory(test) + endif() endif() ################################################################################### # Integration Service ROS 2 SystemHandle API Reference ################################################################################### -# if(BUILD_API_REFERENCE) -# if(NOT BUILD_LIBRARY) -# find_file(IS_DOXYGEN_CONFIG_FILE doxygen-config.in PATHS ${CMAKE_INSTALL_PREFIX}/../is-core) +# if(BUILD_LIBRARY) +# if(BUILD_API_REFERENCE) +# if(NOT BUILD_LIBRARY) +# find_file(IS_DOXYGEN_CONFIG_FILE doxygen-config.in PATHS ${CMAKE_INSTALL_PREFIX}/../is-core) +# endif() +# find_package(Doxygen REQUIRED) +# # Create doxygen directories +# add_custom_target(doc-dirs +# COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen +# COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen/html +# COMMENT "Creating documentation directories" VERBATIM) + +# set(IS_ROS2_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/include/is/sh/ros2") +# set(IS_ROS2_SOURCE_DIR "${CMAKE_SOURCE_DIR}/src") +# file(GLOB_RECURSE HPP_FILES +# "${IS_ROS2_INCLUDE_DIR}/*.h*" +# "${IS_ROS2_SOURCE_DIR}/*.h*") + +# # Doxygen related variables +# set(DOXYGEN_INPUT_DIR "${IS_ROS2_INCLUDE_DIR} ${IS_ROS2_SOURCE_DIR}") +# set(DOXYGEN_OUTPUT_DIR "${PROJECT_BINARY_DIR}/doxygen") +# set(DOXYGEN_INDEX_FILE "${PROJECT_BINARY_DIR}/doxygen/xml/index.xml") +# set(DOXYFILE_IN "${IS_DOXYGEN_CONFIG_FILE}") +# set(DOXYFILE_OUT ${PROJECT_BINARY_DIR}/doxygen-config) +# set(DOXYGEN_TAGFILES "../is-core/is_core.tag=../is-core/doxygen/html") +# set(DOXYGEN_HTML_DIR "${PROJECT_BINARY_DIR}/doxygen/html") + +# # Configure doxygen +# configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) + +# # Doxygen command +# add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} +# COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} +# DEPENDS ${HPP_FILES} +# MAIN_DEPENDENCY ${DOXYFILE_OUT} ${DOXYFILE_IN} +# COMMENT "Generating doxygen documentation") + +# # Generate API reference +# add_custom_target(doxygen-${PROJECT_NAME} ALL +# DEPENDS ${DOXYGEN_INDEX_FILE} +# COMMENT "Generated API documentation with doxygen" VERBATIM) +# add_dependencies(doxygen-${PROJECT_NAME} doc-dirs) + +# # Install doxygen generated XML files +# install(DIRECTORY ${PROJECT_BINARY_DIR}/doxygen/xml +# DESTINATION doxygen +# COMPONENT doxygen-${PROJECT_NAME}) +# set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "ROS2 System Handle doxygen") +# set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION +# "eProsima ROS2 System Handle doxygen documentation") +# set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) # endif() -# find_package(Doxygen REQUIRED) -# # Create doxygen directories -# add_custom_target(doc-dirs -# COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen -# COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen/html -# COMMENT "Creating documentation directories" VERBATIM) - -# set(IS_ROS2_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/include/is/sh/ros2") -# set(IS_ROS2_SOURCE_DIR "${CMAKE_SOURCE_DIR}/src") -# file(GLOB_RECURSE HPP_FILES -# "${IS_ROS2_INCLUDE_DIR}/*.h*" -# "${IS_ROS2_SOURCE_DIR}/*.h*") - -# # Doxygen related variables -# set(DOXYGEN_INPUT_DIR "${IS_ROS2_INCLUDE_DIR} ${IS_ROS2_SOURCE_DIR}") -# set(DOXYGEN_OUTPUT_DIR "${PROJECT_BINARY_DIR}/doxygen") -# set(DOXYGEN_INDEX_FILE "${PROJECT_BINARY_DIR}/doxygen/xml/index.xml") -# set(DOXYFILE_IN "${IS_DOXYGEN_CONFIG_FILE}") -# set(DOXYFILE_OUT ${PROJECT_BINARY_DIR}/doxygen-config) -# set(DOXYGEN_TAGFILES "../is-core/is_core.tag=../is-core/doxygen/html") -# set(DOXYGEN_HTML_DIR "${PROJECT_BINARY_DIR}/doxygen/html") - -# # Configure doxygen -# configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) - -# # Doxygen command -# add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} -# COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} -# DEPENDS ${HPP_FILES} -# MAIN_DEPENDENCY ${DOXYFILE_OUT} ${DOXYFILE_IN} -# COMMENT "Generating doxygen documentation") - -# # Generate API reference -# add_custom_target(doxygen-${PROJECT_NAME} ALL -# DEPENDS ${DOXYGEN_INDEX_FILE} -# COMMENT "Generated API documentation with doxygen" VERBATIM) -# add_dependencies(doxygen-${PROJECT_NAME} doc-dirs) - -# # Install doxygen generated XML files -# install(DIRECTORY ${PROJECT_BINARY_DIR}/doxygen/xml -# DESTINATION doxygen -# COMPONENT doxygen-${PROJECT_NAME}) -# set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "ROS2 System Handle doxygen") -# set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION -# "eProsima ROS2 System Handle doxygen documentation") -# set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) # endif() \ No newline at end of file diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index 1a8844e..907e89b 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -211,46 +211,47 @@ class SystemHandle : public virtual FullSystem const YAML::Node& configuration, TypeRegistry& type_registry) { - logger_ << utils::Logger::Level::INFO << "Using node " << Dump(configuration) << std::endl; std::map, std::string> type_paths; //, Path + logger_ << utils::Logger::Level::INFO + << "Adding types to Type Registry for ROS 2 Dynamic SystemHandle" << std::endl; + for (auto& conf : configuration) { std::string type = conf.as(); - logger_ << utils::Logger::Level::INFO << "TYPE: " << type << std::endl; std::size_t found = type.find("/"); // The type introduced is a package. Register all the types inside that package if (found == std::string::npos) { - logger_ << utils::Logger::Level::INFO << "It is a package" << std::endl; std::string path = "/opt/ros/" + ROS2_DISTRO + "/share/" + type + "/msg"; for (const auto & entry : std::filesystem::directory_iterator(path)) { if (std::filesystem::is_regular_file(entry) && entry.path().extension() == ".idl") { - logger_ << utils::Logger::Level::INFO << "Entry in the path " << path - << " : " << entry.path() << std::endl; + logger_ << utils::Logger::Level::DEBUG << "The ROS 2 package '" + << type << "' was added to the type registry." << std::endl; type_paths.emplace(std::make_pair(type, entry.path().stem().string()), entry.path().string()); } } } else { - logger_ << utils::Logger::Level::INFO << "It is a typename" << std::endl; std::string pkg = type.substr(0, found); std::string type_name = type.substr(found + 1); std::string path = "/opt/ros/" + ROS2_DISTRO + "/share/" + pkg + "/msg/" + type_name + ".idl"; - logger_ << utils::Logger::Level::INFO << "Package " << pkg << " Type " << type_name << std::endl; if (std::filesystem::exists(path)) { + logger_ << utils::Logger::Level::DEBUG << "The ROS 2 message '" + << type << "' was added to the type registry." << std::endl; type_paths.emplace(std::make_pair(pkg, type_name), path); } else { - logger_ << utils::Logger::Level::INFO << "The type '" << type_name << "' doesn't exists within the " - << "package '" << pkg << "'" << std::endl; + logger_ << utils::Logger::Level::WARN << "The type '" << type_name + << "' doesn't exists within the package '" << pkg + << "'. It will be ignored." << std::endl; } } } diff --git a/ros2/dynamic/test/CMakeLists.txt b/ros2/dynamic/test/CMakeLists.txt index cf47d55..374430a 100644 --- a/ros2/dynamic/test/CMakeLists.txt +++ b/ros2/dynamic/test/CMakeLists.txt @@ -23,11 +23,11 @@ cmake_minimum_required(VERSION 3.5.0) # Get Integration Service dependencies find_package(is-mock REQUIRED) +find_package(rclcpp REQUIRED) # Get message dependencies -# find_package(geometry_msgs REQUIRED) -# find_package(nav_msgs REQUIRED) -# find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) macro(compile_test) # Parse arguments @@ -44,19 +44,17 @@ macro(compile_test) target_link_libraries(${TEST_NAME} PUBLIC is::mock - # ${rclcpp_LIBRARIES} - # ${geometry_msgs_LIBRARIES} - # ${nav_msgs_LIBRARIES} - # ${std_msgs_LIBRARIES} + ${rclcpp_LIBRARIES} + ${geometry_msgs_LIBRARIES} + ${std_msgs_LIBRARIES} PRIVATE $,libgtest,gtest> ) target_include_directories(${TEST_NAME} PRIVATE - # ${geometry_msgs_INCLUDE_DIRS} - # ${nav_msgs_INCLUDE_DIRS} - # ${std_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} ) target_compile_options(${TEST_NAME} @@ -77,24 +75,23 @@ include(CTest) include(${IS_GTEST_CMAKE_MODULE_DIR}/gtest.cmake) enable_testing() -# compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msgs.cpp) -# compile_test(${PROJECT_NAME}_primitive_msgs SOURCE integration/ros2__primitives_msgs.cpp) +compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msgs.cpp) compile_test(${PROJECT_NAME}_check_ros2pkg_creation SOURCE integration/check_ros2pkg_creation.cpp) -#if ("${IS_ROS2_DISTRO}" STREQUAL "foxy") -# compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) -#else() -# compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain.cpp) -#endif() +if ("${IS_ROS2_DISTRO}" STREQUAL "foxy") + compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) +else() + compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain.cpp) +endif() set(test_msgs_config_file "ros2__test_msgs.yaml") set_property( - # TARGET ${PROJECT_NAME}_geometry_msgs ${PROJECT_NAME}_test_domain ${PROJECT_NAME}_primitive_msgs - TARGET ${PROJECT_NAME}_check_ros2pkg_creation + TARGET + ${PROJECT_NAME}_check_ros2pkg_creation + ${PROJECT_NAME}_geometry_msgs + ${PROJECT_NAME}_test_domain APPEND PROPERTY COMPILE_DEFINITIONS PRIVATE - # "ROS2__GEOMETRY_MSGS__TEST_CONFIG=\"${CMAKE_CURRENT_LIST_DIR}/resources/ros2__geometry_msgs.yaml\"" - # "ROS2__TEST_DOMAIN__TEST_CONFIG=\"${CMAKE_CURRENT_LIST_DIR}/resources/ros2__domain_change.yaml\"" "ROS2__ROSIDL__BUILD_DIR=\"${CMAKE_BINARY_DIR}/is/rosidl/ros2/lib\"" ) diff --git a/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp b/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp new file mode 100644 index 0000000..f21c011 --- /dev/null +++ b/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp @@ -0,0 +1,352 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2020 - present 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 + +// TODO (@jamoralp): re-think or refactor these tests. + +namespace is = eprosima::is; +namespace xtypes = eprosima::xtypes; + +static is::utils::Logger logger("is::sh::ROS2::test::geometry_msgs"); + +const std::string print_pose( + const geometry_msgs::msg::PoseStamped& ros2_pose) +{ + std::ostringstream oss; + + oss << "{ position: {" + << " x: " << ros2_pose.pose.position.x << "," + << " y: " << ros2_pose.pose.position.y << "," + << " z: " << ros2_pose.pose.position.z + << " }, orientation: { " + << " w: " << ros2_pose.pose.orientation.w << "," + << " x: " << ros2_pose.pose.orientation.x << "," + << " y: " << ros2_pose.pose.orientation.y << "," + << " z: " << ros2_pose.pose.orientation.z + << " } }"; + + return oss.str(); +} + +geometry_msgs::msg::PoseStamped generate_random_pose( + const int sec = 0) +{ + std::mt19937 rng; + // Use a fixed seed for deterministic test results + rng.seed(39); + std::uniform_real_distribution dist(-100.0, 100.0); + + geometry_msgs::msg::PoseStamped ros2_pose; + + ros2_pose.pose.position.x = dist(rng); + ros2_pose.pose.position.y = dist(rng); + ros2_pose.pose.position.z = dist(rng); + + ros2_pose.pose.orientation.w = 1.0; + ros2_pose.pose.orientation.x = 0.0; + ros2_pose.pose.orientation.y = 0.0; + ros2_pose.pose.orientation.z = 0.0; + + ros2_pose.header.frame_id = "map"; + ros2_pose.header.stamp.sec = sec; + + logger << is::utils::Logger::Level::DEBUG + << "Generated random pose -> " << print_pose(ros2_pose) << std::endl; + + return ros2_pose; +} + +void transform_pose_msg( + const geometry_msgs::msg::PoseStamped& p, + xtypes::WritableDynamicDataRef to) +{ + to["header"]["stamp"]["sec"] = p.header.stamp.sec; + to["header"]["stamp"]["nanosec"] = p.header.stamp.nanosec; + to["header"]["frame_id"] = "map"; + to["pose"]["position"]["x"] = p.pose.position.x; + to["pose"]["position"]["y"] = p.pose.position.y; + to["pose"]["position"]["z"] = p.pose.position.z; + to["pose"]["orientation"]["x"] = p.pose.orientation.x; + to["pose"]["orientation"]["y"] = p.pose.orientation.y; + to["pose"]["orientation"]["z"] = p.pose.orientation.z; + to["pose"]["orientation"]["w"] = p.pose.orientation.w; +} + +xtypes::DynamicData generate_plan_request_msg( + const xtypes::DynamicType& request_type, + const geometry_msgs::msg::PoseStamped& start, + const geometry_msgs::msg::PoseStamped& goal, + const float tolerance = 1e-3f) +{ + xtypes::DynamicData message(request_type); + + transform_pose_msg(goal, message["goal"]); + transform_pose_msg(start, message["start"]); + message["tolerance"] = tolerance; + + return message; +} + +const std::string print_header( + const std_msgs::msg::Header& header) +{ + std::ostringstream oss; + + oss << "[stamp: " << header.stamp.sec << " | " << header.stamp.nanosec + << "] [frame_id: " << header.stamp.sec + "]"; + + return oss.str(); +} + +void compare_plans( + const nav_msgs::srv::GetPlan_Response::_plan_type& plan_a, + const nav_msgs::srv::GetPlan_Response::_plan_type& plan_b) +{ + const bool header_matches = (plan_a.header == plan_b.header); + EXPECT_TRUE(header_matches); + + if (!header_matches) + { + logger << is::utils::Logger::Level::WARN + << "[compare_plans] Headers did not match:\n\t -- Header A: " + << print_header(plan_a.header) << "\n\t -- Header B: " + << print_header(plan_b.header) << std::endl; + } + else + { + logger << is::utils::Logger::Level::DEBUG + << "[compare_plans] Headers A and B matched: " + << print_header(plan_a.header) << std::endl; + } + + ASSERT_EQ(plan_a.poses.size(), plan_b.poses.size()); + for (std::size_t i = 0; i < plan_a.poses.size(); ++i) + { + const bool pose_matches = (plan_a.poses[i] == plan_b.poses[i]); + EXPECT_TRUE(pose_matches); + + if (!pose_matches) + { + logger << is::utils::Logger::Level::WARN + << "[compare_plans] Poses at index [" << i << "]" + << " did not match. Pose A is: " << print_pose(plan_a.poses[i]) + << ", and pose B is: " << print_pose(plan_b.poses[i]) << std::endl; + } + else + { + logger << is::utils::Logger::Level::DEBUG + << "[compare_plans] Poses A and B matched: " + << print_pose(plan_a.poses[i]) << std::endl; + } + } +} + +TEST(ROS2Dynamic, Publish_subscribe_between_ros2_and_mock) +{ + using namespace std::chrono_literals; + + const double tolerance = 1e-8; + + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [geometry_msgs/Pose] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit_pose: { type: 'geometry_msgs::msg::Pose', route: ros2_to_mock }\n"; + yaml += " echo_pose: { type: 'geometry_msgs::msg::Pose', route: mock_to_ros2 }\n"; + + std::cout << "YAML " << yaml << std::endl; + + YAML::Node config_node = YAML::Load(yaml); + + is::core::InstanceHandle handle = is::run_instance( + config_node, {ROS2__ROSIDL__BUILD_DIR}); + + ASSERT_TRUE(handle); + + char const* const argv[1] = {"is_ros2_test_geometry_msgs"}; + rclcpp::init(1, argv); + + rclcpp::Node::SharedPtr ros2 = std::make_shared("ros2_test"); + rclcpp::executors::SingleThreadedExecutor executor; + + ASSERT_TRUE( rclcpp::ok() ); + + executor.add_node(ros2); + + const auto publisher = +#ifndef RCLCPP__QOS_HPP_ + ros2->create_publisher("transmit_pose"); +#else + ros2->create_publisher( + "transmit_pose", rclcpp::SystemDefaultsQoS()); +#endif // RCLCPP__QOS_HPP_ + ASSERT_TRUE(publisher); + + std::promise msg_promise; + std::future msg_future = msg_promise.get_future(); + std::mutex mock_sub_mutex; + bool mock_sub_value_received = false; + auto mock_sub = [&](const xtypes::DynamicData& msg) + { + std::unique_lock lock(mock_sub_mutex); + if (mock_sub_value_received) + { + return; + } + + mock_sub_value_received = true; + msg_promise.set_value(msg); + }; + ASSERT_TRUE(is::sh::mock::subscribe("transmit_pose", mock_sub)); + + geometry_msgs::msg::Pose ros2_pose = generate_random_pose().pose; + + publisher->publish(ros2_pose); + + executor.spin_some(); + + // Wait no longer than a few seconds for the message to arrive. If it's not + // ready by that time, then something is probably broken with the test, and + // we should quit instead of waiting for the future and potentially hanging + // forever. + auto start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < 30s) + { + executor.spin_some(); + if (msg_future.wait_for(100ms) == std::future_status::ready) + { + break; + } + + publisher->publish(ros2_pose); + } + + ASSERT_EQ(msg_future.wait_for(0s), std::future_status::ready); + xtypes::DynamicData received_msg = msg_future.get(); + + EXPECT_EQ(received_msg.type().name(), "geometry_msgs::msg::Pose"); + + xtypes::ReadableDynamicDataRef position = received_msg["position"]; + xtypes::ReadableDynamicDataRef orientation = received_msg["orientation"]; + + #define TEST_POSITION_OF( u ) \ + { \ + const double u = position[#u]; \ + ASSERT_NEAR(u, ros2_pose.position.u, tolerance); \ + } + + TEST_POSITION_OF(x); + TEST_POSITION_OF(y); + TEST_POSITION_OF(z); + + bool promise_sent = false; + std::promise pose_promise; + auto pose_future = pose_promise.get_future(); + std::mutex echo_mutex; + auto echo_sub = [&](geometry_msgs::msg::Pose::UniquePtr msg) + { + std::unique_lock lock(echo_mutex); + + // promises will throw an exception if set_value(~) is called more than + // once, so we'll guard against that. + if (promise_sent) + { + return; + } + + promise_sent = true; + pose_promise.set_value(*msg); + }; + +#ifndef RCLCPP__QOS_HPP_ + const auto subscriber = ros2->create_subscription( + "echo_pose", echo_sub); +#else + const auto subscriber = ros2->create_subscription( + "echo_pose", rclcpp::SystemDefaultsQoS(), echo_sub); +#endif // RCLCPP__QOS_HPP_ + ASSERT_TRUE(subscriber); + + // Keep spinning and publishing while we wait for the promise to be + // delivered. Try to cycle this for no more than a few seconds. If it's not + // finished by that time, then something is probably broken with the test or + // with Integratoion Service, and we should quit instead of waiting for the future and + // potentially hanging forever. + start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < 30s) + { + executor.spin_some(); + + is::sh::mock::publish_message("echo_pose", received_msg); + + executor.spin_some(); + if (pose_future.wait_for(100ms) == std::future_status::ready) + { + break; + } + } + + ASSERT_EQ(pose_future.wait_for(0s), std::future_status::ready); + geometry_msgs::msg::Pose received_pose = pose_future.get(); + + EXPECT_EQ(ros2_pose, received_pose); + + // Destroy ros2 instance node + executor.remove_node(ros2); + ros2.reset(); + + // Quit and wait for no more than a minute. We don't want the test to get + // hung here indefinitely in the case of an error. + handle.quit().wait_for(1min); + + // Require that it's no longer running. If it is still running, then it is + // probably stuck, and we should forcefully quit. + ASSERT_TRUE(!handle.running()); + ASSERT_TRUE(handle.wait() == 0); + + rclcpp::shutdown(); +} + +int main( + int argc, + char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros2/dynamic/test/integration/ros2__test_domain.cpp b/ros2/dynamic/test/integration/ros2__test_domain.cpp new file mode 100644 index 0000000..be72075 --- /dev/null +++ b/ros2/dynamic/test/integration/ros2__test_domain.cpp @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2020 - present 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 + +namespace is = eprosima::is; +namespace xtypes = eprosima::xtypes; + +static is::utils::Logger logger("is::sh::ROS2::test::test_domain"); + +constexpr const size_t DOMAIN_ID_1 = 5; +constexpr const size_t DOMAIN_ID_2 = 10; + +TEST(ROS2, Change_ROS2_Domain_id__ROS2_Galactic_or_older) +{ + char const* const argv[1] = {"is_ros2_test_domain_id"}; + if (!rclcpp::ok()) + { + rclcpp::init(1, argv); + } + ASSERT_TRUE(rclcpp::ok()); + + // Create the nodes in separate context, since in ROS2 Galactic each context uses + // an unique DDS participant and thus creating the two nodes in the same context + // would result in them having the same DOMAIN_ID. + + rclcpp::InitOptions init_options_1; + if (rcl_logging_rosout_enabled()) + { + init_options_1.auto_initialize_logging(false); + } + init_options_1.set_domain_id(DOMAIN_ID_1); + + const char* const argv_1[1] = {"is_ros2_test_domain_id_context_1"}; + auto context_1 = std::make_shared(); + context_1->init(1, argv_1, init_options_1); + + rclcpp::NodeOptions node_ops_1; + node_ops_1.context(context_1); + + auto node_1 = std::make_shared("node_1", node_ops_1); + + + rclcpp::InitOptions init_options_2; + if (rcl_logging_rosout_enabled()) + { + init_options_2.auto_initialize_logging(false); + } + init_options_2.set_domain_id(DOMAIN_ID_2); + + const char* const argv_2[1] = {"is_ros2_test_domain_id_context_2"}; + auto context_2 = std::make_shared(); + context_2->init(1, argv_2, init_options_2); + + rclcpp::NodeOptions node_ops_2; + node_ops_2.context(context_2); + + auto node_2 = std::make_shared("node_2", node_ops_2); + + // Check correct DOMAIN ID for node_1 and node_2 + ASSERT_EQ(init_options_1.get_domain_id(), DOMAIN_ID_1); + logger << is::utils::Logger::Level::INFO + << "Domain ID for 'node_1': " << init_options_1.get_domain_id() << std::endl; + + ASSERT_EQ(init_options_2.get_domain_id(), DOMAIN_ID_2); + logger << is::utils::Logger::Level::INFO + << "Domain ID for 'node_2': " << init_options_2.get_domain_id() << std::endl; + + const std::string topic_name("string_topic"); + +#ifdef RCLCPP__QOS_HPP_ + const auto publisher = + node_1->create_publisher(topic_name, rclcpp::SystemDefaultsQoS()); +#else + const auto publisher = + node_1->create_publisher(topic_name); +#endif // ifdef RCLCPP__QOS_HPP_ + + std::promise msg_promise; + std::future msg_future = msg_promise.get_future(); + std::mutex node2_sub_mutex; + auto node2_sub = [&](std_msgs::msg::String::UniquePtr msg) + { + std::unique_lock lock(node2_sub_mutex); + msg_promise.set_value(*msg); + }; + +#ifdef RCLCPP__QOS_HPP_ + const auto subscriber = node_2->create_subscription( + topic_name, rclcpp::SystemDefaultsQoS(), node2_sub); +#else + const auto subscriber = node_2->create_subscription( + topic_name, node2_sub); +#endif // ifdef RCLCPP__QOS_HPP_ + + std_msgs::msg::String pub_msg; + pub_msg.set__data("Hello node"); + + rclcpp::executors::SingleThreadedExecutor executor; + using namespace std::chrono_literals; + + auto rclcpp_delay = 500ms; + publisher->publish(pub_msg); + executor.spin_node_some(node_1); + std::this_thread::sleep_for(rclcpp_delay); + executor.spin_node_some(node_2); + + // In different domains the message should not be received + ASSERT_NE(msg_future.wait_for(0s), std::future_status::ready); + + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2_domain5: { type: ros2_dynamic, domain: 5, node_name: 'is_node_5', using: [std_msgs/String] } \n"; + yaml += " ros2_domain10: { type: ros2_dynamic, domain: 10, node_name: 'is_node_10', using: [std_msgs/String] } \n"; + yaml += "routes:\n"; + yaml += " domain_5_to_10: { from: ros2_domain5, to: ros2_domain10 }\n"; + yaml += "topics:\n"; + yaml += " string_topic: { type: 'std_msgs::msg::String', route: domain_5_to_10 }\n"; + + std::cout << "YAML " << yaml << std::endl; + + YAML::Node config_node = YAML::Load(yaml); + + is::core::InstanceHandle handle = is::run_instance( + config_node, { ROS2__ROSIDL__BUILD_DIR }); + + ASSERT_TRUE(handle); + + // Wait for the Integration Service to start properly before publishing. + std::this_thread::sleep_for(1s); + publisher->publish(pub_msg); + executor.spin_node_some(node_1); + std::this_thread::sleep_for(rclcpp_delay); + executor.spin_node_some(node_2); + + ASSERT_EQ(msg_future.wait_for(0s), std::future_status::ready); + + std_msgs::msg::String received_msg = msg_future.get(); + + ASSERT_EQ(pub_msg, received_msg); +} + +int main( + int argc, + char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros2/dynamic/test/integration/ros2__test_domain__foxy.cpp b/ros2/dynamic/test/integration/ros2__test_domain__foxy.cpp new file mode 100644 index 0000000..eb95204 --- /dev/null +++ b/ros2/dynamic/test/integration/ros2__test_domain__foxy.cpp @@ -0,0 +1,209 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2020 - present 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 + +namespace is = eprosima::is; +namespace xtypes = eprosima::xtypes; + +static is::utils::Logger logger("is::sh::ROS2::test::test_domain"); + +constexpr const char* DOMAIN_ID_1 = "5"; +constexpr const char* DOMAIN_ID_2 = "10"; + +#ifdef WIN32 +#define SETENV(id, value, b) \ + {std::ostringstream _aux_d; \ + _aux_d << id << "=" << value; \ + _putenv(_aux_d.str().c_str());} +#define UNSETENV(id, retValue) SETENV(id, "", false) +#else +#define SETENV(id, value, b) setenv(id, value, b) +#define UNSETENV(id) unsetenv(id) +#endif // ifdef WIN32 + +TEST(ROS2, Change_ROS2_Domain_id__ROS2_Foxy) +{ + char const* const argv[1] = {"is_ros2_test_domain_id"}; + if (!rclcpp::ok()) + { + rclcpp::init(1, argv); + } + ASSERT_TRUE(rclcpp::ok()); + + // Create the nodes in separate context, since in ROS2 Foxy each context uses + // an unique DDS participant and thus creating the two nodes in the same context + // would result in them having the same DOMAIN_ID. + SETENV("ROS_DOMAIN_ID", DOMAIN_ID_1, true); + + rclcpp::InitOptions init_options_1; + if (rcl_logging_rosout_enabled()) + { + init_options_1.auto_initialize_logging(false); + } + + const char* const argv_1[1] = {"is_ros2_test_domain_id_context_1"}; + auto context_1 = std::make_shared(); + context_1->init(1, argv_1, init_options_1); + + rclcpp::NodeOptions node_ops_1; + node_ops_1.context(context_1); + + // This needs to be called so that NodeOptions::node_options_ pointer gets filled. + auto rcl_node_ops_1 = node_ops_1.get_rcl_node_options(); + + auto node_1 = std::make_shared("node_1", node_ops_1); + + UNSETENV("ROS_DOMAIN_ID"); + + SETENV("ROS_DOMAIN_ID", DOMAIN_ID_2, true); + + rclcpp::InitOptions init_options_2; + if (rcl_logging_rosout_enabled()) + { + init_options_2.auto_initialize_logging(false); + } + + const char* const argv_2[1] = {"is_ros2_test_domain_id_context_2"}; + auto context_2 = std::make_shared(); + context_2->init(1, argv_2, init_options_2); + + rclcpp::NodeOptions node_ops_2; + node_ops_2.context(context_2); + + // This needs to be called so that NodeOptions::node_options_ pointer gets filled. + auto rcl_node_ops_2 = node_ops_2.get_rcl_node_options(); + + auto node_2 = std::make_shared("node_2", node_ops_2); + + UNSETENV("ROS_DOMAIN_ID"); + + // Check correct DOMAIN ID for node_1 and node_2 +# define CHECK_DOMAIN_ID(NODE_OPS, DOMAIN_ID) \ + { \ + std::stringstream ss(DOMAIN_ID); \ + size_t domain_id; \ + ss >> domain_id; \ + ASSERT_EQ(NODE_OPS->domain_id, domain_id); \ + } + + CHECK_DOMAIN_ID(rcl_node_ops_1, DOMAIN_ID_1); + logger << is::utils::Logger::Level::INFO + << "Domain ID for 'node_1': " << rcl_node_ops_1->domain_id << std::endl; + + CHECK_DOMAIN_ID(rcl_node_ops_2, DOMAIN_ID_2); + logger << is::utils::Logger::Level::INFO + << "Domain ID for 'node_2': " << rcl_node_ops_2->domain_id << std::endl; + + const std::string topic_name("string_topic"); + +#ifdef RCLCPP__QOS_HPP_ + const auto publisher = + node_1->create_publisher(topic_name, rclcpp::SystemDefaultsQoS()); +#else + const auto publisher = + node_1->create_publisher(topic_name); +#endif // ifdef RCLCPP__QOS_HPP_ + + std::promise msg_promise; + std::future msg_future = msg_promise.get_future(); + std::mutex node2_sub_mutex; + auto node2_sub = [&](std_msgs::msg::String::UniquePtr msg) + { + std::unique_lock lock(node2_sub_mutex); + msg_promise.set_value(*msg); + }; + +#ifdef RCLCPP__QOS_HPP_ + const auto subscriber = node_2->create_subscription( + topic_name, rclcpp::SystemDefaultsQoS(), node2_sub); +#else + const auto subscriber = node_2->create_subscription( + topic_name, node2_sub); +#endif // ifdef RCLCPP__QOS_HPP_ + + std_msgs::msg::String pub_msg; + pub_msg.set__data("Hello node"); + + rclcpp::executors::SingleThreadedExecutor executor; + using namespace std::chrono_literals; + + auto rclcpp_delay = 500ms; + publisher->publish(pub_msg); + executor.spin_node_some(node_1); + std::this_thread::sleep_for(rclcpp_delay); + executor.spin_node_some(node_2); + + // In different domains the message should not be received + ASSERT_NE(msg_future.wait_for(0s), std::future_status::ready); + + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2_domain5: { type: ros2_dynamic, domain: 5, node_name: 'is_node_5', using: [std_msgs/String] } \n"; + yaml += " ros2_domain10: { type: ros2_dynamic, domain: 10, node_name: 'is_node_10', using: [std_msgs/String] } \n"; + yaml += "routes:\n"; + yaml += " domain_5_to_10: { from: ros2_domain5, to: ros2_domain10 }\n"; + yaml += "topics:\n"; + yaml += " string_topic: { type: 'std_msgs::msg::String', route: domain_5_to_10 }\n"; + + std::cout << "YAML " << yaml << std::endl; + + YAML::Node config_node = YAML::Load(yaml); + + is::core::InstanceHandle handle = is::run_instance( + config_node, { ROS2__ROSIDL__BUILD_DIR }); + + ASSERT_TRUE(handle); + + // Wait for the Integration Service to start properly before publishing. + std::this_thread::sleep_for(1s); + publisher->publish(pub_msg); + executor.spin_node_some(node_1); + std::this_thread::sleep_for(rclcpp_delay); + executor.spin_node_some(node_2); + + ASSERT_EQ(msg_future.wait_for(0s), std::future_status::ready); + + std_msgs::msg::String received_msg = msg_future.get(); + + ASSERT_EQ(pub_msg, received_msg); +} + +int main( + int argc, + char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml b/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml index 000f794..84e9d3c 100644 --- a/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml +++ b/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml @@ -1,17 +1,11 @@ systems: - ros2: { type: ros2 } + ros2: { type: ros2_dynamic, using: [geometry_msgs/Pose] } mock: { type: mock, types-from: ros2 } routes: mock_to_ros2: { from: mock, to: ros2 } ros2_to_mock: { from: ros2, to: mock } - mock_srv: { server: mock, clients: ros2 } - ros2_srv: { server: ros2, clients: mock } topics: - transmit_pose: { type: "geometry_msgs/Pose", route: ros2_to_mock } - echo_pose: { type: "geometry_msgs/Pose", route: mock_to_ros2 } - -services: - get_plan: { type: "nav_msgs/GetPlan:request", route: ros2_srv } - echo_plan: { type: "nav_msgs/GetPlan:response", route: mock_srv } + transmit_pose: { type: "geometry_msgs::msg::Pose", route: ros2_to_mock } + echo_pose: { type: "geometry_msgs::msg::Pose", route: mock_to_ros2 } diff --git a/ros2/static/test/CMakeLists.txt b/ros2/static/test/CMakeLists.txt index 8bb8e39..b538df0 100644 --- a/ros2/static/test/CMakeLists.txt +++ b/ros2/static/test/CMakeLists.txt @@ -23,6 +23,7 @@ cmake_minimum_required(VERSION 3.5.0) # Get Integration Service dependencies find_package(is-mock REQUIRED) +find_package(rclcpp REQUIRED) # Get message dependencies find_package(geometry_msgs REQUIRED) diff --git a/ros2/test/resources/ros2__geometry_msgs__pubsub.yaml b/ros2/static/test/resources/ros2__geometry_msgs__pubsub.yaml similarity index 100% rename from ros2/test/resources/ros2__geometry_msgs__pubsub.yaml rename to ros2/static/test/resources/ros2__geometry_msgs__pubsub.yaml diff --git a/ros2/test/resources/ros2__geometry_msgs__services.yaml b/ros2/static/test/resources/ros2__geometry_msgs__services.yaml similarity index 100% rename from ros2/test/resources/ros2__geometry_msgs__services.yaml rename to ros2/static/test/resources/ros2__geometry_msgs__services.yaml From 16016a8b9438dd579a95b953ae7de8d1e4fd1240 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Mon, 21 Jun 2021 15:35:48 +0200 Subject: [PATCH 13/34] Fix ROS 2 Dynamic Tests for Galactic Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/src/Publisher.cpp | 5 ++-- ros2/dynamic/src/Subscriber.cpp | 5 ++-- ros2/dynamic/test/CMakeLists.txt | 8 +++--- .../integration/check_ros2pkg_creation.cpp | 10 +++++--- .../test/integration/ros2__test_domain.cpp | 25 +++++++++++++------ .../test/resources/ros2__geometry_msgs.yaml | 11 -------- 6 files changed, 35 insertions(+), 29 deletions(-) delete mode 100644 ros2/dynamic/test/resources/ros2__geometry_msgs.yaml diff --git a/ros2/dynamic/src/Publisher.cpp b/ros2/dynamic/src/Publisher.cpp index 81444ec..96d9c39 100644 --- a/ros2/dynamic/src/Publisher.cpp +++ b/ros2/dynamic/src/Publisher.cpp @@ -191,13 +191,14 @@ const fastrtps::rtps::InstanceHandle_t Publisher::get_dds_instance_handle() cons } void Publisher::on_publication_matched( - ::fastdds::dds::DataWriter* /*writer*/, + ::fastdds::dds::DataWriter* writer, const ::fastdds::dds::PublicationMatchedStatus& info) { if (1 == info.current_count_change) { logger_ << utils::Logger::Level::INFO - << "Publisher for topic '" << topic_name_ << "' matched" << std::endl; + << "Publisher for topic '" << topic_name_ << "' matched in domain " + << writer->get_publisher()->get_participant()->get_domain_id() << std::endl; } else if (-1 == info.current_count_change) { diff --git a/ros2/dynamic/src/Subscriber.cpp b/ros2/dynamic/src/Subscriber.cpp index 5a968a5..87baf03 100644 --- a/ros2/dynamic/src/Subscriber.cpp +++ b/ros2/dynamic/src/Subscriber.cpp @@ -256,13 +256,14 @@ void Subscriber::on_data_available( } void Subscriber::on_subscription_matched( - ::fastdds::dds::DataReader* /*reader*/, + ::fastdds::dds::DataReader* reader, const ::fastdds::dds::SubscriptionMatchedStatus& info) { if (1 == info.current_count_change) { logger_ << utils::Logger::Level::INFO - << "Subscriber for topic '" << topic_name_ << "' matched" << std::endl; + << "Subscriber for topic '" << topic_name_ << "' matched in domain " + << reader->get_subscriber()->get_participant()->get_domain_id() << std::endl; } else if (-1 == info.current_count_change) { diff --git a/ros2/dynamic/test/CMakeLists.txt b/ros2/dynamic/test/CMakeLists.txt index 374430a..02fd0fc 100644 --- a/ros2/dynamic/test/CMakeLists.txt +++ b/ros2/dynamic/test/CMakeLists.txt @@ -36,7 +36,7 @@ macro(compile_test) else() set(TEST_NAME "${ARGV0}") endif() - set(multiValueArgs SOURCE) + set(multiValueArgs SOURCE ENVIRONMENTS) cmake_parse_arguments(TEST "" "${uniValueArgs}" "${multiValueArgs}" ${ARGN}) add_executable(${TEST_NAME} ${TEST_SOURCE}) @@ -51,6 +51,8 @@ macro(compile_test) $,libgtest,gtest> ) + message(WARNING "INCLUDE PATH ${CMAKE_INSTALL_PREFIX}/include") + target_include_directories(${TEST_NAME} PRIVATE ${geometry_msgs_INCLUDE_DIRS} @@ -68,7 +70,7 @@ macro(compile_test) CXX_STANDARD 17 ) - add_gtest(${TEST_NAME} SOURCES ${TEST_SOURCE}) + add_gtest(${TEST_NAME} SOURCES ${TEST_SOURCE} ENVIRONMENTS ${TEST_ENVIRONMENTS}) endmacro() include(CTest) @@ -76,7 +78,7 @@ include(${IS_GTEST_CMAKE_MODULE_DIR}/gtest.cmake) enable_testing() compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msgs.cpp) -compile_test(${PROJECT_NAME}_check_ros2pkg_creation SOURCE integration/check_ros2pkg_creation.cpp) +compile_test(${PROJECT_NAME}_check_ros2pkg_creation SOURCE integration/check_ros2pkg_creation.cpp ENVIRONMENTS "ROS2_DISTRO=${IS_ROS2_DISTRO}") if ("${IS_ROS2_DISTRO}" STREQUAL "foxy") compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) diff --git a/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp b/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp index 79598bc..6f27350 100644 --- a/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp +++ b/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp @@ -21,9 +21,11 @@ #include #include +// #include + // TODO: resolve this include #include // TODO remove this workaround -#define ROS2_DISTRO "foxy" +// #define ROS2_DISTRO "foxy" #include #include @@ -39,6 +41,8 @@ using namespace std::chrono_literals; static is::utils::Logger logger("is::sh::ROS2_Dynamic::test::check_ros2pkg_creation"); +static const char* ros2_distro = std::getenv("ROS2_DISTRO"); + class ROS2Dynamic : public testing::Test { public: @@ -62,7 +66,7 @@ class ROS2Dynamic : public testing::Test yaml += " };\n"; yaml += " };\n"; yaml += " paths: [\"/opt/ros/"; - yaml += ROS2_DISTRO; + yaml += ros2_distro; yaml += "/share\"]\n"; yaml += "systems:\n"; yaml += " ros2: { type: ros2_dynamic } \n"; @@ -103,7 +107,7 @@ class ROS2Dynamic : public testing::Test logger << is::utils::Logger::Level::INFO << "Execute ROS2_Dynamic test" << std::endl; std::ostringstream command; - command << ". /opt/ros/" << ROS2_DISTRO << "/setup.sh && ros2 topic pub /test "; + command << ". /opt/ros/" << ros2_distro << "/setup.sh && ros2 topic pub /test "; command << "custom_msgs/msg/Message \"{text: {data: 'thisisatest'}}\" --once"; FILE* pipe = popen(command.str().c_str(), "r"); diff --git a/ros2/dynamic/test/integration/ros2__test_domain.cpp b/ros2/dynamic/test/integration/ros2__test_domain.cpp index be72075..2cb6e16 100644 --- a/ros2/dynamic/test/integration/ros2__test_domain.cpp +++ b/ros2/dynamic/test/integration/ros2__test_domain.cpp @@ -70,7 +70,6 @@ TEST(ROS2, Change_ROS2_Domain_id__ROS2_Galactic_or_older) auto node_1 = std::make_shared("node_1", node_ops_1); - rclcpp::InitOptions init_options_2; if (rcl_logging_rosout_enabled()) { @@ -112,6 +111,8 @@ TEST(ROS2, Change_ROS2_Domain_id__ROS2_Galactic_or_older) auto node2_sub = [&](std_msgs::msg::String::UniquePtr msg) { std::unique_lock lock(node2_sub_mutex); + logger << is::utils::Logger::Level::DEBUG + << "Setting value to promise" << std::endl; msg_promise.set_value(*msg); }; @@ -129,14 +130,14 @@ TEST(ROS2, Change_ROS2_Domain_id__ROS2_Galactic_or_older) rclcpp::executors::SingleThreadedExecutor executor; using namespace std::chrono_literals; - auto rclcpp_delay = 500ms; + auto rclcpp_delay = 1s; publisher->publish(pub_msg); executor.spin_node_some(node_1); std::this_thread::sleep_for(rclcpp_delay); executor.spin_node_some(node_2); // In different domains the message should not be received - ASSERT_NE(msg_future.wait_for(0s), std::future_status::ready); + ASSERT_NE(msg_future.wait_for(1s), std::future_status::ready); std::string yaml; @@ -158,11 +159,19 @@ TEST(ROS2, Change_ROS2_Domain_id__ROS2_Galactic_or_older) ASSERT_TRUE(handle); // Wait for the Integration Service to start properly before publishing. - std::this_thread::sleep_for(1s); - publisher->publish(pub_msg); - executor.spin_node_some(node_1); - std::this_thread::sleep_for(rclcpp_delay); - executor.spin_node_some(node_2); + auto start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < 30s) + { + executor.spin_node_some(node_1); + std::this_thread::sleep_for(rclcpp_delay); + executor.spin_node_some(node_2); + if (msg_future.wait_for(100ms) == std::future_status::ready) + { + break; + } + + publisher->publish(pub_msg); + } ASSERT_EQ(msg_future.wait_for(0s), std::future_status::ready); diff --git a/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml b/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml deleted file mode 100644 index 84e9d3c..0000000 --- a/ros2/dynamic/test/resources/ros2__geometry_msgs.yaml +++ /dev/null @@ -1,11 +0,0 @@ -systems: - ros2: { type: ros2_dynamic, using: [geometry_msgs/Pose] } - mock: { type: mock, types-from: ros2 } - -routes: - mock_to_ros2: { from: mock, to: ros2 } - ros2_to_mock: { from: ros2, to: mock } - -topics: - transmit_pose: { type: "geometry_msgs::msg::Pose", route: ros2_to_mock } - echo_pose: { type: "geometry_msgs::msg::Pose", route: mock_to_ros2 } From 976a4f9aba0bad513d649d8a19059f16f0819e5a Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Mon, 21 Jun 2021 16:18:52 +0200 Subject: [PATCH 14/34] Change outdated comment Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/src/SystemHandle.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index 907e89b..e462fcd 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -77,9 +77,8 @@ class SystemHandle : public virtual FullSystem TypeRegistry& type_registry) override { /* - * The ROS 2 Dynamic SH doesn't define new types. - * Needed types will be defined in the 'types' section of the YAML file, and hence, - * already registered in the 'TypeRegistry' by the *Integration Service core*. + * If any type different from the ROS 2 builtin ones want to be used, it need to + * be defined in the 'types' section of the YAML file. */ if (configuration["namespace"]) From f0e1cda3d3872859c3f510f1860fb479c2486793 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Tue, 22 Jun 2021 09:27:59 +0200 Subject: [PATCH 15/34] Update Readme Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- README.md | 59 +++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 46 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index c450b5b..6e164fa 100644 --- a/README.md +++ b/README.md @@ -23,22 +23,38 @@ implementation for the *xTypes* protocol, that is, [eProsima xTypes](https://git -This repository contains the source code of *Integration Service* **System Handle** +This repository contains the source code of *Integration Service* **System Handles** for the [ROS 2](https://docs.ros.org/en/foxy) middleware protocol, widely used in the robotics field. +Two implementations can be distinguished: **static** one based on *ROS 2* `rclcpp` and **dynamic** one based on *Fast DDS* (by the moment it is only available for the Publisher-Subscriber paradigm). -This *System Handle* can be used for two main purposes: +This *System Handles* can be used for two main purposes: 1. Connection between a *ROS 2* application and an application running over a different middleware implementation. This is the classic use-case approach for *Integration Service*. 1. Connecting two *ROS 2* applications running under different Domain IDs. +The main advantages of the *Dynamic ROS 2 System Handle* over the *Static ROS 2 System Handle* are the following ones: + +1. It allows using types defined by *IDL* without previously generating and installing the ROS 2 Type + Support for that type, which gives it a greater versatility with respect to the *Static ROS 2 System + Handle*. + +1. It is not necessary to decide which ROS 2 builtin types you are going to use during the compilation + phase, as it allows you to use any of them. + ## Dependencies -This section provides a list of the dependencies needed in order to compile *ROS 2 System Handle*. +This section provides a list of the dependencies needed in order to compile *ROS 2 System Handles*. + +**Static ROS 2 System Handle** * [ROS 2](https://docs.ros.org/en/foxy/Installation.html): *Foxy/Galactic ROS 2* distribution. +**Dynamic ROS 2 System Handle** + +* [Fast DDS](https://github.com/eProsima/Fast-DDS#installation-guide): eProsima C++ implementation for DDS. + ## Configuration *Integration Service* is configured by means of a YAML configuration file, which specifies @@ -51,7 +67,7 @@ To get a more precise idea on how these YAML files have to be filled and which f in order to succesfully configure and launch an *Integration Service* project, please refer to the [dedicated configuration section](https://integration-service.docs.eprosima.com/en/latest/user_manual/yaml_config.html) of the official documentation. -Regarding the *ROS 2 System Handle*, there are several specific parameters which can be configured +Regarding the *ROS 2 System Handles*, there are several specific parameters which can be configured for the ROS 2 middleware. All of these parameters are optional, and fall as suboptions of the main five sections described in the *Configuration* chapter of the *Integration Service* repository: @@ -118,18 +134,31 @@ five sections described in the *Configuration* chapter of the *Integration Servi * `reliability`: This QoS indicates the level of reliability offered and requested by the service. There are two possible values: `RELIABLE` and `BEST_EFFORT`. + The *Dynamic ROS 2 System Handle* has an additional parameter: + + ```yaml + systems: + ros2: + type: ros2_dynamic + namespace: "/" + node_name: "my_ros2_node" + domain: 4 + using: [std_msgs/String, geometry_msgs] + ``` + * `using`: List of the *ROS 2* builtin types or packages that want to be used in the communication. + ## Examples -There are several *Integration Service* examples using the *ROS 2 System Handle* available +There are several *Integration Service* examples using the *Static ROS 2 System Handle* available in the project's [main source code repository]([https://](https://github.com/eProsima/Integration-Service/tree/main/examples)). -Some of these examples, where the *ROS 2 System Handle* plays a different role in each of them, are introduced here. +Some of these examples, where the *Static ROS 2 System Handle* plays a different role in each of them, are introduced here. Green icon ### ROS 2 - ROS 1 bridge (publisher - subscriber) -In this example, *Integration Service* uses both this *ROS 2 System Handle* and the *ROS 1 System Handle* +In this example, *Integration Service* uses both this *Static ROS 2 System Handle* and the *ROS 1 System Handle* to transmit data coming from a ROS 2 publisher into the ROS 1 data space, so that it can be consumed by a ROS 1 subscriber on the same topic, and viceversa. @@ -147,7 +176,7 @@ For a detailed step by step guide on how to build and test this example, please ### ROS 2 - DDS bridge (publisher - subscriber) -In this example, *Integration Service* uses both this *ROS 2 System Handle* and the *Fast DDS System Handle* +In this example, *Integration Service* uses both this *Static ROS 2 System Handle* and the *Fast DDS System Handle* to transmit data coming from a ROS 2 publisher into the DDS data space, so that it can be consumed by a Fast DDS subscriber on the same topic, and viceversa. @@ -165,7 +194,7 @@ For a detailed step by step guide on how to build and test this example, please ### ROS 2 - WebSocket bridge (publisher - subscriber) -In this example, *Integration Service* uses both this *ROS 2 System Handle* and the *WebSocket System Handle* +In this example, *Integration Service* uses both this *Static ROS 2 System Handle* and the *WebSocket System Handle* to transmit data coming from a ROS 2 publisher to a WebSocket Client, and viceversa.

@@ -182,7 +211,7 @@ For a detailed step by step guide on how to build and test this example, please ### ROS 2 - FIWARE bridge (publisher - subscriber) -In this example, *Integration Service* uses both this *ROS 2 System Handle* and the *FIWARE System Handle* +In this example, *Integration Service* uses both this *Static ROS 2 System Handle* and the *FIWARE System Handle* to transmit data coming from a ROS 2 publisher and update them in a FIWARE Context Broker MongoDB database, and viceversa.

@@ -200,7 +229,7 @@ For a detailed step by step guide on how to build and test this example, please ### ROS 2 service server -In this example, the *ROS 2 System Handle* tackles the task of bridging a ROS 2 server with one or more client applications, +In this example, the *Static ROS 2 System Handle* tackles the task of bridging a ROS 2 server with one or more client applications, playing the role of a service server capable of processing incoming requests from several middlewares (*DDS*, *ROS1*, *WebSocket*) and producing an appropriate answer for them. @@ -254,7 +283,7 @@ For a detailed step by step guide on how to build and test this example, please Besides the [global compilation flags](https://integration-service.docs.eprosima.com/en/latest/installation_manual/installation.html#global-compilation-flags) available for the whole *Integration Service* product suite, there are some specific flags which apply only to the -*ROS 2 System Handle*; they are listed below: +*ROS 2 System Handles*; they are listed below: * `BUILD_ROS2_TESTS`: Allows to specifically compile the *ROS 2 System Handle* unitary and integration tests; this is useful to avoid compiling each *System Handle's* test suite present @@ -269,6 +298,10 @@ whole *Integration Service* product suite, there are some specific flags which a If not set, the version will be retrieved from the last *ROS distro* sourced in the compilation environment; this means that if the last *ROS* environment sourced corresponds to *ROS 1*, the compilation process will stop and warn the user about it. +* `IS_ROS2_SH_MODE`: This flag is to decide which *ROS 2 System Handle* mode will be compiled, as the static and dynamic modes are exclusive which means that they cannot be compiled at the same time. It accepts two different values: `static` or `dynamic`. + +The following flags are only applicable for the *Static ROS 2 System Handle*: + * `MIX_ROS_PACKAGES`: It accepts as an argument a list of [ROS packages](https://index.ros.org/packages/), such as `std_msgs`, `geometry_msgs`, `sensor_msgs`, `nav_msgs`... for which the required transformation library to convert the specific ROS 2 type definitions into *xTypes*, and the other way around, will be built. @@ -303,7 +336,7 @@ whole *Integration Service* product suite, there are some specific flags which a ## Documentation -The official documentation for the *ROS 2 System Handle* is included within the official *Integration Service* +The official documentation for the *ROS 2 System Handles* is included within the official *Integration Service* documentation, hosted by [Read the Docs](https://integration-service.docs.eprosima.com/), and comprises the following sections: * [Installation Manual](https://integration-service.docs.eprosima.com/en/latest/installation_manual/installation_manual.html) From bf26b663053519fc385c2f7420ebaf44d9690513 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Tue, 22 Jun 2021 10:17:19 +0200 Subject: [PATCH 16/34] API Reference Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/CMakeLists.txt | 61 ++++++++++++++++++++++++++++++++++++- ros2/dynamic/CMakeLists.txt | 59 +---------------------------------- ros2/static/CMakeLists.txt | 55 --------------------------------- 3 files changed, 61 insertions(+), 114 deletions(-) diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index 4eba9f9..b8df12f 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -29,7 +29,7 @@ option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) set(IS_ROS2_SH_MODE "Static" CACHE STRING "Select the ROS 2 SystemHandle Mode") ################################################################################### -# Load external CMake Modules. +# Select the proper ROS 2 Distro to use ################################################################################### if(BUILD_LIBRARY) # Check if the user specified which ROS 2 version he/she is using. If not, warn and try to automatically get it @@ -58,7 +58,9 @@ if(BUILD_LIBRARY) ) endif() +################################################################################### # Build ROS2 Static SH or ROS2 Dynamic SH +################################################################################### set(IS_ROS2_SH_MODE_LOWERCASE "" CACHE STRING "Build mode to lowercase") string(TOLOWER "${IS_ROS2_SH_MODE}" IS_ROS2_SH_MODE_LOWERCASE) @@ -75,3 +77,60 @@ elseif("${IS_ROS2_SH_MODE_LOWERCASE}" STREQUAL "dynamic") else() message(WARNING "Invalid mode selected. Please choose between 'static' or 'dynamic'") endif() + +################################################################################### +# Integration Service ROS 2 SystemHandle API Reference +################################################################################### +if(BUILD_API_REFERENCE) + if(NOT BUILD_LIBRARY) + find_file(IS_DOXYGEN_CONFIG_FILE doxygen-config.in PATHS ${CMAKE_INSTALL_PREFIX}/../is-core) + endif() + find_package(Doxygen REQUIRED) + # Create doxygen directories + add_custom_target(doc-dirs + COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen + COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen/html + COMMENT "Creating documentation directories" VERBATIM) + + set(IS_ROS2_STATIC_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/static/include/is/sh/ros2") + set(IS_ROS2_STATIC_SOURCE_DIR "${CMAKE_SOURCE_DIR}/static/src") + set(IS_ROS2_DYNAMIC_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/dynamic/include/is/sh/ros2") + file(GLOB_RECURSE HPP_FILES + "${IS_ROS2_STATIC_INCLUDE_DIR}/*.h*" + "${IS_ROS2_STATIC_SOURCE_DIR}/*.h*" + "${IS_ROS2_DYNAMIC_INCLUDE_DIR}/*.h*") + + # Doxygen related variables + set(DOXYGEN_INPUT_DIR "${IS_ROS2_STATIC_INCLUDE_DIR} ${IS_ROS2_STATIC_SOURCE_DIR} ${IS_ROS2_DYNAMIC_INCLUDE_DIR}") + set(DOXYGEN_OUTPUT_DIR "${PROJECT_BINARY_DIR}/doxygen") + set(DOXYGEN_INDEX_FILE "${PROJECT_BINARY_DIR}/doxygen/xml/index.xml") + set(DOXYFILE_IN "${IS_DOXYGEN_CONFIG_FILE}") + set(DOXYFILE_OUT ${PROJECT_BINARY_DIR}/doxygen-config) + set(DOXYGEN_TAGFILES "../is-core/is_core.tag=../is-core/doxygen/html") + set(DOXYGEN_HTML_DIR "${PROJECT_BINARY_DIR}/doxygen/html") + + # Configure doxygen + configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) + + # Doxygen command + add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} + COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} + DEPENDS ${HPP_FILES} + MAIN_DEPENDENCY ${DOXYFILE_OUT} ${DOXYFILE_IN} + COMMENT "Generating doxygen documentation") + + # Generate API reference + add_custom_target(doxygen-${PROJECT_NAME} ALL + DEPENDS ${DOXYGEN_INDEX_FILE} + COMMENT "Generated API documentation with doxygen" VERBATIM) + add_dependencies(doxygen-${PROJECT_NAME} doc-dirs) + + # Install doxygen generated XML files + install(DIRECTORY ${PROJECT_BINARY_DIR}/doxygen/xml + DESTINATION doxygen + COMPONENT doxygen-${PROJECT_NAME}) + set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "ROS2 System Handles doxygen") + set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION + "eProsima ROS2 System Handles doxygen documentation") + set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) +endif() diff --git a/ros2/dynamic/CMakeLists.txt b/ros2/dynamic/CMakeLists.txt index 58fa661..8aaedad 100644 --- a/ros2/dynamic/CMakeLists.txt +++ b/ros2/dynamic/CMakeLists.txt @@ -142,61 +142,4 @@ if(BUILD_LIBRARY) enable_testing() add_subdirectory(test) endif() -endif() - -################################################################################### -# Integration Service ROS 2 SystemHandle API Reference -################################################################################### -# if(BUILD_LIBRARY) -# if(BUILD_API_REFERENCE) -# if(NOT BUILD_LIBRARY) -# find_file(IS_DOXYGEN_CONFIG_FILE doxygen-config.in PATHS ${CMAKE_INSTALL_PREFIX}/../is-core) -# endif() -# find_package(Doxygen REQUIRED) -# # Create doxygen directories -# add_custom_target(doc-dirs -# COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen -# COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen/html -# COMMENT "Creating documentation directories" VERBATIM) - -# set(IS_ROS2_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/include/is/sh/ros2") -# set(IS_ROS2_SOURCE_DIR "${CMAKE_SOURCE_DIR}/src") -# file(GLOB_RECURSE HPP_FILES -# "${IS_ROS2_INCLUDE_DIR}/*.h*" -# "${IS_ROS2_SOURCE_DIR}/*.h*") - -# # Doxygen related variables -# set(DOXYGEN_INPUT_DIR "${IS_ROS2_INCLUDE_DIR} ${IS_ROS2_SOURCE_DIR}") -# set(DOXYGEN_OUTPUT_DIR "${PROJECT_BINARY_DIR}/doxygen") -# set(DOXYGEN_INDEX_FILE "${PROJECT_BINARY_DIR}/doxygen/xml/index.xml") -# set(DOXYFILE_IN "${IS_DOXYGEN_CONFIG_FILE}") -# set(DOXYFILE_OUT ${PROJECT_BINARY_DIR}/doxygen-config) -# set(DOXYGEN_TAGFILES "../is-core/is_core.tag=../is-core/doxygen/html") -# set(DOXYGEN_HTML_DIR "${PROJECT_BINARY_DIR}/doxygen/html") - -# # Configure doxygen -# configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) - -# # Doxygen command -# add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} -# COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} -# DEPENDS ${HPP_FILES} -# MAIN_DEPENDENCY ${DOXYFILE_OUT} ${DOXYFILE_IN} -# COMMENT "Generating doxygen documentation") - -# # Generate API reference -# add_custom_target(doxygen-${PROJECT_NAME} ALL -# DEPENDS ${DOXYGEN_INDEX_FILE} -# COMMENT "Generated API documentation with doxygen" VERBATIM) -# add_dependencies(doxygen-${PROJECT_NAME} doc-dirs) - -# # Install doxygen generated XML files -# install(DIRECTORY ${PROJECT_BINARY_DIR}/doxygen/xml -# DESTINATION doxygen -# COMPONENT doxygen-${PROJECT_NAME}) -# set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "ROS2 System Handle doxygen") -# set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION -# "eProsima ROS2 System Handle doxygen documentation") -# set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) -# endif() -# endif() \ No newline at end of file +endif() \ No newline at end of file diff --git a/ros2/static/CMakeLists.txt b/ros2/static/CMakeLists.txt index 3dd7e15..8751c79 100644 --- a/ros2/static/CMakeLists.txt +++ b/ros2/static/CMakeLists.txt @@ -142,59 +142,4 @@ if(BUILD_LIBRARY) if(BUILD_TESTS OR BUILD_ROS2_TESTS) add_subdirectory(test) endif() -endif() - -################################################################################### -# Integration Service ROS 2 SystemHandle API Reference -################################################################################### -if(BUILD_API_REFERENCE) - if(NOT BUILD_LIBRARY) - find_file(IS_DOXYGEN_CONFIG_FILE doxygen-config.in PATHS ${CMAKE_INSTALL_PREFIX}/../is-core) - endif() - find_package(Doxygen REQUIRED) - # Create doxygen directories - add_custom_target(doc-dirs - COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen - COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/doxygen/html - COMMENT "Creating documentation directories" VERBATIM) - - set(IS_ROS2_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/include/is/sh/ros2") - set(IS_ROS2_SOURCE_DIR "${CMAKE_SOURCE_DIR}/src") - file(GLOB_RECURSE HPP_FILES - "${IS_ROS2_INCLUDE_DIR}/*.h*" - "${IS_ROS2_SOURCE_DIR}/*.h*") - - # Doxygen related variables - set(DOXYGEN_INPUT_DIR "${IS_ROS2_INCLUDE_DIR} ${IS_ROS2_SOURCE_DIR}") - set(DOXYGEN_OUTPUT_DIR "${PROJECT_BINARY_DIR}/doxygen") - set(DOXYGEN_INDEX_FILE "${PROJECT_BINARY_DIR}/doxygen/xml/index.xml") - set(DOXYFILE_IN "${IS_DOXYGEN_CONFIG_FILE}") - set(DOXYFILE_OUT ${PROJECT_BINARY_DIR}/doxygen-config) - set(DOXYGEN_TAGFILES "../is-core/is_core.tag=../is-core/doxygen/html") - set(DOXYGEN_HTML_DIR "${PROJECT_BINARY_DIR}/doxygen/html") - - # Configure doxygen - configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) - - # Doxygen command - add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} - COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} - DEPENDS ${HPP_FILES} - MAIN_DEPENDENCY ${DOXYFILE_OUT} ${DOXYFILE_IN} - COMMENT "Generating doxygen documentation") - - # Generate API reference - add_custom_target(doxygen-${PROJECT_NAME} ALL - DEPENDS ${DOXYGEN_INDEX_FILE} - COMMENT "Generated API documentation with doxygen" VERBATIM) - add_dependencies(doxygen-${PROJECT_NAME} doc-dirs) - - # Install doxygen generated XML files - install(DIRECTORY ${PROJECT_BINARY_DIR}/doxygen/xml - DESTINATION doxygen - COMPONENT doxygen-${PROJECT_NAME}) - set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "ROS2 System Handle doxygen") - set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION - "eProsima ROS2 System Handle doxygen documentation") - set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} doxygen-${PROJECT_NAME}) endif() \ No newline at end of file From b22d77a33316482d2dc6382f4966fb3670696d7f Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Tue, 22 Jun 2021 12:19:10 +0200 Subject: [PATCH 17/34] Avoid creation of file no longer neccessary Signed-off-by: laura-eprosima Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- utils/ros2-mix-generator/CMakeLists.txt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/utils/ros2-mix-generator/CMakeLists.txt b/utils/ros2-mix-generator/CMakeLists.txt index df79186..cdfe427 100644 --- a/utils/ros2-mix-generator/CMakeLists.txt +++ b/utils/ros2-mix-generator/CMakeLists.txt @@ -100,12 +100,6 @@ is_ros2_rosidl_mix( MIDDLEWARES ros2 ) -find_program(NODERED_EXECUTABLE NAMES node-red node-red@ PATH_SUFFIXES bin) - -if(NODERED_EXECUTABLE) - file(WRITE $ENV{HOME}/rosidl_msgs_path.txt "${CMAKE_INSTALL_PREFIX}/lib/is/ros2/msg\n${CMAKE_INSTALL_PREFIX}/include/is/rosidl/ros2") -endif() - # Install Config.cmake file install( FILES From 6d987bc5f97f67ac953fb2281ea1a1c8e96074e4 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Thu, 1 Jul 2021 10:09:59 +0200 Subject: [PATCH 18/34] Add support for IDLs with several modules and structures Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/resources/generator.bash | 36 ++-- ros2/dynamic/src/SystemHandle.cpp | 250 ++++++++++++++++---------- 2 files changed, 176 insertions(+), 110 deletions(-) diff --git a/ros2/dynamic/resources/generator.bash b/ros2/dynamic/resources/generator.bash index ac224fd..4dbabcf 100755 --- a/ros2/dynamic/resources/generator.bash +++ b/ros2/dynamic/resources/generator.bash @@ -5,30 +5,40 @@ install_path="" while [[ "$#" -gt 0 ]]; do case $1 in -p|--package_name) package_name="$2"; shift ;; - -d|--dependencies) dependencies="$2"; shift ;; -i|--install_path) install_path="$2"; shift ;; - *) echo "Unknown parameter passed: $1" ;; + *) + if [[ $1 == *"--"* ]]; then + param="${1/--/}" + declare $param="$2" + echo $1 $2 + fi ;; esac shift done -echo "Generating Type Support for package $package_name with dependencies $dependencies"; +for pkg in $package_name; do -cp /tmp/CMakeLists.txt /tmp/$package_name/ + echo "Generating Type Support for package $pkg with dependencies ${!pkg}"; -sed "s#\([^<][^<]*\)#$package_name#" /tmp/package.xml > /tmp/$package_name/package.xml + cp /tmp/CMakeLists.txt /tmp/$pkg/ -cd /tmp/$package_name + sed "s#\([^<][^<]*\)#$pkg#" /tmp/package.xml > /tmp/$pkg/package.xml -echo "Installing the generated Type Support in $install_path"; + cd /tmp/$pkg -cmake . -DIS_PACKAGE_NAME=$package_name -DPACKAGE_DEPENDENCIES=$dependencies -DCMAKE_INSTALL_PREFIX:PATH=$install_path + echo "Installing the generated Type Support in $install_path"; -cmake --build . --target install + echo "Command: cmake . -DIS_PACKAGE_NAME=$pkg -DPACKAGE_DEPENDENCIES=${!pkg} -DCMAKE_INSTALL_PREFIX:PATH=$install_path"; -if [ $? -ne 0 ] -then - exit 1 -fi + cmake . -DIS_PACKAGE_NAME=$pkg -DPACKAGE_DEPENDENCIES=${!pkg} -DCMAKE_INSTALL_PREFIX:PATH=$install_path + + cmake --build . --target install + + if [ $? -ne 0 ] + then + exit 1 + fi + +done . $install_path/setup.bash \ No newline at end of file diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index e462fcd..2639a92 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -325,9 +325,13 @@ class SystemHandle : public virtual FullSystem std::vector custom_include_paths; bool custom_includes = false; // Retrieve the include clauses from the IDL - std::regex incl_reg("#\\s*include\\s+[<\"][^>\"]*[>\"]"); + std::regex incl_reg("#\\s*include\\s*[<\"][^>\"]*[>\"]"); std::string idl = entry.as(); replace_all_string(idl, "#include", "\n#include"); + + logger_ << utils::Logger::Level::DEBUG + << "IDL string" << idl << std::endl; + std::sregex_iterator iter(idl.begin(), idl.end(), incl_reg); std::sregex_iterator end; @@ -413,7 +417,7 @@ class SystemHandle : public virtual FullSystem logger_ << utils::Logger::Level::DEBUG << "Number of modules: " << modules << std::endl; - if (modules == 0) + if (modules < 1) { logger_ << utils::Logger::Level::ERROR << "The type is not declared within an IDL module. Please follow the ROS 2 naming convention." @@ -421,14 +425,6 @@ class SystemHandle : public virtual FullSystem return false; } - else if (modules > 1) - { - logger_ << utils::Logger::Level::ERROR - << "There can only be one module per IDL, add" - << " another entry in the YAML `idls` tag." << std::endl; - - return false; - } else { bool is_success = true; @@ -468,43 +464,33 @@ class SystemHandle : public virtual FullSystem package_names.insert(mod.name()); - std::string aux; - std::size_t found = idl.rfind("struct"); - aux = idl.substr(found + 7); - found = aux.find("{"); - aux = aux.substr(0, found); - aux.erase(remove_if(aux.begin(), aux.end(), isspace), aux.end()); - - // Get the type corresponding to the last struct inside the current module - auto type = context.module().submodule(mod.name()).get()->type(aux, true); - - // Check if it is a Structure - if (type.get()->kind() != xtypes::TypeKind::STRUCTURE_TYPE) - { - logger_ << utils::Logger::Level::ERROR - << "[" << aux - << "] is not a structure." - << std::endl; - is_success = false; - return; - } + auto all_types = context.module().submodule(mod.name()).get()->get_all_types(); - // Check that the message name follows the ROS2 naming convention - std::regex type_reg("[A-Z]([a-zA-Z0-9])+"); - if (!std::regex_match(aux, type_reg)) + for (auto [name, dtype] : all_types) { - logger_ << utils::Logger::Level::ERROR - << "The message name [" << aux - << "] doesn't follow the ROS2 naming convention." - << std::endl; - is_success = false; - return; - } + // Check if it is a Structure + if (dtype.get()->kind() != xtypes::TypeKind::STRUCTURE_TYPE) + { + logger_ << utils::Logger::Level::ERROR + << "[" << name + << "] is not a structure." + << std::endl; + is_success = false; + return; + } - //TODO: Change when the services are implemented - std::ofstream idlfile ("/tmp/" + mod.name() + "/msg/" + aux + ".idl"); - idlfile << idl << std::endl; - idlfile.close(); + // Check that the message name follows the ROS2 naming convention + std::regex type_reg("[A-Z]([a-zA-Z0-9])*"); + if (!std::regex_match(name, type_reg)) + { + logger_ << utils::Logger::Level::ERROR + << "The message name [" << name + << "] doesn't follow the ROS2 naming convention." + << std::endl; + is_success = false; + return; + } + }; for (const auto& mv_idl : custom_include_paths) { @@ -519,78 +505,148 @@ class SystemHandle : public virtual FullSystem } } - const std::string package_name = "--package_name " + mod.name(); - const std::string path = "--install_path /opt/ros/" + ROS2_DISTRO; - - logger_ << is::utils::Logger::Level::INFO - << "Generating ROS2 Type Support for package: " << package_name - << std::endl; + }, false); - std::string command = "exec bash /tmp/generator.bash " + package_name + " " + path; + std::map resulting_idl; + auto m_idl = eprosima::xtypes::idl::generate( + static_cast + (context.module()), &resulting_idl); - if (ros2_modules.size() != 0) - { - std::ostringstream stream; - std::copy(ros2_modules.begin(), ros2_modules.end(), std::ostream_iterator(stream, ";")); - const std::string depends = "--dependencies \"" + stream.str() + "\""; - logger_ << is::utils::Logger::Level::DEBUG - << "ROS2 Type Support Dependencies [" << depends - << "]" << std::endl; + std::string depends = ""; - command += " " + depends; + // The Xtypes generator generates code also for the ROS2 types, which is not neccessary + std::set remove; + if (!ros2_modules.empty()) + { + for (auto& pair : resulting_idl) + { + if (ros2_modules.find(pair.first.substr(0, pair.first.find("::"))) != ros2_modules.end()) + { + remove.insert(pair.second); + } } + } - FILE* pipe = popen(command.c_str(), "r"); - if (!pipe) + // Transform the dependencies into include clauses + for (auto& pair : resulting_idl) + { + if (pair.first.find(":dependencies") == std::string::npos + && remove.find(pair.second) == remove.end()) { - logger_ << utils::Logger::Level::ERROR - << " Failed to execute command: " << command - << std::endl; - is_success = false; - return; - } + std::set dependencies; + logger_ << utils::Logger::Level::DEBUG + << "[" << pair.first << "]"; - char buffer[128]; - std::string output = ""; - while(!feof(pipe)) { - if(fgets(buffer, 128, pipe) != NULL) - output += buffer; - } + for (auto& r_str : remove) + { + auto pos = pair.second.find(r_str); + if (pos != std::string::npos) + { + pair.second.erase(pos, r_str.length()); + } - logger_ << is::utils::Logger::Level::DEBUG - << output << std::endl; + } - int st = pclose(pipe); - if (WIFEXITED(st)) - { - if (1 == WEXITSTATUS(st)) + auto it = resulting_idl.find(pair.first + ":dependencies"); + if (it != resulting_idl.end()) { - logger_ << is::utils::Logger::Level::ERROR - << "Failed to generate the Type Support for package '" << package_name - << "'. Make sure you follow all the ROS2 naming and structure convention" - << " rules" << std::endl; - - if (logger_.get_level() != is::utils::Logger::Level::DEBUG) - { - logger_ << is::utils::Logger::Level::ERROR - << output << std::endl; + std::stringstream ss(resulting_idl[pair.first + ":dependencies"]); + std::string str; + std::string includes; + while (getline(ss, str, ',')) { + std::string package = str.substr(0, str.find("::")); + // If the dependency corresponds with the same package as the current idl + // don't insert it. It will generate a circular dependency. + if (pair.first.find(package) == std::string::npos) + { + dependencies.insert(package); + } + includes += "#include <" + str + ".idl>\n"; } + replace_all_string(includes, "::", "/"); + resulting_idl.erase(it); + resulting_idl[pair.first] = includes + "\n" + pair.second; + } - is_success = false; - return; + logger_ << pair.second << std::endl; + + std::string struct_path = pair.first; + replace_all_string(struct_path, "::", "/"); + std::ofstream idlfile ("/tmp/" + struct_path + ".idl"); + idlfile << pair.second << std::endl; + idlfile.close(); + + if (!dependencies.empty()) + { + std::ostringstream dep_stream; + std::copy(dependencies.begin(), dependencies.end(), std::ostream_iterator(dep_stream, ";")); + depends += "--" + pair.first.substr(0, pair.first.find("::")) + " \"" + dep_stream.str() + "\" "; } - else if (0 == WEXITSTATUS(st)) + } + } + + // Call the bash that generates the ROS 2 type support for the idl types + std::ostringstream pkg_stream; + std::copy(package_names.begin(), package_names.end(), std::ostream_iterator(pkg_stream, " ")); + const std::string package_name = "--package_name \"" + pkg_stream.str() + "\" "; + const std::string path = "--install_path /opt/ros/" + ROS2_DISTRO; + + logger_ << is::utils::Logger::Level::INFO + << "Generating ROS2 Type Support for package: " << package_name + << std::endl; + + std::string command = "exec bash /tmp/generator.bash " + package_name + " " + path + " " + depends; + + logger_ << utils::Logger::Level::DEBUG + << "Command: " << command << std::endl; + + FILE* pipe = popen(command.c_str(), "r"); + if (!pipe) + { + logger_ << utils::Logger::Level::ERROR + << " Failed to execute command: " << command + << std::endl; + return false; + } + + char buffer[128]; + std::string output = ""; + while(!feof(pipe)) { + if(fgets(buffer, 128, pipe) != NULL) + output += buffer; + } + + logger_ << is::utils::Logger::Level::DEBUG + << output << std::endl; + + int st = pclose(pipe); + if (WIFEXITED(st)) + { + if (1 == WEXITSTATUS(st)) + { + logger_ << is::utils::Logger::Level::ERROR + << "Failed to generate the Type Support for package '" << package_name + << "'. Make sure you follow all the ROS2 naming and structure convention" + << " rules" << std::endl; + + if (logger_.get_level() != is::utils::Logger::Level::DEBUG) { - logger_ << is::utils::Logger::Level::INFO - << "ROS2 Type Supports generation finished for package '" - << package_name << "', installedin path " - << path << std::endl; + logger_ << is::utils::Logger::Level::ERROR + << output << std::endl; } + return false; + } + else if (0 == WEXITSTATUS(st)) + { + logger_ << is::utils::Logger::Level::INFO + << "ROS2 Type Supports generation finished for package '" + << package_name << "', installedin path " + << path << std::endl; } - }, false); + } return is_success; } From 0c12cddefd2176d5d3a8a8ca3d3c43b5a3cd09f5 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Thu, 1 Jul 2021 10:22:47 +0200 Subject: [PATCH 19/34] Adapt CI until the branches are merged Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- .github/workflows/ci.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index fe2dfdb..8f5dbda 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -38,7 +38,8 @@ jobs: - name: Download the Integration Service run: | - git clone --recursive https://github.com/eProsima/Integration-Service -b feature/ros2_dynamic src/integration-service + git clone --recursive https://github.com/eProsima/xtypes.git -b feature/ros2_sh_dynamic src/xtypes + git clone https://github.com/eProsima/Integration-Service -b feature/ros2_dynamic src/integration-service - name: Build run: | From 24231b1c89478b4a8b87281d5f0f0778203e3f11 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Mon, 5 Jul 2021 07:46:48 +0200 Subject: [PATCH 20/34] Add support for QoS Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/include/is/sh/ros2/Publisher.hpp | 4 + .../dynamic/include/is/sh/ros2/Subscriber.hpp | 8 +- ros2/dynamic/src/Publisher.cpp | 176 ++++++++++++++++- ros2/dynamic/src/Subscriber.cpp | 177 +++++++++++++++++- ros2/dynamic/src/SystemHandle.cpp | 4 +- 5 files changed, 364 insertions(+), 5 deletions(-) diff --git a/ros2/dynamic/include/is/sh/ros2/Publisher.hpp b/ros2/dynamic/include/is/sh/ros2/Publisher.hpp index 5742dfc..b8d4bd9 100644 --- a/ros2/dynamic/include/is/sh/ros2/Publisher.hpp +++ b/ros2/dynamic/include/is/sh/ros2/Publisher.hpp @@ -142,6 +142,10 @@ class Publisher ::fastdds::dds::DataWriter* /*writer*/, const ::fastdds::dds::PublicationMatchedStatus& info) override; + void get_qos_from_config( + ::fastdds::dds::DataWriterQos* dw_qos, + const YAML::Node& config); + /** * Class members. */ diff --git a/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp b/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp index e18057a..e672334 100644 --- a/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp +++ b/ros2/dynamic/include/is/sh/ros2/Subscriber.hpp @@ -80,7 +80,8 @@ class Subscriber : private ::fastdds::dds::DataReaderListener Participant* participant, const std::string& topic_name, const xtypes::DynamicType& message_type, - TopicSubscriberSystem::SubscriptionCallback* is_callback); + TopicSubscriberSystem::SubscriptionCallback* is_callback, + const YAML::Node& config); // TODO(@jamoralp): Create subscriber based on XML profiles? @@ -145,6 +146,11 @@ class Subscriber : private ::fastdds::dds::DataReaderListener */ void cleaner_function(); + + void get_qos_from_config( + ::fastdds::dds::DataReaderQos* dw_qos, + const YAML::Node& config); + /** * Class members. */ diff --git a/ros2/dynamic/src/Publisher.cpp b/ros2/dynamic/src/Publisher.cpp index 96d9c39..f2c6669 100644 --- a/ros2/dynamic/src/Publisher.cpp +++ b/ros2/dynamic/src/Publisher.cpp @@ -25,6 +25,8 @@ #include #include +#define NS_TO_S(nanoseconds) (nanoseconds / (1000 * 1000 * 1000)) + namespace eprosima { namespace is { namespace sh { @@ -34,7 +36,7 @@ Publisher::Publisher( Participant* participant, const std::string& topic_name, const xtypes::DynamicType& message_type, - const YAML::Node& /*config*/) + const YAML::Node& config) : participant_(participant) , topic_name_(topic_name) , logger_("is::sh::ROS2_Dynamic::Publisher") @@ -123,6 +125,12 @@ Publisher::Publisher( // Create DDS datawriter ::fastdds::dds::DataWriterQos datawriter_qos = ::fastdds::dds::DATAWRITER_QOS_DEFAULT; + // Configure datawriter QoS according to config node + if (config["qos"]) + { + get_qos_from_config(&datawriter_qos, config["qos"]); + } + dds_datawriter_ = dds_publisher_->create_datawriter(dds_topic_, datawriter_qos, this); if (dds_datawriter_) { @@ -207,6 +215,172 @@ void Publisher::on_publication_matched( } } +void Publisher::get_qos_from_config( + ::fastdds::dds::DataWriterQos* dw_qos, + const YAML::Node& config) +{ + logger_ << utils::Logger::Level::DEBUG + << "Publisher created with QoS: " << YAML::Dump(config) << std::endl; + + // Configure datawriter QoS according to config node + if (config["history"]) + { + ::fastdds::dds::HistoryQosPolicy hist_policy; + if (config["history"]["kind"]) + { + std::string hist_kind = config["history"]["kind"].as(); + if (hist_kind == "KEEP_LAST") + { + hist_policy.kind = ::fastdds::dds::KEEP_LAST_HISTORY_QOS; + } + else if (hist_kind == "KEEP_ALL") + { + hist_policy.kind = ::fastdds::dds::KEEP_ALL_HISTORY_QOS; + } + else + { + logger_ << utils::Logger::Level::WARN + << "History QoS kind is unknown. " + << "The valid values are: KEEP_LAST and KEEP_ALL." << std::endl; + } + } + + if (config["history"]["depth"]) + { + hist_policy.depth = config["history"]["depth"].as(); + } + + dw_qos->history(hist_policy); + } + + if (config["reliability"]) + { + ::fastdds::dds::ReliabilityQosPolicy rel_policy; + std::string rel_kind = config["reliability"].as(); + if (rel_kind == "RELIABLE") + { + rel_policy.kind = ::fastdds::dds::RELIABLE_RELIABILITY_QOS; + } + else if (rel_kind == "BEST_EFFORT") + { + rel_policy.kind = ::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS; + } + else + { + logger_ << utils::Logger::Level::WARN + << "Reliability QoS kind is unknown. " + << "The valid values are: RELIABLE and BEST_EFFORT." << std::endl; + } + + dw_qos->reliability(rel_policy); + } + + if (config["durability"]) + { + ::fastdds::dds::DurabilityQosPolicy dur_policy; + std::string dur_kind = config["durability"].as(); + if (dur_kind == "VOLATILE") + { + dur_policy.kind = ::fastdds::dds::VOLATILE_DURABILITY_QOS; + } + else if (dur_kind == "TRANSIENT_LOCAL") + { + dur_policy.kind = ::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS; + } + else + { + logger_ << utils::Logger::Level::WARN + << "Durability QoS kind is unknown. " + << "The valid values are: VOLATILE and TRANSIENT_LOCAL." << std::endl; + } + + dw_qos->durability(dur_policy); + } + + if (config["deadline"]) + { + ::fastdds::dds::DeadlineQosPolicy d_policy; + eprosima::fastrtps::rtps::Time_t d_period = eprosima::fastrtps::c_TimeInfinite; + if (config["deadline"]["sec"]) + { + d_period.seconds(config["deadline"]["sec"].as()); + } + + if (config["deadline"]["nanosec"]) + { + d_period.nanosec(config["deadline"]["sec"].as()); + } + + d_policy.period = d_period.to_duration_t(); + dw_qos->deadline(d_policy); + } + + if (config["lifespan"]) + { + ::fastdds::dds::LifespanQosPolicy life_policy; + eprosima::fastrtps::rtps::Time_t life_duration = eprosima::fastrtps::c_TimeInfinite; + if (config["lifespan"]["sec"]) + { + life_duration.seconds(config["lifespan"]["sec"].as()); + } + + if (config["lifespan"]["nanosec"]) + { + life_duration.nanosec(config["lifespan"]["sec"].as()); + } + + life_policy.duration = life_duration.to_duration_t(); + dw_qos->lifespan(life_policy); + } + + if (config["liveliness"]) + { + ::fastdds::dds::LivelinessQosPolicy live_policy; + if (config["liveliness"]["kind"]) + { + std::string live_kind = config["liveliness"]["kind"].as(); + if (live_kind == "AUTOMATIC") + { + live_policy.kind = ::fastdds::dds::AUTOMATIC_LIVELINESS_QOS; + } + else if (live_kind == "MANUAL_BY_TOPIC") + { + live_policy.kind = ::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS; + } + else + { + logger_ << utils::Logger::Level::WARN + << "Liveliness QoS kind is unknown. " + << "The valid values are: AUTOMATIC and MANUAL_BY_TOPIC." << std::endl; + } + } + + eprosima::fastrtps::rtps::Time_t live_duration = eprosima::fastrtps::c_TimeInfinite; + bool lease_duration_changed = false; + if (config["liveliness"]["sec"]) + { + live_duration.seconds(config["liveliness"]["sec"].as()); + lease_duration_changed = true; + } + + if (config["liveliness"]["nanosec"]) + { + live_duration.nanosec(config["liveliness"]["sec"].as()); + lease_duration_changed = true; + } + + if (lease_duration_changed) + { + double ns = live_duration.to_ns() * 2.0 / 3.0; + double s = NS_TO_S(ns); + live_policy.announcement_period = eprosima::fastrtps::Duration_t(s); + } + live_policy.lease_duration = live_duration.to_duration_t(); + + dw_qos->liveliness(live_policy); + } +} + } // namespace ros2 } // namespace sh } // namespace is diff --git a/ros2/dynamic/src/Subscriber.cpp b/ros2/dynamic/src/Subscriber.cpp index 87baf03..e09f91a 100644 --- a/ros2/dynamic/src/Subscriber.cpp +++ b/ros2/dynamic/src/Subscriber.cpp @@ -31,6 +31,8 @@ #include #include +#define NS_TO_S(nanoseconds) (nanoseconds / (1000 * 1000 * 1000)) + namespace eprosima { namespace is { namespace sh { @@ -40,7 +42,8 @@ Subscriber::Subscriber( Participant* participant, const std::string& topic_name, const xtypes::DynamicType& message_type, - TopicSubscriberSystem::SubscriptionCallback* is_callback) + TopicSubscriberSystem::SubscriptionCallback* is_callback, + const YAML::Node& config) : participant_(participant) , dds_subscriber_(nullptr) , dynamic_data_(nullptr) @@ -137,6 +140,12 @@ Subscriber::Subscriber( rel_policy.kind = ::fastdds::dds::RELIABLE_RELIABILITY_QOS; datareader_qos.reliability(rel_policy); + // Configure datareader QoS according to config node + if (config["qos"]) + { + get_qos_from_config(&datareader_qos, config["qos"]); + } + dds_datareader_ = dds_subscriber_->create_datareader(dds_topic_, datareader_qos, this); if (dds_datareader_) { @@ -329,6 +338,172 @@ void Subscriber::cleaner_function() } } +void Subscriber::get_qos_from_config( + ::fastdds::dds::DataReaderQos* dr_qos, + const YAML::Node& config) +{ + logger_ << utils::Logger::Level::DEBUG + << "Subscriber created wit QoS: " << YAML::Dump(config) << std::endl; + + // Configure datawriter QoS according to config node + if (config["history"]) + { + ::fastdds::dds::HistoryQosPolicy hist_policy; + if (config["history"]["kind"]) + { + std::string hist_kind = config["history"]["kind"].as(); + if (hist_kind == "KEEP_LAST") + { + hist_policy.kind = ::fastdds::dds::KEEP_LAST_HISTORY_QOS; + } + else if (hist_kind == "KEEP_ALL") + { + hist_policy.kind = ::fastdds::dds::KEEP_ALL_HISTORY_QOS; + } + else + { + logger_ << utils::Logger::Level::WARN + << "History QoS kind is unknown. " + << "The valid values are: KEEP_LAST and KEEP_ALL." << std::endl; + } + } + + if (config["history"]["depth"]) + { + hist_policy.depth = config["history"]["depth"].as(); + } + + dr_qos->history(hist_policy); + } + + if (config["reliability"]) + { + ::fastdds::dds::ReliabilityQosPolicy rel_policy; + std::string rel_kind = config["reliability"].as(); + if (rel_kind == "RELIABLE") + { + rel_policy.kind = ::fastdds::dds::RELIABLE_RELIABILITY_QOS; + } + else if (rel_kind == "BEST_EFFORT") + { + rel_policy.kind = ::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS; + } + else + { + logger_ << utils::Logger::Level::WARN + << "Reliability QoS kind is unknown. " + << "The valid values are: RELIABLE and BEST_EFFORT." << std::endl; + } + + dr_qos->reliability(rel_policy); + } + + if (config["durability"]) + { + ::fastdds::dds::DurabilityQosPolicy dur_policy; + std::string dur_kind = config["durability"].as(); + if (dur_kind == "VOLATILE") + { + dur_policy.kind = ::fastdds::dds::VOLATILE_DURABILITY_QOS; + } + else if (dur_kind == "TRANSIENT_LOCAL") + { + dur_policy.kind = ::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS; + } + else + { + logger_ << utils::Logger::Level::WARN + << "Durability QoS kind is unknown. " + << "The valid values are: VOLATILE and TRANSIENT_LOCAL." << std::endl; + } + + dr_qos->durability(dur_policy); + } + + if (config["deadline"]) + { + ::fastdds::dds::DeadlineQosPolicy d_policy; + eprosima::fastrtps::rtps::Time_t d_period = eprosima::fastrtps::c_TimeInfinite; + if (config["deadline"]["sec"]) + { + d_period.seconds(config["deadline"]["sec"].as()); + } + + if (config["deadline"]["nanosec"]) + { + d_period.nanosec(config["deadline"]["sec"].as()); + } + + d_policy.period = d_period.to_duration_t(); + dr_qos->deadline(d_policy); + } + + if (config["lifespan"]) + { + ::fastdds::dds::LifespanQosPolicy life_policy; + eprosima::fastrtps::rtps::Time_t life_duration = eprosima::fastrtps::c_TimeInfinite; + if (config["lifespan"]["sec"]) + { + life_duration.seconds(config["lifespan"]["sec"].as()); + } + + if (config["lifespan"]["nanosec"]) + { + life_duration.nanosec(config["lifespan"]["sec"].as()); + } + + life_policy.duration = life_duration.to_duration_t(); + dr_qos->lifespan(life_policy); + } + + if (config["liveliness"]) + { + ::fastdds::dds::LivelinessQosPolicy live_policy; + if (config["liveliness"]["kind"]) + { + std::string live_kind = config["liveliness"]["kind"].as(); + if (live_kind == "AUTOMATIC") + { + live_policy.kind = ::fastdds::dds::AUTOMATIC_LIVELINESS_QOS; + } + else if (live_kind == "MANUAL_BY_TOPIC") + { + live_policy.kind = ::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS; + } + else + { + logger_ << utils::Logger::Level::WARN + << "Liveliness QoS kind is unknown. " + << "The valid values are: AUTOMATIC and MANUAL_BY_TOPIC." << std::endl; + } + } + + eprosima::fastrtps::rtps::Time_t live_duration = eprosima::fastrtps::c_TimeInfinite; + bool lease_duration_changed = false; + if (config["liveliness"]["sec"]) + { + live_duration.seconds(config["liveliness"]["sec"].as()); + lease_duration_changed = true; + } + + if (config["liveliness"]["nanosec"]) + { + live_duration.nanosec(config["liveliness"]["sec"].as()); + lease_duration_changed = true; + } + + if (lease_duration_changed) + { + double ns = live_duration.to_ns() * 2.0 / 3.0; + double s = NS_TO_S(ns); + live_policy.announcement_period = eprosima::fastrtps::Duration_t(s); + } + live_policy.lease_duration = live_duration.to_duration_t(); + + dr_qos->liveliness(live_policy); + } +} + } // namespace ros2 } // namespace sh } // namespace is diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index 2639a92..4bf2c80 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -128,13 +128,13 @@ class SystemHandle : public virtual FullSystem const std::string& topic_name, const xtypes::DynamicType& message_type, SubscriptionCallback* callback, - const YAML::Node& /* configuration */) override + const YAML::Node& configuration) override { try { std::string topic_name_mangling = get_ros2_topic_name(topic_name); auto subscriber = std::make_shared( - participant_.get(), topic_name_mangling, message_type, callback); + participant_.get(), topic_name_mangling, message_type, callback, configuration); subscribers_.emplace_back(std::move(subscriber)); From 0b718e418c1e48b3e53163c66a75d6b1f8f9682b Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Tue, 6 Jul 2021 07:28:41 +0200 Subject: [PATCH 21/34] Fix nanosec calculus Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/src/Publisher.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros2/dynamic/src/Publisher.cpp b/ros2/dynamic/src/Publisher.cpp index f2c6669..e3970b0 100644 --- a/ros2/dynamic/src/Publisher.cpp +++ b/ros2/dynamic/src/Publisher.cpp @@ -308,7 +308,7 @@ void Publisher::get_qos_from_config( if (config["deadline"]["nanosec"]) { - d_period.nanosec(config["deadline"]["sec"].as()); + d_period.nanosec(config["deadline"]["nanosec"].as()); } d_policy.period = d_period.to_duration_t(); @@ -326,7 +326,7 @@ void Publisher::get_qos_from_config( if (config["lifespan"]["nanosec"]) { - life_duration.nanosec(config["lifespan"]["sec"].as()); + life_duration.nanosec(config["lifespan"]["nanosec"].as()); } life_policy.duration = life_duration.to_duration_t(); @@ -365,7 +365,7 @@ void Publisher::get_qos_from_config( if (config["liveliness"]["nanosec"]) { - live_duration.nanosec(config["liveliness"]["sec"].as()); + live_duration.nanosec(config["liveliness"]["nanosec"].as()); lease_duration_changed = true; } From 9b3f9ecd4e7f9e36e0088cb2a5b9dc4cb183aa48 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Mon, 12 Jul 2021 09:15:36 +0200 Subject: [PATCH 22/34] Adapt ROS 2 QoS tests to dynamic SystemHandle Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/test/CMakeLists.txt | 4 +- .../test/integration/ros2__geometry_msgs.cpp | 2 +- .../test/integration/ros2__test_domain.cpp | 2 +- .../integration/ros2__test_domain__foxy.cpp | 2 +- .../test/integration/ros2__test_qos.cpp | 394 ++++++++++++++++++ .../test/integration/ros2__test_qos.cpp | 0 6 files changed, 399 insertions(+), 5 deletions(-) create mode 100644 ros2/dynamic/test/integration/ros2__test_qos.cpp rename ros2/{ => static}/test/integration/ros2__test_qos.cpp (100%) diff --git a/ros2/dynamic/test/CMakeLists.txt b/ros2/dynamic/test/CMakeLists.txt index 02fd0fc..16e15d3 100644 --- a/ros2/dynamic/test/CMakeLists.txt +++ b/ros2/dynamic/test/CMakeLists.txt @@ -51,8 +51,6 @@ macro(compile_test) $,libgtest,gtest> ) - message(WARNING "INCLUDE PATH ${CMAKE_INSTALL_PREFIX}/include") - target_include_directories(${TEST_NAME} PRIVATE ${geometry_msgs_INCLUDE_DIRS} @@ -79,6 +77,7 @@ enable_testing() compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msgs.cpp) compile_test(${PROJECT_NAME}_check_ros2pkg_creation SOURCE integration/check_ros2pkg_creation.cpp ENVIRONMENTS "ROS2_DISTRO=${IS_ROS2_DISTRO}") +compile_test(${PROJECT_NAME}_test_qos SOURCE integration/ros2__test_qos.cpp) if ("${IS_ROS2_DISTRO}" STREQUAL "foxy") compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) @@ -93,6 +92,7 @@ set_property( ${PROJECT_NAME}_check_ros2pkg_creation ${PROJECT_NAME}_geometry_msgs ${PROJECT_NAME}_test_domain + ${PROJECT_NAME}_test_qos APPEND PROPERTY COMPILE_DEFINITIONS PRIVATE "ROS2__ROSIDL__BUILD_DIR=\"${CMAKE_BINARY_DIR}/is/rosidl/ros2/lib\"" ) diff --git a/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp b/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp index f21c011..2b1b795 100644 --- a/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp +++ b/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp @@ -38,7 +38,7 @@ namespace is = eprosima::is; namespace xtypes = eprosima::xtypes; -static is::utils::Logger logger("is::sh::ROS2::test::geometry_msgs"); +static is::utils::Logger logger("is::sh::ROS2_Dynamic::test::geometry_msgs"); const std::string print_pose( const geometry_msgs::msg::PoseStamped& ros2_pose) diff --git a/ros2/dynamic/test/integration/ros2__test_domain.cpp b/ros2/dynamic/test/integration/ros2__test_domain.cpp index 2cb6e16..7b30ac6 100644 --- a/ros2/dynamic/test/integration/ros2__test_domain.cpp +++ b/ros2/dynamic/test/integration/ros2__test_domain.cpp @@ -36,7 +36,7 @@ namespace is = eprosima::is; namespace xtypes = eprosima::xtypes; -static is::utils::Logger logger("is::sh::ROS2::test::test_domain"); +static is::utils::Logger logger("is::sh::ROS2_Dynamic::test::test_domain"); constexpr const size_t DOMAIN_ID_1 = 5; constexpr const size_t DOMAIN_ID_2 = 10; diff --git a/ros2/dynamic/test/integration/ros2__test_domain__foxy.cpp b/ros2/dynamic/test/integration/ros2__test_domain__foxy.cpp index eb95204..943e444 100644 --- a/ros2/dynamic/test/integration/ros2__test_domain__foxy.cpp +++ b/ros2/dynamic/test/integration/ros2__test_domain__foxy.cpp @@ -36,7 +36,7 @@ namespace is = eprosima::is; namespace xtypes = eprosima::xtypes; -static is::utils::Logger logger("is::sh::ROS2::test::test_domain"); +static is::utils::Logger logger("is::sh::ROS2_Dynamic::test::test_domain"); constexpr const char* DOMAIN_ID_1 = "5"; constexpr const char* DOMAIN_ID_2 = "10"; diff --git a/ros2/dynamic/test/integration/ros2__test_qos.cpp b/ros2/dynamic/test/integration/ros2__test_qos.cpp new file mode 100644 index 0000000..8832fcf --- /dev/null +++ b/ros2/dynamic/test/integration/ros2__test_qos.cpp @@ -0,0 +1,394 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2020 - present 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 + +namespace is = eprosima::is; +namespace xtypes = eprosima::xtypes; +using namespace std::chrono_literals; + +static is::utils::Logger logger("is::sh::ROS2_Dynamic::test::test_qos"); + +class ROS2QoS : public ::testing::Test +{ +public: + + void set_up( + const std::string& yaml) + { + std::cout << "YAML " << yaml << std::endl; + + YAML::Node config_node = YAML::Load(yaml); + + handle = std::make_unique(is::run_instance( + config_node, {ROS2__ROSIDL__BUILD_DIR})); + + ASSERT_TRUE(handle.get()); + + char const* const argv[1] = {"is_ros2_test_qos"}; + rclcpp::init(1, argv); + + ros2 = std::make_shared("ros2_test"); + executor = std::make_shared(); + + ASSERT_TRUE( rclcpp::ok() ); + + executor->add_node(ros2); + } + + void test( + const rclcpp::QoS& qos_profile_publisher, + bool compatible) + { + const auto publisher = + ros2->create_publisher("transmit", qos_profile_publisher); + + ASSERT_TRUE(publisher); + + std::promise msg_promise; + std::future msg_future = msg_promise.get_future(); + std::mutex mock_sub_mutex; + bool mock_sub_value_received = false; + auto mock_sub = [&](const xtypes::DynamicData& msg) + { + std::unique_lock lock(mock_sub_mutex); + if (mock_sub_value_received) + { + return; + } + + logger << is::utils::Logger::Level::INFO + << "Message received" << std::endl; + + mock_sub_value_received = true; + msg_promise.set_value(msg); + }; + + ASSERT_TRUE(is::sh::mock::subscribe("transmit", mock_sub)); + + std_msgs::msg::String message; + message.set__data("Transmiting"); + + publisher->publish(message); + + executor->spin_some(); + + // Try sending several times to assure that the message is not received + auto start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < 10s) + { + executor->spin_some(); + if (msg_future.wait_for(100ms) == std::future_status::ready) + { + break; + } + + publisher->publish(message); + } + + if (!compatible) + { + ASSERT_NE(msg_future.wait_for(0s), std::future_status::ready); + } + else + { + ASSERT_EQ(msg_future.wait_for(0s), std::future_status::ready); + xtypes::DynamicData received_msg = msg_future.get(); + + EXPECT_EQ(received_msg.type().name(), "std_msgs::msg::String"); + + xtypes::ReadableDynamicDataRef xtypes_msg = received_msg; + typename std_msgs::msg::String::_data_type ros2_field; + is::utils::Convert::from_xtype_field(xtypes_msg["data"], ros2_field); + EXPECT_EQ(ros2_field, message.data); + } + + // Destroy ros2 instance node + executor->remove_node(ros2); + ros2.reset(); + + // Quit and wait for no more than a minute. We don't want the test to get + // hung here indefinitely in the case of an error. + handle->quit().wait_for(1min); + + // Require that it's no longer running. If it is still running, then it is + // probably stuck, and we should forcefully quit. + ASSERT_TRUE(!handle->running()); + ASSERT_TRUE(handle->wait() == 0); + } + +protected: + + std::shared_ptr ros2; + std::shared_ptr executor; + std::unique_ptr handle; + +}; + +TEST_F(ROS2QoS, Durability_Incompatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { durability: TRANSIENT_LOCAL } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + test(qos_profile_publisher, false); + +} + +TEST_F(ROS2QoS, Durability_Compatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { durability: VOLATILE } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + test(qos_profile_publisher, true); + +} + +TEST_F(ROS2QoS, Deadline_Incompatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { deadline: { sec: 1 } } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.deadline(std::chrono::milliseconds(2000)); + + test(qos_profile_publisher, false); + +} + +TEST_F(ROS2QoS, Deadline_Compatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { deadline: { sec: 2 } } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.deadline(std::chrono::milliseconds(2000)); + + test(qos_profile_publisher, true); + +} + +TEST_F(ROS2QoS, Liveliness_Incompatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { liveliness: { kind: MANUAL_BY_TOPIC } } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC); + + test(qos_profile_publisher, false); + +} + +TEST_F(ROS2QoS, Liveliness_Compatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { liveliness: { kind: AUTOMATIC } } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); + + test(qos_profile_publisher, true); + +} + +TEST_F(ROS2QoS, Lease_Duration_Incompatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { liveliness: { sec: 1 } } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.liveliness_lease_duration(std::chrono::milliseconds(2000)); + + test(qos_profile_publisher, false); +} + +TEST_F(ROS2QoS, Lease_Duration_Compatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { liveliness: { sec: 3 } } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.liveliness_lease_duration(std::chrono::milliseconds(2000)); + + test(qos_profile_publisher, true); + +} + +TEST_F(ROS2QoS, Reliability_Incompatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { reliability: RELIABLE } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + + test(qos_profile_publisher, false); + +} + +TEST_F(ROS2QoS, Reliability_Compatible_QoS) +{ + std::string yaml; + + yaml += "systems:\n"; + yaml += " ros2: { type: ros2_dynamic, using: [ std_msgs/String ] } \n"; + yaml += " mock: { type: mock, types-from: ros2 }\n"; + yaml += "routes:\n"; + yaml += " mock_to_ros2: { from: mock, to: ros2 }\n"; + yaml += " ros2_to_mock: { from: ros2, to: mock }\n"; + yaml += "topics:\n"; + yaml += " transmit: { type: 'std_msgs::msg::String', route: ros2_to_mock, "; + yaml += " ros2: { qos: { reliability: BEST_EFFORT } } }\n"; + + set_up(yaml); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + + test(qos_profile_publisher, true); +} + +int main( + int argc, + char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros2/test/integration/ros2__test_qos.cpp b/ros2/static/test/integration/ros2__test_qos.cpp similarity index 100% rename from ros2/test/integration/ros2__test_qos.cpp rename to ros2/static/test/integration/ros2__test_qos.cpp From 260286b1f54be48a891f4f2b788db4384e5afea3 Mon Sep 17 00:00:00 2001 From: laura-eprosima Date: Wed, 14 Jul 2021 11:15:37 +0200 Subject: [PATCH 23/34] Add option to disable internal communication Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/src/SystemHandle.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index 4bf2c80..55c2dba 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -58,6 +58,7 @@ class SystemHandle : public virtual FullSystem SystemHandle() : FullSystem() + , allow_internal(false) , logger_("is::sh::ROS2_Dynamic") { } @@ -97,6 +98,12 @@ class SystemHandle : public virtual FullSystem } } + // If true allows the communication between internal publishers and subscribers in the same topic + if (configuration["allow_internal"]) + { + allow_internal = configuration["allow_internal"].as(); + } + try { participant_ = std::make_unique(configuration); @@ -158,7 +165,7 @@ class SystemHandle : public virtual FullSystem auto sample_writer_guid = fastrtps::rtps::iHandle2GUID(sample_info->publication_handle); - if (sample_writer_guid.guidPrefix == participant_->get_dds_participant()->guid().guidPrefix) + if (sample_writer_guid.guidPrefix == participant_->get_dds_participant()->guid().guidPrefix && !allow_internal) { if (utils::Logger::Level::DEBUG == logger_.get_level()) { @@ -791,6 +798,8 @@ class SystemHandle : public virtual FullSystem std::string namespace_; + bool allow_internal; + std::set package_names; utils::Logger logger_; From 2404c570bfcfb9846efc1abedb81649e09e8348f Mon Sep 17 00:00:00 2001 From: lauramg15 Date: Thu, 5 Aug 2021 09:17:42 +0200 Subject: [PATCH 24/34] Add comments to dynamic system handle Signed-off-by: lauramg15 Signed-off-by: Miguel Barro --- ros2/dynamic/src/Publisher.cpp | 1 + ros2/dynamic/src/Subscriber.cpp | 1 + ros2/dynamic/src/SystemHandle.cpp | 108 +++++++++++++++++++++++++----- 3 files changed, 93 insertions(+), 17 deletions(-) diff --git a/ros2/dynamic/src/Publisher.cpp b/ros2/dynamic/src/Publisher.cpp index e3970b0..4e02eaa 100644 --- a/ros2/dynamic/src/Publisher.cpp +++ b/ros2/dynamic/src/Publisher.cpp @@ -46,6 +46,7 @@ Publisher::Publisher( std::string type_name = message_type.name(); std::size_t found = type_name.find_last_of("::"); + // packagename::msg::dds_::typename_ if (found == std::string::npos) { logger_ << utils::Logger::Level::ERROR diff --git a/ros2/dynamic/src/Subscriber.cpp b/ros2/dynamic/src/Subscriber.cpp index e09f91a..55c886b 100644 --- a/ros2/dynamic/src/Subscriber.cpp +++ b/ros2/dynamic/src/Subscriber.cpp @@ -59,6 +59,7 @@ Subscriber::Subscriber( std::string type_name = message_type.name(); std::size_t found = type_name.find_last_of("::"); + // packagename::msg::dds_::typename_ if (found == std::string::npos) { logger_ << utils::Logger::Level::ERROR diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index 55c2dba..d8316a7 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -65,6 +65,8 @@ class SystemHandle : public virtual FullSystem ~SystemHandle() { + // Iterate over all the packages created during the execution and remove all the associated data from the + // temporal folder std::filesystem::path tmp = std::filesystem::temp_directory_path(); for (auto pkg_name: package_names) { @@ -139,6 +141,7 @@ class SystemHandle : public virtual FullSystem { try { + // Apply the name mangling to the topic name before creating the subscriber std::string topic_name_mangling = get_ros2_topic_name(topic_name); auto subscriber = std::make_shared( participant_.get(), topic_name_mangling, message_type, callback, configuration); @@ -195,6 +198,7 @@ class SystemHandle : public virtual FullSystem { try { + // Apply the name mangling to the topic name before creating the publisher std::string topic_name_mangling = get_ros2_topic_name(topic_name); auto publisher = std::make_shared( participant_.get(), topic_name_mangling, message_type, configuration); @@ -237,10 +241,13 @@ class SystemHandle : public virtual FullSystem { logger_ << utils::Logger::Level::DEBUG << "The ROS 2 package '" << type << "' was added to the type registry." << std::endl; + + // Add the path to the map along with the package_name and type_name type_paths.emplace(std::make_pair(type, entry.path().stem().string()), entry.path().string()); } } } + // If it is a message instead of a package, just insert it else { std::string pkg = type.substr(0, found); @@ -250,7 +257,9 @@ class SystemHandle : public virtual FullSystem if (std::filesystem::exists(path)) { logger_ << utils::Logger::Level::DEBUG << "The ROS 2 message '" - << type << "' was added to the type registry." << std::endl; + << type << "' was added to the type registry." << std::endl; + + // Add the path to the map along with the package_name and type_name type_paths.emplace(std::make_pair(pkg, type_name), path); } else @@ -262,6 +271,8 @@ class SystemHandle : public virtual FullSystem } } + // Iterate over all the entries of the map, parse the file associated using the xtypes parser and add a new + // entry in the type_registry for (auto const& [type, path] : type_paths) { xtypes::idl::Context context; @@ -287,6 +298,7 @@ class SystemHandle : public virtual FullSystem const std::string& from, const std::string& to) const { + // Replace all the occurrences of a substring with the second one size_t froms = from.size(); size_t tos = to.size(); size_t pos = str.find(from); @@ -300,51 +312,65 @@ class SystemHandle : public virtual FullSystem bool preprocess_types( const YAML::Node& types_node) override { + // Check that the types YAML node is not empty if (!types_node) { return true; } + // Storage for the IDL paths that are needed to construct the xtype creation std::vector include_paths; if (types_node["paths"]) { - // Check if there are paths to custom idls - std::regex reg("/opt/ros/([a-z])+/share/*"); for (const auto& path : types_node["paths"]) { - if (!std::regex_match (path.as(), reg)) + // Insert each of the paths in the vector + include_paths.push_back(path.as()); + + if (logger_.get_level() == is::utils::Logger::Level::DEBUG) { - logger_ << utils::Logger::Level::DEBUG - << "Added path for custom IDL." << std::endl; + // Check if there are paths to custom IDLs + std::regex reg("/opt/ros/([a-z])+/share/*"); + if (!std::regex_match (path.as(), reg)) + { + logger_ << utils::Logger::Level::DEBUG + << "Added path for custom IDL." << std::endl; + } } - include_paths.push_back(path.as()); } } if(types_node["idls"]) { + // Iterate over each of the IDLs set on the YAML configuration file for (const auto& entry : types_node["idls"]) { logger_ << utils::Logger::Level::DEBUG << "IDL: " << Dump(entry) << std::endl; + // Set to store the modules that corresponds to ROS 2 messages std::set ros2_modules; + // Vector to include the paths to custom IDL files std::vector custom_include_paths; bool custom_includes = false; + // Retrieve the include clauses from the IDL std::regex incl_reg("#\\s*include\\s*[<\"][^>\"]*[>\"]"); std::string idl = entry.as(); + // This is due to the way libyaml-cpp treats the string lists replace_all_string(idl, "#include", "\n#include"); logger_ << utils::Logger::Level::DEBUG << "IDL string" << idl << std::endl; + // Apply the include regex std::sregex_iterator iter(idl.begin(), idl.end(), incl_reg); std::sregex_iterator end; logger_ << utils::Logger::Level::DEBUG << "Searching include clauses..." << std::endl; + // Iterate over all the include regex matches while (iter != end) { for(unsigned i = 0; i < iter->size(); ++i) @@ -354,22 +380,26 @@ class SystemHandle : public virtual FullSystem logger_ << utils::Logger::Level::DEBUG << "Include Clause: " << incl << std::endl; + // Regex to extract the first part of the include std::regex pkg_reg("[<\"][^>\"/]*[/]"); std::cmatch cm; std::regex_search(incl.c_str(), cm, pkg_reg); std::string pkg = cm[0]; + // Check that the regex search success if (!pkg.empty()) { + // Remove from the package name the first character (<) and the last one (/) pkg = pkg.substr(1, pkg.length() - 2); logger_ << utils::Logger::Level::DEBUG << "Include Package: " << pkg << std::endl; - // Check if the package has a corresponding folder within ros installation + // Check if the package has a corresponding folder within ROS 2 installation struct stat sb; std::string dir = "/opt/ros/" + ROS2_DISTRO + "/include/" + pkg; if (stat(dir.c_str(), &sb) == 0 && S_ISDIR(sb.st_mode)) { + // If the folder is found, it is inserted in the ros2_modules set ros2_modules.insert(pkg); } else @@ -382,17 +412,25 @@ class SystemHandle : public virtual FullSystem custom_includes = true; } + // If there are custom includes, obtain the complete path to copy it later to the new ROS 2 + // package so that it can be found during the execution if (custom_includes) { + // Extract the path from the include clause std::regex path_reg("[<\"][^>\"]*[\">]"); std::cmatch cm1; std::regex_search(incl.c_str(), cm1, path_reg); std::string in_path = cm1[0]; in_path = in_path.substr(1, in_path.length() - 2); + + // Check if the file can be found within any of the paths set in the YAML config file for (const auto& path : include_paths) { logger_ << utils::Logger::Level::DEBUG << "Looking for file " << in_path << " within path " << path << std::endl; + + // If the file exists in the path, it is stored in the custom_include_paths vector + // which will be used later to copy the file if (std::filesystem::exists(path + in_path)) { logger_ << utils::Logger::Level::DEBUG @@ -405,29 +443,36 @@ class SystemHandle : public virtual FullSystem ++iter; } - // Obtain the xtypes representation of the IDL logger_ << utils::Logger::Level::INFO << "Parsing IDL" << std::endl; + // Configures the xtypes parser options xtypes::idl::Context context; + // Allow setting keywords as identifier within the IDL representation context.allow_keyword_identifiers = true; if (!include_paths.empty()) { + // Pass all the YAML paths, so that the parser can find the IDLs context.include_paths = include_paths; } + // Obtain the xtypes representation of the IDL xtypes::idl::parse(entry.as(), context); + // If the IDL parse ends successfully continue with the type preprocess if (context.success) { + // Due to the ROS 2 convention any IDL type must have two modules (package_name and msg/srv) size_t modules = context.module().submodule_size() - ros2_modules.size(); logger_ << utils::Logger::Level::DEBUG << "Number of modules: " << modules << std::endl; + // If there isn't any module, it throws an error if (modules < 1) { logger_ << utils::Logger::Level::ERROR - << "The type is not declared within an IDL module. Please follow the ROS 2 naming convention." + << "The type is not declared within an IDL module." + << " Please follow the ROS 2 naming convention." << std::endl; return false; @@ -435,8 +480,10 @@ class SystemHandle : public virtual FullSystem else { bool is_success = true; + // Iterates over all the modules found by the xtypes parser context.module().for_each_submodule([&] (const xtypes::idl::Module& mod) { + // If the module is inside the ROS 2 modules' set it is discarded if (ros2_modules.find(mod.name()) != ros2_modules.end()) { return; @@ -454,7 +501,7 @@ class SystemHandle : public virtual FullSystem return; } - // Check whether the following submodule is msg or srv + // Check whether it has a submodule that corresponds to msg or srv if (!mod.has_submodule("msg") && !mod.has_submodule("srv")) { logger_ << utils::Logger::Level::ERROR @@ -465,17 +512,21 @@ class SystemHandle : public virtual FullSystem return; } + // Create a new folder with the name of the ROS 2 package within the temporal location + // and inside it another called msg/srv to store the .idl files that will be created std::filesystem::path tmp = std::filesystem::temp_directory_path(); //TODO: Change when the services are implemented std::filesystem::create_directories(tmp / mod.name() / "msg"); + // Save the package_name for cleaning the temporal location when the execution is ended package_names.insert(mod.name()); + // Gets from the specific submodule a map with the name and the dynamic type associated auto all_types = context.module().submodule(mod.name()).get()->get_all_types(); for (auto [name, dtype] : all_types) { - // Check if it is a Structure + // Check if that the type is an Structure (ROS 2 restriction) if (dtype.get()->kind() != xtypes::TypeKind::STRUCTURE_TYPE) { logger_ << utils::Logger::Level::ERROR @@ -499,6 +550,7 @@ class SystemHandle : public virtual FullSystem } }; + // Copy the custom IDLs to the created folder using the previously stored paths for (const auto& mv_idl : custom_include_paths) { std::string filename = std::filesystem::path(mv_idl).filename(); @@ -514,6 +566,9 @@ class SystemHandle : public virtual FullSystem }, false); + // Due to a rosidl restriction each IDL file can only contain one structure definition. + // The generate function take the module xtypes objects and generate the associated IDL + // strings. Each map entry represents the name of the type and the generated IDL. std::map resulting_idl; auto m_idl = eprosima::xtypes::idl::generate( static_cast @@ -522,7 +577,8 @@ class SystemHandle : public virtual FullSystem std::string depends = ""; - // The Xtypes generator generates code also for the ROS2 types, which is not neccessary + // The Xtypes generator generates code also for the ROS2 types, which is not neccessary. + // Each of them are marked as removed std::set remove; if (!ros2_modules.empty()) { @@ -538,6 +594,10 @@ class SystemHandle : public virtual FullSystem // Transform the dependencies into include clauses for (auto& pair : resulting_idl) { + // The generate function returns to types of entries, the ones having the type name and + // its IDL and the one with the dependencies associated to a specific type + + // If the pair corresponds with a type_name-IDL entry and it is not a ROS 2 message if (pair.first.find(":dependencies") == std::string::npos && remove.find(pair.second) == remove.end()) { @@ -545,6 +605,8 @@ class SystemHandle : public virtual FullSystem logger_ << utils::Logger::Level::DEBUG << "[" << pair.first << "]"; + // If the IDL associated to a ROS 2 message is within the current type IDL, it is + // removed before continuing for (auto& r_str : remove) { auto pos = pair.second.find(r_str); @@ -555,15 +617,18 @@ class SystemHandle : public virtual FullSystem } + // Find within the map the dependencies entry associated with the current type auto it = resulting_idl.find(pair.first + ":dependencies"); if (it != resulting_idl.end()) { + // Generate the include clauses based on the dependencies and add them to the + // type associated IDL std::stringstream ss(resulting_idl[pair.first + ":dependencies"]); std::string str; std::string includes; while (getline(ss, str, ',')) { std::string package = str.substr(0, str.find("::")); - // If the dependency corresponds with the same package as the current idl + // If the dependency corresponds with the same package as the current IDL // don't insert it. It will generate a circular dependency. if (pair.first.find(package) == std::string::npos) { @@ -578,22 +643,28 @@ class SystemHandle : public virtual FullSystem logger_ << pair.second << std::endl; + // Generate a new file within the previously created folder with the type name and + // the IDL as content std::string struct_path = pair.first; replace_all_string(struct_path, "::", "/"); std::ofstream idlfile ("/tmp/" + struct_path + ".idl"); idlfile << pair.second << std::endl; idlfile.close(); + // Fill the command to call the generator.bash file + // This parameter corresponds to the dependencies associated to a specific package if (!dependencies.empty()) { std::ostringstream dep_stream; - std::copy(dependencies.begin(), dependencies.end(), std::ostream_iterator(dep_stream, ";")); - depends += "--" + pair.first.substr(0, pair.first.find("::")) + " \"" + dep_stream.str() + "\" "; + std::copy(dependencies.begin(), dependencies.end(), + std::ostream_iterator(dep_stream, ";")); + depends += "--" + pair.first.substr(0, pair.first.find("::")) + + " \"" + dep_stream.str() + "\" "; } } } - // Call the bash that generates the ROS 2 type support for the idl types + // The rest of parameters correspond to the name of the package and the installation path std::ostringstream pkg_stream; std::copy(package_names.begin(), package_names.end(), std::ostream_iterator(pkg_stream, " ")); const std::string package_name = "--package_name \"" + pkg_stream.str() + "\" "; @@ -603,11 +674,13 @@ class SystemHandle : public virtual FullSystem << "Generating ROS2 Type Support for package: " << package_name << std::endl; + // Then compound the whose command to call the generator.bash std::string command = "exec bash /tmp/generator.bash " + package_name + " " + path + " " + depends; logger_ << utils::Logger::Level::DEBUG << "Command: " << command << std::endl; + // Call the bash that generates the ROS 2 type support for the IDL types FILE* pipe = popen(command.c_str(), "r"); if (!pipe) { @@ -624,6 +697,7 @@ class SystemHandle : public virtual FullSystem output += buffer; } + // Redirect the bash output to the console logger_ << is::utils::Logger::Level::DEBUG << output << std::endl; From 4c000a995d315493b3497b1b1449834c2b0440d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Laura=20Mart=C3=ADn?= <52071480+lauramg15@users.noreply.github.com> Date: Tue, 10 Aug 2021 13:53:30 +0200 Subject: [PATCH 25/34] Unify Logger between System Handles [12339] (#33) * Update version * Add more logger traces Signed-off-by: Miguel Barro --- ros2/CMakeLists.txt | 2 +- ros2/static/src/MetaPublisher.cpp | 6 ++++++ ros2/static/src/SystemHandle.cpp | 10 ++++++++++ ros2/static/src/SystemHandle__foxy.cpp | 10 ++++++++++ .../resources/convert__msg.cpp.em | 18 +++++++++++++++++ .../resources/convert__msg.hpp.em | 5 +++++ .../resources/convert__srv.cpp.em | 20 +++++++++++++++++++ 7 files changed, 70 insertions(+), 1 deletion(-) diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index b8df12f..34de67b 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -20,7 +20,7 @@ ################################################################################## cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) -project(is-ros2 VERSION "3.0.0" LANGUAGES CXX) +project(is-ros2 VERSION "3.1.0" LANGUAGES CXX) ################################################################################### # Configure options diff --git a/ros2/static/src/MetaPublisher.cpp b/ros2/static/src/MetaPublisher.cpp index 0637347..3748eed 100644 --- a/ros2/static/src/MetaPublisher.cpp +++ b/ros2/static/src/MetaPublisher.cpp @@ -46,6 +46,7 @@ class MetaPublisher : public is::TopicPublisher , _message_type(message_type) , _node(node) , _qos_profile(qos_profile) + , logger_("is::sh::ROS2::Publisher") { // Do nothing } @@ -66,6 +67,10 @@ class MetaPublisher : public is::TopicPublisher _message_type, _node, topic_name, _qos_profile); } + logger_ << utils::Logger::Level::INFO + << "Sending message from Integration Service to ROS 2 for topic '" << topic_name + << "': [[ " << message << " ]]" << std::endl; + return publisher->publish(message); } @@ -75,6 +80,7 @@ class MetaPublisher : public is::TopicPublisher const eprosima::xtypes::DynamicType& _message_type; rclcpp::Node& _node; const rclcpp::QoS _qos_profile; + utils::Logger logger_; using TopicPublisherPtr = std::shared_ptr; using PublisherMap = std::unordered_map; diff --git a/ros2/static/src/SystemHandle.cpp b/ros2/static/src/SystemHandle.cpp index a5c8794..6a6eba1 100644 --- a/ros2/static/src/SystemHandle.cpp +++ b/ros2/static/src/SystemHandle.cpp @@ -185,6 +185,11 @@ rclcpp::QoS parse_rmw_qos_configuration( qos.liveliness_lease_duration(lease_duration); } } + else + { + _logger << utils::Logger::Level::INFO + << "Creating entity using the default QoS." << std::endl; + } return qos; } @@ -406,6 +411,11 @@ bool SystemHandle::configure( } } + if (success) + { + _logger << utils::Logger::Level::INFO << "Configured!" << std::endl; + } + return success; } diff --git a/ros2/static/src/SystemHandle__foxy.cpp b/ros2/static/src/SystemHandle__foxy.cpp index a19db57..9c6b475 100644 --- a/ros2/static/src/SystemHandle__foxy.cpp +++ b/ros2/static/src/SystemHandle__foxy.cpp @@ -209,6 +209,11 @@ rclcpp::QoS parse_rmw_qos_configuration( qos.liveliness_lease_duration(lease_duration); } } + else + { + _logger << utils::Logger::Level::INFO + << "Creating entity using the default QoS." << std::endl; + } return qos; } @@ -455,6 +460,11 @@ bool SystemHandle::configure( } } + if (success) + { + _logger << utils::Logger::Level::INFO << "Configured!" << std::endl; + } + return success; } diff --git a/utils/ros2-mix-generator/resources/convert__msg.cpp.em b/utils/ros2-mix-generator/resources/convert__msg.cpp.em index 152bf2d..a4dbf43 100644 --- a/utils/ros2-mix-generator/resources/convert__msg.cpp.em +++ b/utils/ros2-mix-generator/resources/convert__msg.cpp.em @@ -58,6 +58,7 @@ public: const rclcpp::QoS& qos_profile) : _callback(callback) , _message_type(message_type) + , _topic_name(topic_name) { auto subscription_options = rclcpp::SubscriptionOptionsWithAllocator>(); subscription_options.ignore_local_publications = true; // Enable ignore_local_publications option @@ -76,8 +77,16 @@ private: void subscription_callback( const Ros2_Msg& msg) { + logger << utils::Logger::Level::INFO + << "Receiving message from ROS 2 for topic '" + << _topic_name << "'" << std::endl; + xtypes::DynamicData data(_message_type); convert_to_xtype(msg, data); + + logger << utils::Logger::Level::INFO + << "Received message: [[ " << data << " ]]" << std::endl; + (*_callback)(data, nullptr); } @@ -86,6 +95,8 @@ private: const xtypes::DynamicType& _message_type; + std::string _topic_name; + // Hang onto the subscription handle to make sure the connection to the topic // stays alive rclcpp::Subscription::SharedPtr _subscription; @@ -117,6 +128,7 @@ public: rclcpp::Node& node, const std::string& topic_name, const rclcpp::QoS& qos_profile) + : _topic_name(topic_name) { _publisher = node.create_publisher( topic_name, @@ -129,6 +141,10 @@ public: Ros2_Msg ros2_msg; convert_to_ros2(message, ros2_msg); + logger << utils::Logger::Level::INFO + << "Sending message from Integration Service to ROS 2 for topic '" << _topic_name << "': " + << "[[ " << message << " ]]" << std::endl; + _publisher->publish(ros2_msg); return true; } @@ -137,6 +153,8 @@ private: rclcpp::Publisher::SharedPtr _publisher; + std::string _topic_name; + }; //============================================================================== diff --git a/utils/ros2-mix-generator/resources/convert__msg.hpp.em b/utils/ros2-mix-generator/resources/convert__msg.hpp.em index 3b318b2..ee5a340 100644 --- a/utils/ros2-mix-generator/resources/convert__msg.hpp.em +++ b/utils/ros2-mix-generator/resources/convert__msg.hpp.em @@ -60,6 +60,9 @@ alphabetical_fields = sorted(spec.fields, key=lambda x: x.name) // Include the header for the conversions #include +// Include the header for the logger +#include + // Include the header for the concrete ros2 message type #include <@(ros2_msg_dependency)> @@ -124,6 +127,8 @@ inline void convert_to_xtype(const Ros2_Msg& from, eprosima::xtypes::WritableDyn (void)to; } +static eprosima::is::utils::Logger logger ("is::sh::ROS2"); + } // namespace @(namespace_variable) } // namespace ros2 } // namespace sh diff --git a/utils/ros2-mix-generator/resources/convert__srv.cpp.em b/utils/ros2-mix-generator/resources/convert__srv.cpp.em index 93108e5..9b80c6e 100644 --- a/utils/ros2-mix-generator/resources/convert__srv.cpp.em +++ b/utils/ros2-mix-generator/resources/convert__srv.cpp.em @@ -53,6 +53,9 @@ alphabetical_response_fields = sorted(spec.response.fields, key=lambda x: x.name // Include the header for the conversions #include +// Include the header for the logger +#include + // Include the header for the concrete service type #include <@(ros2_srv_dependency)> @@ -75,6 +78,8 @@ namespace sh { namespace ros2 { namespace @(namespace_variable_srv) { +static eprosima::is::utils::Logger logger ("is::sh::ROS2"); + using Ros2_Srv = @(cpp_srv_type); using Ros2_Request = Ros2_Srv::Request; using Ros2_Response = Ros2_Srv::Response; @@ -183,6 +188,7 @@ public: : _callback(callback) , _handle(std::make_shared()) , _request_data(request_type()) + , _service_name(service_name) { _service = node.create_service( service_name, @@ -203,6 +209,11 @@ public: std::static_pointer_cast(call_handle); response_to_ros2(result, _response); + + logger << utils::Logger::Level::INFO + << "Translating reply from Integration Service to ROS 2 for service reply topic '" + << _service_name << "_Reply': [[ " << result << " ]]" << std::endl; + handle->promise->set_value(_response); } @@ -213,6 +224,10 @@ private: const std::shared_ptr& request, const std::shared_ptr& response) { + logger << utils::Logger::Level::INFO + << "Receiving request from ROS 2 for service request topic '" + << _service_name << "_Request'" << std::endl; + request_to_xtype(*request, _request_data); std::promise response_promise; @@ -237,6 +252,7 @@ private: }; ServiceClientSystem::RequestCallback* _callback; + std::string _service_name; const std::shared_ptr _handle; xtypes::DynamicData _request_data; Ros2_Response _response; @@ -288,6 +304,10 @@ public: return; } + logger << utils::Logger::Level::INFO + << "Translating request from Integration Service to ROS 2 for service request topic '" + << _service_name << "_Request': [[ " << request << " ]]" << std::endl; + // This helps the lambda to value-capture the address of the Integration Service client. // TODO(MXG): Would it be dangerous for the lambda to reference-capture the // Integration Service client? The lambda might be called after this reference has left From a8dd8bd73cdee8ca4f758dbb14b6598303c747bb Mon Sep 17 00:00:00 2001 From: Eduardo Ponz Segrelles Date: Wed, 26 Apr 2023 11:15:30 +0200 Subject: [PATCH 26/34] Use correct variable to reference the python3 interpreter (#37) Signed-off-by: Eduardo Ponz Signed-off-by: Miguel Barro --- utils/ros2-mix-generator/CMakeLists.txt | 1 + utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/utils/ros2-mix-generator/CMakeLists.txt b/utils/ros2-mix-generator/CMakeLists.txt index cdfe427..0f542bf 100644 --- a/utils/ros2-mix-generator/CMakeLists.txt +++ b/utils/ros2-mix-generator/CMakeLists.txt @@ -43,6 +43,7 @@ endif() find_package(is-core REQUIRED) find_package(rosidl_parser REQUIRED) find_package(is-ros2 REQUIRED) +find_package(Python3 REQUIRED) list(APPEND MIX_ROS_PACKAGES_LIST std_msgs) diff --git a/utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake b/utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake index 1f52281..f2fd7cb 100644 --- a/utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake +++ b/utils/ros2-mix-generator/cmake/is_ros2_rosidl_mix.cmake @@ -71,7 +71,7 @@ function(is_ros2_rosidl_mix) rosidl SCRIPT INTERPRETER - ${PYTHON_EXECUTABLE} + ${Python3_EXECUTABLE} FIND ${CMAKE_CURRENT_LIST_DIR}/scripts/is_ros2_rosidl_find_package_info.py GENERATE From 6d7a61196aa61e09bccc266496f41cfe31231ee5 Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Thu, 27 Apr 2023 10:06:58 +0200 Subject: [PATCH 27/34] Refs 18334. Fixing tests CMakeLists.txt for humble Signed-off-by: Miguel Barro --- ros2/static/test/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2/static/test/CMakeLists.txt b/ros2/static/test/CMakeLists.txt index b538df0..c6adab2 100644 --- a/ros2/static/test/CMakeLists.txt +++ b/ros2/static/test/CMakeLists.txt @@ -55,6 +55,7 @@ macro(compile_test) target_include_directories(${TEST_NAME} PRIVATE + ${rclcpp_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} ${nav_msgs_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} From 0844470f837dd59e6ae17decfd0bf6d7e95d8391 Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Fri, 28 Apr 2023 09:38:33 +0200 Subject: [PATCH 28/34] Refs 18334. Rely on cmake versioning instead of heuristics to select sources Signed-off-by: Miguel Barro --- README.md | 4 -- ros2/CMakeLists.txt | 105 ++++++++++++++++++++++++++------ ros2/static/test/CMakeLists.txt | 6 +- 3 files changed, 89 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 6e164fa..6693ef3 100644 --- a/README.md +++ b/README.md @@ -294,10 +294,6 @@ whole *Integration Service* product suite, there are some specific flags which a ~/is_ws$ colcon build --cmake-args -DBUILD_ROS2_TESTS=ON ``` -* `IS_ROS2_DISTRO`: This flag is intended to select the *ROS 2* distro that should be used to compile the *ROS 2 System Handle*. - If not set, the version will be retrieved from the last *ROS distro* sourced in the compilation environment; - this means that if the last *ROS* environment sourced corresponds to *ROS 1*, the compilation process will stop and warn the user about it. - * `IS_ROS2_SH_MODE`: This flag is to decide which *ROS 2 System Handle* mode will be compiled, as the static and dynamic modes are exclusive which means that they cannot be compiled at the same time. It accepts two different values: `static` or `dynamic`. The following flags are only applicable for the *Static ROS 2 System Handle*: diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index 34de67b..e28cbfb 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -32,29 +32,96 @@ set(IS_ROS2_SH_MODE "Static" CACHE STRING "Select the ROS 2 SystemHandle Mode") # Select the proper ROS 2 Distro to use ################################################################################### if(BUILD_LIBRARY) - # Check if the user specified which ROS 2 version he/she is using. If not, warn and try to automatically get it - if(NOT IS_ROS2_DISTRO) - - message(WARNING " - The variable 'IS_ROS2_DISTRO' was not used. You might want to set it to - specify which ROS 2 version should be used to compile the ROS 2 System Handle. - By default, a ROS 2 version from the sourced environment will be retrieved automatically.") - - if ("$ENV{ROS_VERSION}" STREQUAL "1") - message(FATAL_ERROR " - A ROS 1 version was sourced last in your build environment. - Please use the 'IS_ROS2_VERSION' variable!") - else() - set(IS_ROS2_DISTRO $ENV{ROS_DISTRO}) - endif() + find_package(is-core REQUIRED) + find_package(rclcpp REQUIRED) +endif() +################################################################################### +# Configure the Integration Service ROS 2 SystemHandle library +################################################################################### +if(BUILD_LIBRARY) + add_library(${PROJECT_NAME} + SHARED + src/Factory.cpp + src/MetaPublisher.cpp + $,src/SystemHandle.cpp,src/SystemHandle__foxy.cpp> + ) + + if (Sanitizers_FOUND) + add_sanitizers(${PROJECT_NAME}) endif() - message(STATUS "Compiling the ${PROJECT_NAME} System Handle for ROS 2 ${IS_ROS2_DISTRO}...") + set_target_properties(${PROJECT_NAME} PROPERTIES + VERSION + ${PROJECT_VERSION} + SOVERSION + ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + CXX_STANDARD + 17 + CXX_STANDARD_REQUIRED + YES + ) + + target_compile_options(${PROJECT_NAME} + PRIVATE + $<$:-pedantic> + $<$:-fstrict-aliasing> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wcast-align> + $<$:-Wshadow> + $<$:/W4> + $<$:/wd4700> + $<$:/wd4996> + $<$:/wd4820> + $<$:/wd4255> + $<$:/wd4668> + ) + + include(GNUInstallDirs) + message(STATUS "Configuring [${PROJECT_NAME}]...") + + target_link_libraries(${PROJECT_NAME} + PUBLIC + is::core + ${rclcpp_LIBRARIES} + ) + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${rclcpp_INCLUDE_DIRS} + ) + + is_generate_export_header(ros2) + + if (NOT rosidl_runtime_cpp_FOUND) + target_compile_definitions(${PROJECT_NAME} + PUBLIC + "IS_SH_ROS2__ROSIDL_GENERATOR_CPP" + ) + endif() +endif() - # Generate the config.hpp file - configure_file(${PROJECT_SOURCE_DIR}/config.hpp.in - ${CMAKE_INSTALL_PREFIX}/include/is/sh/ros2/config.hpp +################################################################################### +# Install the Integration Service ROS 2 SystemHandle library +################################################################################### +if(BUILD_LIBRARY) + is_install_middleware_plugin( + MIDDLEWARE + ros2 + TARGET + ${PROJECT_NAME} + DEPENDENCIES + rclcpp + ) + + file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/include/ + DESTINATION + ${CMAKE_INSTALL_PREFIX}/include/ ) endif() diff --git a/ros2/static/test/CMakeLists.txt b/ros2/static/test/CMakeLists.txt index c6adab2..7bd3d4e 100644 --- a/ros2/static/test/CMakeLists.txt +++ b/ros2/static/test/CMakeLists.txt @@ -83,10 +83,10 @@ compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msg compile_test(${PROJECT_NAME}_primitive_msgs SOURCE integration/ros2__primitives_msgs.cpp) compile_test(${PROJECT_NAME}_test_qos SOURCE integration/ros2__test_qos.cpp) -if ("${IS_ROS2_DISTRO}" STREQUAL "foxy") - compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) -else() +if (rclcpp_VERSION VERSION_GREATER 2.4.2) compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain.cpp) +else() + compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) endif() set(test_msgs_config_file "ros2__test_msgs.yaml") From 79da828d3a0edea788cbffead08c023343edbe62 Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Wed, 3 May 2023 15:26:33 +0200 Subject: [PATCH 29/34] fix flaky geometry msgs test accounting slow performance of debug binaries Signed-off-by: Miguel Barro --- ros2/static/test/integration/ros2__geometry_msgs.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ros2/static/test/integration/ros2__geometry_msgs.cpp b/ros2/static/test/integration/ros2__geometry_msgs.cpp index ce69dfa..dbb9a4b 100644 --- a/ros2/static/test/integration/ros2__geometry_msgs.cpp +++ b/ros2/static/test/integration/ros2__geometry_msgs.cpp @@ -401,12 +401,12 @@ TEST(ROS2, Request_reply_between_ros2_and_mock) // Make sure that we got the expected request message auto start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 30s) + while (std::chrono::steady_clock::now() - start_time < 1min) { executor.spin_some(); // Note: future_goal gets set after future_start, so waiting on // future_goal alone is sufficient for waiting on both. - if (future_goal.wait_for(100ms) == std::future_status::ready) + if (future_goal.wait_for(1s) == std::future_status::ready) { break; } @@ -420,10 +420,10 @@ TEST(ROS2, Request_reply_between_ros2_and_mock) EXPECT_EQ(requested_goal, plan_response.plan.poses.back()); start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 30s) + while (std::chrono::steady_clock::now() - start_time < 1min) { executor.spin_some(); - if (future_response_msg.wait_for(100ms) == std::future_status::ready) + if (future_response_msg.wait_for(1s) == std::future_status::ready) { break; } @@ -457,10 +457,10 @@ TEST(ROS2, Request_reply_between_ros2_and_mock) // should quit instead of waiting for the future and potentially hanging // forever. start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 30s) + while (std::chrono::steady_clock::now() - start_time < 1min) { executor.spin_some(); - if (future_response.wait_for(100ms) == std::future_status::ready) + if (future_response.wait_for(1s) == std::future_status::ready) { break; } From 35549b6ee33d38feae084c1c14d01807c676c938 Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Mon, 29 May 2023 12:59:22 +0200 Subject: [PATCH 30/34] missing fixes in the rebase Signed-off-by: Miguel Barro --- .github/workflows/ci.yml | 15 +- ros2/CMakeLists.txt | 97 --------- ros2/dynamic/CMakeLists.txt | 201 ++++++++++-------- ros2/dynamic/test/CMakeLists.txt | 10 +- .../test/integration/ros2__geometry_msgs.cpp | 8 +- ros2/static/CMakeLists.txt | 176 ++++++++------- 6 files changed, 211 insertions(+), 296 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8f5dbda..2289334 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -20,14 +20,14 @@ jobs: ros2-sh_CI: strategy: matrix: - ros2_version: [foxy, galactic] + ros2_version: [foxy, galactic, humble] sh_mode: [static, dynamic] - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 container: ros:${{ matrix.ros2_version }} steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: path: src/ros2-sh @@ -38,15 +38,14 @@ jobs: - name: Download the Integration Service run: | - git clone --recursive https://github.com/eProsima/xtypes.git -b feature/ros2_sh_dynamic src/xtypes - git clone https://github.com/eProsima/Integration-Service -b feature/ros2_dynamic src/integration-service + git clone https://github.com/eProsima/Integration-Service -b feature/node-fiware-support src/integration-service - name: Build run: | - . /opt/ros/${{ matrix.ros2_version }}/setup.sh - colcon build --cmake-args -DCMAKE_BUILD_TYPE=DEBUG -DBUILD_ROS2_TESTS=ON -DIS_ROS2_SH_MODE=${{ matrix.sh_mode }} --event-handlers console_direct+ + /ros_entrypoint.sh colcon build --cmake-args -DCMAKE_BUILD_TYPE=DEBUG -DBUILD_ROS2_TESTS=ON -DIS_ROS2_SH_MODE=${{ matrix.sh_mode }} --event-handlers console_direct+ - name: Test run: | - . /opt/ros/${{ matrix.ros2_version }}/setup.sh && . install/local_setup.sh && RMW_IMPLEMENTATION=rmw_fastrtps_cpp colcon test --packages-select is-ros2 --event-handlers console_direct+ + . install/setup.sh + RMW_IMPLEMENTATION=rmw_fastrtps_cpp colcon test --packages-select is-ros2 --event-handlers console_direct+ colcon test-result diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt index e28cbfb..44aad95 100644 --- a/ros2/CMakeLists.txt +++ b/ros2/CMakeLists.txt @@ -28,103 +28,6 @@ project(is-ros2 VERSION "3.1.0" LANGUAGES CXX) option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) set(IS_ROS2_SH_MODE "Static" CACHE STRING "Select the ROS 2 SystemHandle Mode") -################################################################################### -# Select the proper ROS 2 Distro to use -################################################################################### -if(BUILD_LIBRARY) - find_package(is-core REQUIRED) - find_package(rclcpp REQUIRED) -endif() - -################################################################################### -# Configure the Integration Service ROS 2 SystemHandle library -################################################################################### -if(BUILD_LIBRARY) - add_library(${PROJECT_NAME} - SHARED - src/Factory.cpp - src/MetaPublisher.cpp - $,src/SystemHandle.cpp,src/SystemHandle__foxy.cpp> - ) - - if (Sanitizers_FOUND) - add_sanitizers(${PROJECT_NAME}) - endif() - - set_target_properties(${PROJECT_NAME} PROPERTIES - VERSION - ${PROJECT_VERSION} - SOVERSION - ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} - CXX_STANDARD - 17 - CXX_STANDARD_REQUIRED - YES - ) - - target_compile_options(${PROJECT_NAME} - PRIVATE - $<$:-pedantic> - $<$:-fstrict-aliasing> - $<$:-Wall> - $<$:-Wextra> - $<$:-Wcast-align> - $<$:-Wshadow> - $<$:/W4> - $<$:/wd4700> - $<$:/wd4996> - $<$:/wd4820> - $<$:/wd4255> - $<$:/wd4668> - ) - - include(GNUInstallDirs) - message(STATUS "Configuring [${PROJECT_NAME}]...") - - target_link_libraries(${PROJECT_NAME} - PUBLIC - is::core - ${rclcpp_LIBRARIES} - ) - - target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ - ${rclcpp_INCLUDE_DIRS} - ) - - is_generate_export_header(ros2) - - if (NOT rosidl_runtime_cpp_FOUND) - target_compile_definitions(${PROJECT_NAME} - PUBLIC - "IS_SH_ROS2__ROSIDL_GENERATOR_CPP" - ) - endif() -endif() - -################################################################################### -# Install the Integration Service ROS 2 SystemHandle library -################################################################################### -if(BUILD_LIBRARY) - is_install_middleware_plugin( - MIDDLEWARE - ros2 - TARGET - ${PROJECT_NAME} - DEPENDENCIES - rclcpp - ) - - file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/include/ - DESTINATION - ${CMAKE_INSTALL_PREFIX}/include/ - ) -endif() - ################################################################################### # Build ROS2 Static SH or ROS2 Dynamic SH ################################################################################### diff --git a/ros2/dynamic/CMakeLists.txt b/ros2/dynamic/CMakeLists.txt index 8aaedad..34aef7a 100644 --- a/ros2/dynamic/CMakeLists.txt +++ b/ros2/dynamic/CMakeLists.txt @@ -25,121 +25,138 @@ cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) ################################################################################### option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) +if(NOT BUILD_LIBRARY) + return() +endif() + ################################################################################### # Load external CMake Modules. ################################################################################### -if(BUILD_LIBRARY) - set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) - find_package(Sanitizers QUIET) +find_package(Sanitizers QUIET) - if(SANITIZE_ADDRESS) - message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") - endif() +if(SANITIZE_ADDRESS) + message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") endif() +# identify the distro +if(ENV{ROS_DISTRO}) + set(IS_ROS2_DISTRO "ENV{ROS_DISTRO}") +else() + find_package(ros_environment REQUIRED) + + file(GLOB_RECURSE hook_files "${ros_environment_DIR}/../*ros_distro.sh" "${ros_environment_DIR}/../*ros_distro.bat") + file(STRINGS ${hook_files} export_statement REGEX "ROS_DISTRO=") + string(REGEX MATCH "ROS_DISTRO=(.*)" hook_files "${export_statement}") + unset(hook_files) + unset(export_statement) + + set(IS_ROS2_DISTRO "${CMAKE_MATCH_1}") +endif() + +# Generate the config.hpp file +configure_file(${PROJECT_SOURCE_DIR}/config.hpp.in + ${CMAKE_CURRENT_BINARY_DIR}/include/config.hpp + ) + +include(GNUInstallDirs) + +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/include/config.hpp + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/is/sh/ros2 + ) + +# install the headers +install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + ################################################################################### # External dependencies for the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - find_package(is-core REQUIRED) - find_package(fastrtps REQUIRED) -endif() +find_package(is-core REQUIRED) +find_package(fastrtps REQUIRED) ################################################################################### # Configure the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - add_library(${PROJECT_NAME} - SHARED - src/Conversion.cpp - src/Participant.cpp - src/Publisher.cpp - src/Subscriber.cpp - src/SystemHandle.cpp +add_library(${PROJECT_NAME} + SHARED + src/Conversion.cpp + src/Participant.cpp + src/Publisher.cpp + src/Subscriber.cpp + src/SystemHandle.cpp +) + +if (Sanitizers_FOUND) + add_sanitizers(${PROJECT_NAME}) +endif() + +set_target_properties(${PROJECT_NAME} PROPERTIES + VERSION + ${PROJECT_VERSION} + SOVERSION + ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + CXX_STANDARD + 17 + CXX_STANDARD_REQUIRED + YES ) - if (Sanitizers_FOUND) - add_sanitizers(${PROJECT_NAME}) - endif() - - set_target_properties(${PROJECT_NAME} PROPERTIES - VERSION - ${PROJECT_VERSION} - SOVERSION - ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} - CXX_STANDARD - 17 - CXX_STANDARD_REQUIRED - YES - ) - - target_compile_options(${PROJECT_NAME} - PRIVATE - $<$:-pedantic> - $<$:-fstrict-aliasing> - $<$:-Wall> - $<$:-Wextra> - $<$:-Wcast-align> - $<$:-Wshadow> - $<$:/W4> - $<$:/wd4700> - $<$:/wd4996> - $<$:/wd4820> - $<$:/wd4255> - $<$:/wd4668> - ) - - include(GNUInstallDirs) - message(STATUS "Configuring [${PROJECT_NAME}]...") - - target_link_libraries(${PROJECT_NAME} - PUBLIC - is::core - fastrtps - ) - - target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ - $ - $ - ) -endif() +target_compile_options(${PROJECT_NAME} + PRIVATE + $<$:-pedantic> + $<$:-fstrict-aliasing> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wcast-align> + $<$:-Wshadow> + $<$:/W4> + $<$:/wd4700> + $<$:/wd4996> + $<$:/wd4820> + $<$:/wd4255> + $<$:/wd4668> + ) + +include(GNUInstallDirs) +message(STATUS "Configuring [${PROJECT_NAME}]...") + +target_link_libraries(${PROJECT_NAME} + PUBLIC + is::core + fastrtps + ) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + $ + ) ################################################################################### # Install the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - is_install_middleware_plugin( - MIDDLEWARE - ros2_dynamic - TARGET - ${PROJECT_NAME} - ) - - file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/include/ - DESTINATION - ${CMAKE_INSTALL_PREFIX}/include/ +is_install_middleware_plugin( + MIDDLEWARE + ros2_dynamic + TARGET + ${PROJECT_NAME} ) - file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/resources/ - DESTINATION - /tmp/ - ) -endif() +file( + COPY + ${CMAKE_CURRENT_LIST_DIR}/resources/ + DESTINATION + /tmp/ +) ################################################################################### # Integration Service ROS 2 SystemHandle tests ################################################################################### -if(BUILD_LIBRARY) - if(BUILD_TESTS OR BUILD_ROS2_TESTS) - enable_testing() - add_subdirectory(test) - endif() -endif() \ No newline at end of file +if(BUILD_TESTS OR BUILD_ROS2_TESTS) + enable_testing() + add_subdirectory(test) +endif() diff --git a/ros2/dynamic/test/CMakeLists.txt b/ros2/dynamic/test/CMakeLists.txt index 16e15d3..5f1d529 100644 --- a/ros2/dynamic/test/CMakeLists.txt +++ b/ros2/dynamic/test/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(rclcpp REQUIRED) # Get message dependencies find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(std_msgs REQUIRED) macro(compile_test) @@ -46,6 +47,7 @@ macro(compile_test) is::mock ${rclcpp_LIBRARIES} ${geometry_msgs_LIBRARIES} + ${nav_msgs_LIBRARIES} ${std_msgs_LIBRARIES} PRIVATE $,libgtest,gtest> @@ -53,6 +55,8 @@ macro(compile_test) target_include_directories(${TEST_NAME} PRIVATE + ${rclcpp_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} ) @@ -79,10 +83,10 @@ compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msg compile_test(${PROJECT_NAME}_check_ros2pkg_creation SOURCE integration/check_ros2pkg_creation.cpp ENVIRONMENTS "ROS2_DISTRO=${IS_ROS2_DISTRO}") compile_test(${PROJECT_NAME}_test_qos SOURCE integration/ros2__test_qos.cpp) -if ("${IS_ROS2_DISTRO}" STREQUAL "foxy") - compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) -else() +if (rclcpp_VERSION VERSION_GREATER 2.4.2) compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain.cpp) +else() + compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) endif() set(test_msgs_config_file "ros2__test_msgs.yaml") diff --git a/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp b/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp index 2b1b795..32b7d02 100644 --- a/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp +++ b/ros2/dynamic/test/integration/ros2__geometry_msgs.cpp @@ -246,10 +246,10 @@ TEST(ROS2Dynamic, Publish_subscribe_between_ros2_and_mock) // we should quit instead of waiting for the future and potentially hanging // forever. auto start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 30s) + while (std::chrono::steady_clock::now() - start_time < 1min) { executor.spin_some(); - if (msg_future.wait_for(100ms) == std::future_status::ready) + if (msg_future.wait_for(1s) == std::future_status::ready) { break; } @@ -309,14 +309,14 @@ TEST(ROS2Dynamic, Publish_subscribe_between_ros2_and_mock) // with Integratoion Service, and we should quit instead of waiting for the future and // potentially hanging forever. start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 30s) + while (std::chrono::steady_clock::now() - start_time < 1min) { executor.spin_some(); is::sh::mock::publish_message("echo_pose", received_msg); executor.spin_some(); - if (pose_future.wait_for(100ms) == std::future_status::ready) + if (pose_future.wait_for(1s) == std::future_status::ready) { break; } diff --git a/ros2/static/CMakeLists.txt b/ros2/static/CMakeLists.txt index 8751c79..e0a2ed1 100644 --- a/ros2/static/CMakeLists.txt +++ b/ros2/static/CMakeLists.txt @@ -25,121 +25,113 @@ cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) ################################################################################### option(BUILD_LIBRARY "Compile the ROS 2 SystemHandle" ON) +if(NOT BUILD_LIBRARY) + return() +endif() + ################################################################################### # Load external CMake Modules. ################################################################################### -if(BUILD_LIBRARY) - set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${SANITIZERS_ROOT}/cmake) - find_package(Sanitizers QUIET) +find_package(Sanitizers QUIET) - if(SANITIZE_ADDRESS) - message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") - endif() +if(SANITIZE_ADDRESS) + message(STATUS "Preloading AddressSanitizer library could be done using \"${ASan_WRAPPER}\" to run your program.") endif() ################################################################################### # External dependencies for the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - find_package(is-core REQUIRED) - find_package(rclcpp REQUIRED) -endif() +find_package(is-core REQUIRED) +find_package(rclcpp REQUIRED) ################################################################################### # Configure the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - add_library(${PROJECT_NAME} - SHARED - src/Factory.cpp - $,src/SystemHandle__foxy.cpp,src/SystemHandle.cpp> - src/MetaPublisher.cpp - ) - - if (Sanitizers_FOUND) - add_sanitizers(${PROJECT_NAME}) - endif() - - set_target_properties(${PROJECT_NAME} PROPERTIES - VERSION - ${PROJECT_VERSION} - SOVERSION - ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} - CXX_STANDARD - 17 - CXX_STANDARD_REQUIRED - YES - ) - - target_compile_options(${PROJECT_NAME} - PRIVATE - $<$:-pedantic> - $<$:-fstrict-aliasing> - $<$:-Wall> - $<$:-Wextra> - $<$:-Wcast-align> - $<$:-Wshadow> - $<$:/W4> - $<$:/wd4700> - $<$:/wd4996> - $<$:/wd4820> - $<$:/wd4255> - $<$:/wd4668> - ) - - include(GNUInstallDirs) - message(STATUS "Configuring [${PROJECT_NAME}]...") - - target_link_libraries(${PROJECT_NAME} - PUBLIC - is::core - ${rclcpp_LIBRARIES} - ) - target_include_directories(${PROJECT_NAME} +add_library(${PROJECT_NAME} + SHARED + src/Factory.cpp + src/MetaPublisher.cpp + $,src/SystemHandle.cpp,src/SystemHandle__foxy.cpp> + ) + +if (Sanitizers_FOUND) + add_sanitizers(${PROJECT_NAME}) +endif() + +set_target_properties(${PROJECT_NAME} PROPERTIES + VERSION + ${PROJECT_VERSION} + SOVERSION + ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + CXX_STANDARD + 17 + CXX_STANDARD_REQUIRED + YES + ) + +target_compile_options(${PROJECT_NAME} + PRIVATE + $<$:-pedantic> + $<$:-fstrict-aliasing> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wcast-align> + $<$:-Wshadow> + $<$:/W4> + $<$:/wd4700> + $<$:/wd4996> + $<$:/wd4820> + $<$:/wd4255> + $<$:/wd4668> + ) + +include(GNUInstallDirs) +message(STATUS "Configuring [${PROJECT_NAME}]...") + +# install the headers +install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + is::core + ${rclcpp_LIBRARIES} + ) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${rclcpp_INCLUDE_DIRS} + ) + +is_generate_export_header(ros2) + +if (NOT rosidl_runtime_cpp_FOUND) + target_compile_definitions(${PROJECT_NAME} PUBLIC - $ - $ - ${rclcpp_INCLUDE_DIRS} - ) - - is_generate_export_header(ros2) - - if (NOT rosidl_runtime_cpp_FOUND) - target_compile_definitions(${PROJECT_NAME} - PUBLIC - "IS_SH_ROS2__ROSIDL_GENERATOR_CPP" - ) - endif() + "IS_SH_ROS2__ROSIDL_GENERATOR_CPP" + ) endif() ################################################################################### # Install the Integration Service ROS 2 SystemHandle library ################################################################################### -if(BUILD_LIBRARY) - is_install_middleware_plugin( - MIDDLEWARE - ros2 - TARGET - ${PROJECT_NAME} - DEPENDENCIES - rclcpp - ) - - file( - COPY - ${CMAKE_CURRENT_LIST_DIR}/include/ - DESTINATION - ${CMAKE_INSTALL_PREFIX}/include/ +is_install_middleware_plugin( + MIDDLEWARE + ros2 + TARGET + ${PROJECT_NAME} + DEPENDENCIES + rclcpp ) -endif() ################################################################################### # Integration Service ROS 2 SystemHandle tests ################################################################################### -if(BUILD_LIBRARY) - if(BUILD_TESTS OR BUILD_ROS2_TESTS) - add_subdirectory(test) - endif() -endif() \ No newline at end of file +if(BUILD_TESTS OR BUILD_ROS2_TESTS) + add_subdirectory(test) +endif() From b8d7fda3068823e0ba7dac498b7cd22d344531e4 Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Tue, 30 May 2023 08:36:23 +0200 Subject: [PATCH 31/34] refactor CMake for non-docker use Signed-off-by: Miguel Barro --- ros2/dynamic/CMakeLists.txt | 2 +- ros2/dynamic/resources/generator.bash | 6 +++--- ros2/dynamic/src/SystemHandle.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ros2/dynamic/CMakeLists.txt b/ros2/dynamic/CMakeLists.txt index 34aef7a..7dedb4f 100644 --- a/ros2/dynamic/CMakeLists.txt +++ b/ros2/dynamic/CMakeLists.txt @@ -150,7 +150,7 @@ file( COPY ${CMAKE_CURRENT_LIST_DIR}/resources/ DESTINATION - /tmp/ + /tmp/is_ros2_sh ) ################################################################################### diff --git a/ros2/dynamic/resources/generator.bash b/ros2/dynamic/resources/generator.bash index 4dbabcf..51696ac 100755 --- a/ros2/dynamic/resources/generator.bash +++ b/ros2/dynamic/resources/generator.bash @@ -20,9 +20,9 @@ for pkg in $package_name; do echo "Generating Type Support for package $pkg with dependencies ${!pkg}"; - cp /tmp/CMakeLists.txt /tmp/$pkg/ + cp /tmp/is_ros2_sh/CMakeLists.txt /tmp/$pkg/ - sed "s#\([^<][^<]*\)#$pkg#" /tmp/package.xml > /tmp/$pkg/package.xml + sed "s#\([^<][^<]*\)#$pkg#" /tmp/is_ros2_sh/package.xml > /tmp/$pkg/package.xml cd /tmp/$pkg @@ -41,4 +41,4 @@ for pkg in $package_name; do done -. $install_path/setup.bash \ No newline at end of file +. $install_path/setup.bash diff --git a/ros2/dynamic/src/SystemHandle.cpp b/ros2/dynamic/src/SystemHandle.cpp index d8316a7..9aa3026 100644 --- a/ros2/dynamic/src/SystemHandle.cpp +++ b/ros2/dynamic/src/SystemHandle.cpp @@ -675,7 +675,7 @@ class SystemHandle : public virtual FullSystem << std::endl; // Then compound the whose command to call the generator.bash - std::string command = "exec bash /tmp/generator.bash " + package_name + " " + path + " " + depends; + std::string command = "exec bash /tmp/is_ros2_sh/generator.bash " + package_name + " " + path + " " + depends; logger_ << utils::Logger::Level::DEBUG << "Command: " << command << std::endl; From da311589ec6cb9b304e93e1d497d2b21e6e34a73 Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Tue, 30 May 2023 10:31:14 +0200 Subject: [PATCH 32/34] fixing building issues Signed-off-by: Miguel Barro --- ros2/dynamic/CMakeLists.txt | 22 +++++--- ros2/dynamic/src/Participant.cpp | 2 +- ros2/dynamic/test/CMakeLists.txt | 54 +++++++++++-------- .../integration/check_ros2pkg_creation.cpp | 15 ++---- 4 files changed, 54 insertions(+), 39 deletions(-) diff --git a/ros2/dynamic/CMakeLists.txt b/ros2/dynamic/CMakeLists.txt index 7dedb4f..7769582 100644 --- a/ros2/dynamic/CMakeLists.txt +++ b/ros2/dynamic/CMakeLists.txt @@ -46,7 +46,7 @@ if(ENV{ROS_DISTRO}) else() find_package(ros_environment REQUIRED) - file(GLOB_RECURSE hook_files "${ros_environment_DIR}/../*ros_distro.sh" "${ros_environment_DIR}/../*ros_distro.bat") + file(GLOB_RECURSE hook_files "${ros_environment_DIR}/../*ros_distro.sh" "${ros_environment_DIR}/../*ros_distro.bat") file(STRINGS ${hook_files} export_statement REGEX "ROS_DISTRO=") string(REGEX MATCH "ROS_DISTRO=(.*)" hook_files "${export_statement}") unset(hook_files) @@ -57,13 +57,22 @@ endif() # Generate the config.hpp file configure_file(${PROJECT_SOURCE_DIR}/config.hpp.in - ${CMAKE_CURRENT_BINARY_DIR}/include/config.hpp + ${CMAKE_CURRENT_BINARY_DIR}/include/is/sh/ros2/config.hpp ) include(GNUInstallDirs) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/include/config.hpp - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/is/sh/ros2 +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/include/is/sh/ros2/config.hpp + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/is/sh/ros2 + ) + +# Use an interface target to distribute the distro dependency +add_library(is-ros2-distro INTERFACE) + +target_include_directories(is-ros2-distro + INTERFACE + $ + $ ) # install the headers @@ -126,14 +135,13 @@ target_link_libraries(${PROJECT_NAME} PUBLIC is::core fastrtps + is-ros2-distro ) target_include_directories(${PROJECT_NAME} PUBLIC $ $ - $ - $ ) ################################################################################### @@ -144,6 +152,8 @@ is_install_middleware_plugin( ros2_dynamic TARGET ${PROJECT_NAME} + INTERFACES + is-ros2-distro ) file( diff --git a/ros2/dynamic/src/Participant.cpp b/ros2/dynamic/src/Participant.cpp index e7efd6c..184a6c3 100644 --- a/ros2/dynamic/src/Participant.cpp +++ b/ros2/dynamic/src/Participant.cpp @@ -241,7 +241,7 @@ const fastrtps::types::DynamicType* Participant::get_dynamic_type( return nullptr; } - return static_cast(it->second.GetDynamicType().get()); + return static_cast(it->second.GetDynamicType().get()); } const std::string& Participant::get_topic_type( diff --git a/ros2/dynamic/test/CMakeLists.txt b/ros2/dynamic/test/CMakeLists.txt index 5f1d529..76f8703 100644 --- a/ros2/dynamic/test/CMakeLists.txt +++ b/ros2/dynamic/test/CMakeLists.txt @@ -37,28 +37,40 @@ macro(compile_test) else() set(TEST_NAME "${ARGV0}") endif() + set(options NO_ROS_DEPENDENCIES) set(multiValueArgs SOURCE ENVIRONMENTS) - cmake_parse_arguments(TEST "" "${uniValueArgs}" "${multiValueArgs}" ${ARGN}) + cmake_parse_arguments(TEST "${options}" "${uniValueArgs}" "${multiValueArgs}" ${ARGN}) add_executable(${TEST_NAME} ${TEST_SOURCE}) - target_link_libraries(${TEST_NAME} - PUBLIC - is::mock + if(NOT TEST_NO_ROS_DEPENDENCIES) + set(link_dependencies ${rclcpp_LIBRARIES} ${geometry_msgs_LIBRARIES} ${nav_msgs_LIBRARIES} ${std_msgs_LIBRARIES} + ) + + set(include_dependencies + ${rclcpp_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} + ) + endif() + + target_link_libraries(${TEST_NAME} + PUBLIC + is::mock + is-ros2-distro + ${link_dependencies} PRIVATE $,libgtest,gtest> ) target_include_directories(${TEST_NAME} PRIVATE - ${rclcpp_INCLUDE_DIRS} - ${nav_msgs_INCLUDE_DIRS} - ${geometry_msgs_INCLUDE_DIRS} - ${std_msgs_INCLUDE_DIRS} + ${include_dependencies} ) target_compile_options(${TEST_NAME} @@ -67,12 +79,20 @@ macro(compile_test) $<$:-Wextra> ) + target_compile_definitions(${TEST_NAME} + PRIVATE + "ROS2__ROSIDL__BUILD_DIR=\"${CMAKE_BINARY_DIR}/is/rosidl/ros2/lib\"" + ) + set_target_properties(${TEST_NAME} PROPERTIES CXX_STANDARD 17 ) - add_gtest(${TEST_NAME} SOURCES ${TEST_SOURCE} ENVIRONMENTS ${TEST_ENVIRONMENTS}) + add_gtest(${TEST_NAME} SOURCES ${TEST_SOURCE}) + + unset(link_dependencies) + unset(include_dependencies) endmacro() include(CTest) @@ -80,7 +100,9 @@ include(${IS_GTEST_CMAKE_MODULE_DIR}/gtest.cmake) enable_testing() compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msgs.cpp) -compile_test(${PROJECT_NAME}_check_ros2pkg_creation SOURCE integration/check_ros2pkg_creation.cpp ENVIRONMENTS "ROS2_DISTRO=${IS_ROS2_DISTRO}") +compile_test(${PROJECT_NAME}_check_ros2pkg_creation + NO_ROS_DEPENDENCIES + SOURCE integration/check_ros2pkg_creation.cpp) compile_test(${PROJECT_NAME}_test_qos SOURCE integration/ros2__test_qos.cpp) if (rclcpp_VERSION VERSION_GREATER 2.4.2) @@ -89,18 +111,6 @@ else() compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) endif() -set(test_msgs_config_file "ros2__test_msgs.yaml") - -set_property( - TARGET - ${PROJECT_NAME}_check_ros2pkg_creation - ${PROJECT_NAME}_geometry_msgs - ${PROJECT_NAME}_test_domain - ${PROJECT_NAME}_test_qos - APPEND PROPERTY COMPILE_DEFINITIONS PRIVATE - "ROS2__ROSIDL__BUILD_DIR=\"${CMAKE_BINARY_DIR}/is/rosidl/ros2/lib\"" - ) - # Windows dll dependencies installation if(WIN32) find_file(MOCKDLL NAMES "is-mock.dll" PATHS "${is-mock_DIR}" PATH_SUFFIXES "lib" ) diff --git a/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp b/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp index 6f27350..c15095b 100644 --- a/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp +++ b/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp @@ -21,14 +21,9 @@ #include #include -// #include - -// TODO: resolve this include #include -// TODO remove this workaround -// #define ROS2_DISTRO "foxy" +#include #include -#include #include #include @@ -39,9 +34,9 @@ namespace is = eprosima::is; namespace xtypes = eprosima::xtypes; using namespace std::chrono_literals; -static is::utils::Logger logger("is::sh::ROS2_Dynamic::test::check_ros2pkg_creation"); +using is::sh::ros2::ROS2_DISTRO; -static const char* ros2_distro = std::getenv("ROS2_DISTRO"); +static is::utils::Logger logger("is::sh::ROS2_Dynamic::test::check_ros2pkg_creation"); class ROS2Dynamic : public testing::Test { @@ -66,7 +61,7 @@ class ROS2Dynamic : public testing::Test yaml += " };\n"; yaml += " };\n"; yaml += " paths: [\"/opt/ros/"; - yaml += ros2_distro; + yaml += ROS2_DISTRO; yaml += "/share\"]\n"; yaml += "systems:\n"; yaml += " ros2: { type: ros2_dynamic } \n"; @@ -107,7 +102,7 @@ class ROS2Dynamic : public testing::Test logger << is::utils::Logger::Level::INFO << "Execute ROS2_Dynamic test" << std::endl; std::ostringstream command; - command << ". /opt/ros/" << ros2_distro << "/setup.sh && ros2 topic pub /test "; + command << ". /opt/ros/" << ROS2_DISTRO << "/setup.sh && ros2 topic pub /test "; command << "custom_msgs/msg/Message \"{text: {data: 'thisisatest'}}\" --once"; FILE* pipe = popen(command.str().c_str(), "r"); From 33e1d2e10dd21256c478d5a7a6213b7226e48556 Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Wed, 31 May 2023 15:37:00 +0200 Subject: [PATCH 33/34] fix testing due new topic pub behavior Signed-off-by: Miguel Barro --- ros2/dynamic/test/CMakeLists.txt | 28 ++++++++++++++++--- .../integration/check_ros2pkg_creation.cpp | 2 +- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/ros2/dynamic/test/CMakeLists.txt b/ros2/dynamic/test/CMakeLists.txt index 76f8703..15f92de 100644 --- a/ros2/dynamic/test/CMakeLists.txt +++ b/ros2/dynamic/test/CMakeLists.txt @@ -38,7 +38,7 @@ macro(compile_test) set(TEST_NAME "${ARGV0}") endif() set(options NO_ROS_DEPENDENCIES) - set(multiValueArgs SOURCE ENVIRONMENTS) + set(multiValueArgs SOURCE ENVIRONMENTS PREPROCESSOR) cmake_parse_arguments(TEST "${options}" "${uniValueArgs}" "${multiValueArgs}" ${ARGN}) add_executable(${TEST_NAME} ${TEST_SOURCE}) @@ -79,9 +79,14 @@ macro(compile_test) $<$:-Wextra> ) + if(TEST_PREPROCESSOR) + string(REPLACE ";" " " preprocessor_definitions "${TEST_PREPROCESSOR}") + endif() + target_compile_definitions(${TEST_NAME} PRIVATE "ROS2__ROSIDL__BUILD_DIR=\"${CMAKE_BINARY_DIR}/is/rosidl/ros2/lib\"" + ${preprocessor_definitions} ) set_target_properties(${TEST_NAME} @@ -93,6 +98,7 @@ macro(compile_test) unset(link_dependencies) unset(include_dependencies) + unset(preprocessor_definitions) endmacro() include(CTest) @@ -100,9 +106,6 @@ include(${IS_GTEST_CMAKE_MODULE_DIR}/gtest.cmake) enable_testing() compile_test(${PROJECT_NAME}_geometry_msgs SOURCE integration/ros2__geometry_msgs.cpp) -compile_test(${PROJECT_NAME}_check_ros2pkg_creation - NO_ROS_DEPENDENCIES - SOURCE integration/check_ros2pkg_creation.cpp) compile_test(${PROJECT_NAME}_test_qos SOURCE integration/ros2__test_qos.cpp) if (rclcpp_VERSION VERSION_GREATER 2.4.2) @@ -111,6 +114,23 @@ else() compile_test(${PROJECT_NAME}_test_domain SOURCE integration/ros2__test_domain__foxy.cpp) endif() +# Check if this version of ros forces wait for subscription +execute_process( + COMMAND bash -c ". /opt/ros/${IS_ROS2_DISTRO}/setup.bash && ros2 topic pub --help" + OUTPUT_VARIABLE ROS2_PUB_HELP + ) + +if(ROS2_PUB_HELP MATCHES WAIT_MATCHING_SUBSCRIPTIONS) + set(WAIT "WAIT=\" -w 0\"") +else() + set(WAIT "WAIT=\"\"") +endif() + +compile_test(${PROJECT_NAME}_check_ros2pkg_creation + NO_ROS_DEPENDENCIES + PREPROCESSOR ${WAIT} + SOURCE integration/check_ros2pkg_creation.cpp) + # Windows dll dependencies installation if(WIN32) find_file(MOCKDLL NAMES "is-mock.dll" PATHS "${is-mock_DIR}" PATH_SUFFIXES "lib" ) diff --git a/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp b/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp index c15095b..03fc013 100644 --- a/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp +++ b/ros2/dynamic/test/integration/check_ros2pkg_creation.cpp @@ -103,7 +103,7 @@ class ROS2Dynamic : public testing::Test std::ostringstream command; command << ". /opt/ros/" << ROS2_DISTRO << "/setup.sh && ros2 topic pub /test "; - command << "custom_msgs/msg/Message \"{text: {data: 'thisisatest'}}\" --once"; + command << "custom_msgs/msg/Message \"{text: {data: 'thisisatest'}}\" --once" WAIT; FILE* pipe = popen(command.str().c_str(), "r"); if (!pipe) From 2df9b41cee5da30829b47012a744443f25a16e94 Mon Sep 17 00:00:00 2001 From: Miguel Barro Date: Thu, 8 Jun 2023 08:21:19 +0200 Subject: [PATCH 34/34] removing pre-merge branch dependencies Signed-off-by: Miguel Barro --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 2289334..9092733 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -38,7 +38,7 @@ jobs: - name: Download the Integration Service run: | - git clone https://github.com/eProsima/Integration-Service -b feature/node-fiware-support src/integration-service + git clone https://github.com/eProsima/Integration-Service src/integration-service - name: Build run: |