From 3f861270df3452c7a153d264fe7f7a2336436841 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 30 Oct 2024 12:48:54 +0100 Subject: [PATCH] implement custom converter for rcl_interfaces/Log --- CMakeLists.txt | 3 +- .../builtin_interfaces_factories.hpp | 20 +++++ .../convert_builtin_interfaces.hpp | 14 +++ ros1_bridge/__init__.py | 3 + src/builtin_interfaces_factories.cpp | 36 ++++++++ src/convert_builtin_interfaces.cpp | 89 +++++++++++++++++++ 6 files changed, 164 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 964630ac..49f66ae3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,7 +105,8 @@ set(actionlib_msgs_DEPRECATED_QUIET TRUE) foreach(package_name ${ros2_interface_packages}) find_package(${package_name} QUIET REQUIRED) message(STATUS "Found ${package_name}: ${${package_name}_VERSION} (${${package_name}_DIR})") - if(NOT "${package_name}" STREQUAL "builtin_interfaces") + if(NOT "${package_name}" STREQUAL "builtin_interfaces" AND + NOT "${package_name}" STREQUAL "rcl_interfaces") list(APPEND generated_files "${generated_path}/${package_name}_factories.cpp") list(APPEND generated_files "${generated_path}/${package_name}_factories.hpp") foreach(interface_file ${${package_name}_IDL_FILES}) diff --git a/include/ros1_bridge/builtin_interfaces_factories.hpp b/include/ros1_bridge/builtin_interfaces_factories.hpp index 61714975..398eb6fe 100644 --- a/include/ros1_bridge/builtin_interfaces_factories.hpp +++ b/include/ros1_bridge/builtin_interfaces_factories.hpp @@ -16,12 +16,14 @@ #define ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_ // include ROS 1 messages +#include #include #include // include ROS 2 messages #include #include +#include #include #include @@ -74,6 +76,24 @@ Factory< const builtin_interfaces::msg::Time & ros2_msg, std_msgs::Time & ros1_msg); +template<> +void +Factory< + rosgraph_msgs::Log, + rcl_interfaces::msg::Log +>::convert_1_to_2( + const rosgraph_msgs::Log & ros1_msg, + rcl_interfaces::msg::Log & ros2_msg); + +template<> +void +Factory< + rosgraph_msgs::Log, + rcl_interfaces::msg::Log +>::convert_2_to_1( + const rcl_interfaces::msg::Log & ros2_msg, + rosgraph_msgs::Log & ros1_msg); + } // namespace ros1_bridge #endif // ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_ diff --git a/include/ros1_bridge/convert_builtin_interfaces.hpp b/include/ros1_bridge/convert_builtin_interfaces.hpp index 9c6ea381..db6a360f 100644 --- a/include/ros1_bridge/convert_builtin_interfaces.hpp +++ b/include/ros1_bridge/convert_builtin_interfaces.hpp @@ -18,12 +18,14 @@ #include "ros1_bridge/convert_decl.hpp" // include ROS 1 builtin messages +#include #include "ros/duration.h" #include "ros/time.h" // include ROS 2 builtin messages #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" +#include namespace ros1_bridge { @@ -52,6 +54,18 @@ convert_2_to_1( const builtin_interfaces::msg::Time & ros2_msg, ros::Time & ros1_type); +template<> +void +convert_1_to_2( + const rosgraph_msgs::Log & ros1_msg, + rcl_interfaces::msg::Log & ros2_msg); + +template<> +void +convert_2_to_1( + const rcl_interfaces::msg::Log & ros2_msg, + rosgraph_msgs::Log & ros1_msg); + } // namespace ros1_bridge #endif // ROS1_BRIDGE__CONVERT_BUILTIN_INTERFACES_HPP_ diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 4fbcf0e2..b7abb0a0 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -98,6 +98,9 @@ def generate_cpp(output_path, template_dir): data['ros2_package_names_msg'] + data['ros2_package_names_srv']) # skip builtin_interfaces since there is a custom implementation unique_package_names -= {'builtin_interfaces'} + # skip rcl_interfaces since there is a custom implementation + # Only for Log which is the only type that is useful to map. + unique_package_names -= {'rcl_interfaces'} data['ros2_package_names'] = list(unique_package_names) template_file = os.path.join(template_dir, 'get_factory.cpp.em') diff --git a/src/builtin_interfaces_factories.cpp b/src/builtin_interfaces_factories.cpp index 9b8d34e8..1564ab11 100644 --- a/src/builtin_interfaces_factories.cpp +++ b/src/builtin_interfaces_factories.cpp @@ -52,6 +52,18 @@ get_factory_builtin_interfaces( > >("std_msgs/Time", ros2_type_name); } + if ( + (ros1_type_name == "rosgraph_msgs/Log" || ros1_type_name == "") && + ros2_type_name == "rcl_interfaces/msg/Log") + { + return std::make_shared< + Factory< + rosgraph_msgs::Log, + rcl_interfaces::msg::Log + > + >("rosgraph_msgs/Log", ros2_type_name); + } + return std::shared_ptr(); } @@ -104,4 +116,28 @@ Factory< ros1_bridge::convert_2_to_1(ros2_msg, ros1_msg.data); } +template<> +void +Factory< + rosgraph_msgs::Log, + rcl_interfaces::msg::Log +>::convert_1_to_2( + const rosgraph_msgs::Log & ros1_msg, + rcl_interfaces::msg::Log & ros2_msg) +{ + ros1_bridge::convert_1_to_2(ros1_msg, ros2_msg); +} + +template<> +void +Factory< + rosgraph_msgs::Log, + rcl_interfaces::msg::Log +>::convert_2_to_1( + const rcl_interfaces::msg::Log & ros2_msg, + rosgraph_msgs::Log & ros1_msg) +{ + ros1_bridge::convert_2_to_1(ros2_msg, ros1_msg); +} + } // namespace ros1_bridge diff --git a/src/convert_builtin_interfaces.cpp b/src/convert_builtin_interfaces.cpp index f0412f5b..f3def087 100644 --- a/src/convert_builtin_interfaces.cpp +++ b/src/convert_builtin_interfaces.cpp @@ -58,4 +58,93 @@ convert_2_to_1( ros1_type.nsec = ros2_msg.nanosec; } + +template<> +void +convert_1_to_2( + const rosgraph_msgs::Log & ros1_msg, + rcl_interfaces::msg::Log & ros2_msg) +{ + ros1_bridge::convert_1_to_2(ros1_msg.header.stamp, ros2_msg.stamp); + + // Explicitly map the values of the log level as they differ between + // ROS1 and ROS2. + switch (ros1_msg.level) + { + case rosgraph_msgs::Log::DEBUG: + ros2_msg.level = rcl_interfaces::msg::Log::DEBUG; + break; + + case rosgraph_msgs::Log::INFO: + ros2_msg.level = rcl_interfaces::msg::Log::INFO; + break; + + case rosgraph_msgs::Log::WARN: + ros2_msg.level = rcl_interfaces::msg::Log::WARN; + break; + + case rosgraph_msgs::Log::ERROR: + ros2_msg.level = rcl_interfaces::msg::Log::ERROR; + break; + + case rosgraph_msgs::Log::FATAL: + ros2_msg.level = rcl_interfaces::msg::Log::FATAL; + break; + + default: + ros2_msg.level = ros1_msg.level; + break; + } + + ros2_msg.name = ros1_msg.name; + ros2_msg.msg = ros1_msg.msg; + ros2_msg.file = ros1_msg.file; + ros2_msg.function = ros1_msg.function; + ros2_msg.line = ros1_msg.line; +} + +template<> +void +convert_2_to_1( + const rcl_interfaces::msg::Log & ros2_msg, + rosgraph_msgs::Log & ros1_msg) +{ + ros1_bridge::convert_2_to_1(ros2_msg.stamp, ros1_msg.header.stamp); + + // Explicitly map the values of the log level as they differ between + // ROS1 and ROS2. + switch (ros2_msg.level) + { + case rcl_interfaces::msg::Log::DEBUG: + ros1_msg.level = rosgraph_msgs::Log::DEBUG; + break; + + case rcl_interfaces::msg::Log::INFO: + ros1_msg.level = rosgraph_msgs::Log::INFO; + break; + + case rcl_interfaces::msg::Log::WARN: + ros1_msg.level = rosgraph_msgs::Log::WARN; + break; + + case rcl_interfaces::msg::Log::ERROR: + ros1_msg.level = rosgraph_msgs::Log::ERROR; + break; + + case rcl_interfaces::msg::Log::FATAL: + ros1_msg.level = rosgraph_msgs::Log::FATAL; + break; + + default: + ros1_msg.level = ros1_msg.level; + break; + } + + ros1_msg.name = ros2_msg.name; + ros1_msg.msg = ros2_msg.msg; + ros1_msg.file = ros2_msg.file; + ros1_msg.function = ros2_msg.function; + ros1_msg.line = ros2_msg.line; +} + } // namespace ros1_bridge