├── .github └── workflows │ └── main.yml ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── CODEOWNERS ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── bin └── ros1_bridge_generate_factories ├── cmake ├── find_ros1_interface_packages.cmake └── find_ros1_package.cmake ├── doc ├── index.rst ├── ros1_talker_ros2_listener.png └── ros2_camera_ros1_rqt.png ├── include └── ros1_bridge │ ├── bridge.hpp │ ├── builtin_interfaces_factories.hpp │ ├── convert_builtin_interfaces.hpp │ ├── convert_decl.hpp │ ├── factory.hpp │ └── factory_interface.hpp ├── package.xml ├── resource ├── get_factory.cpp.em ├── get_mappings.cpp.em ├── interface_factories.cpp.em ├── pkg_factories.cpp.em └── pkg_factories.hpp.em ├── ros1_bridge └── __init__.py ├── src ├── bridge.cpp ├── builtin_interfaces_factories.cpp ├── convert_builtin_interfaces.cpp ├── dynamic_bridge.cpp ├── parameter_bridge.cpp ├── simple_bridge.cpp ├── simple_bridge_1_to_2.cpp ├── simple_bridge_2_to_1.cpp └── static_bridge.cpp └── test ├── test_convert_generic.cpp ├── test_ros1_client.cpp ├── test_ros1_server.cpp ├── test_ros2_client.cpp ├── test_ros2_server.cpp ├── test_ros2_to_ros1_serialization.cpp ├── test_services_across_dynamic_bridge.py.in └── test_topics_across_dynamic_bridge.py.in /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] # on all pushes and PRs 4 | 5 | jobs: 6 | ros1_bridge: 7 | runs-on: ubuntu-latest 8 | steps: 9 | - uses: actions/checkout@v2 10 | - uses: ros-tooling/setup-ros@v0.2 11 | with: 12 | required-ros-distributions: "noetic rolling" 13 | - name: Build and test ros1-bridge 14 | uses: ros-tooling/action-ros-ci@v0.2 15 | with: 16 | package-name: ros1_bridge 17 | target-ros1-distro: noetic 18 | target-ros2-distro: rolling 19 | 20 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Python Caching 3 | __pycache__ 4 | 5 | # ROS2 Build Folders 6 | build/ 7 | _build/ 8 | log/ 9 | install/ 10 | 11 | # Editors 12 | .vscode 13 | .idea 14 | .#* 15 | .project 16 | .cproject 17 | 18 | *~ 19 | 20 | # Prerequisites 21 | *.d 22 | 23 | # Compiled Object files 24 | *.slo 25 | *.lo 26 | *.o 27 | *.obj 28 | 29 | # Precompiled Headers 30 | *.gch 31 | *.pch 32 | 33 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros1_bridge 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.10.3 (2022-03-29) 6 | ------------------- 7 | * Cleanup of README.md (`#342 `_) 8 | * Parametrizing service execution timeout (`#340 `_) 9 | * Fix cpplint error (`#341 `_) 10 | * Update package maintainers (`#335 `_) 11 | * Contributors: Cem Karan, Geoffrey Biggs, Jorge Perez, Marco Bassa, Tim Clephas, Tomoya Fujita 12 | 13 | 0.10.2 (2021-11-05) 14 | ------------------- 15 | * Example for `parameter_bridge` (`#330 `_) 16 | * Use rcpputils/scope_exit.hpp instead of rclcpp/scope_exit.hpp (`#324 `_) 17 | * Use FindPython3 and make Python dependency explicit (`#322 `_) 18 | * Bump ros-tooling/setup-ros@v0.2 (`#323 `_) 19 | * Add GitHub workflow for CI (`#310 `_) 20 | * Update includes after rcutils/get_env.h deprecation (`#311 `_) 21 | * Contributors: Christophe Bedard, Harsh Deshpande, Loy, Shane Loretz 22 | 23 | 0.10.1 (2021-01-25) 24 | ------------------- 25 | * Fix logging for updated rclcpp interface (`#303 `_) 26 | * Fix typo in comments (`#297 `_) 27 | * Contributors: Michael Carroll, Vicidel 28 | 29 | 0.9.5 (2020-12-08) 30 | ------------------ 31 | * Update to use rosidl_parser and .idl files rather than rosidl_adapter and .msg files (`#296 `_) 32 | * Update maintainers (`#286 `_) 33 | * Contributors: Jacob Perron, William Woodall 34 | 35 | 0.9.4 (2020-09-10) 36 | ------------------ 37 | * use hardcoded QoS (keep all, transient local) for /tf_static topic in dynamic_bridge (`#282 `_) 38 | * document explicitly passing the topic type to 'ros2 topic echo' (`#279 `_) 39 | 40 | 0.9.3 (2020-07-07) 41 | ------------------ 42 | * Fix multiple definition if message with same name as service exists (`#272 `_) 43 | * Contributors: Dirk Thomas 44 | 45 | 0.9.2 (2020-06-01) 46 | ------------------ 47 | * When generating service mappings cast pair to set to avoid duplicate pairs (`#268 `_) 48 | * Contributors: Gavin Suddrey 49 | 50 | 0.9.1 (2020-05-27) 51 | ------------------ 52 | * Deprecate package key for service parameters, use full type instead (`#263 `_) 53 | * Fix removing obsolete ROS 2 service bridges (`#267 `_) 54 | * Gracefully handle invalid ROS 1 publishers (`#266 `_) 55 | * Use reliable publisher in simple bridge (`#264 `_) 56 | * Remove outdated information on Fast RTPS bug (`#260 `_) 57 | * Contributors: Dirk Thomas, Thom747 58 | 59 | 0.9.0 (2020-05-18) 60 | ------------------ 61 | * Avoid new deprecations (`#255 `_) 62 | * Updates since changes to message_info in rclcpp (`#253 `_) 63 | * Assert ROS 1 nodes' stdout (`#247 `_) 64 | * Ignore actionlib_msgs deprecation warning (`#245 `_) 65 | * Drop workaround for https://github.com/ros2/rmw_fastrtps/issues/265. (`#233 `_) 66 | * Code style only: wrap after open parenthesis if not in one line (`#238 `_) 67 | * Contributors: Dirk Thomas, Jacob Perron, Michel Hidalgo, William Woodall 68 | 69 | 0.8.2 (2020-01-17) 70 | ------------------ 71 | * fix building test when ROS 1 diagnostic_msgs is isolated from roscpp (`#236 `_) 72 | * fix service with custom mapped message field (`#234 `_) 73 | * Contributors: Dirk Thomas 74 | 75 | 0.8.1 (2019-10-23) 76 | ------------------ 77 | * fix showing duplicate keys in --print-pairs (`#225 `_) 78 | * fix bridging builtin_interfaces Duration and Time (`#224 `_) 79 | * Don't use features that will be deprecated (`#222 `_) 80 | * Contributors: Dirk Thomas, Peter Baughman 81 | 82 | 0.8.0 (2019-09-27) 83 | ------------------ 84 | * Promote special CLI rules to flags. (`#217 `_) 85 | * Update __log_rosout_disable workaround to use --ros-args. (`#216 `_) 86 | * Clearer instructions for example (`#211 `_) 87 | * add services bridging to parameter_bridge (`#176 `_) 88 | * Contributors: Jose Luis Blanco-Claraco, Michel Hidalgo, cyrilleberger 89 | 90 | 0.7.3 (2019-08-02) 91 | ------------------ 92 | * fix typename in static bridge (`#209 `_) 93 | * fix cosmetic in message (`#207 `_) 94 | * Use %zu print format for size_t (`#204 `_) 95 | * Fix parameter bridge for topic if ros1 and ros2 type have a different name (`#177 `_) 96 | * Contributors: Dirk Thomas, Emerson Knapp, cyrilleberger 97 | 98 | 0.7.2 (2019-05-29) 99 | ------------------ 100 | * add note about rostopic echo (`#202 `_) 101 | * add workspace setup documentation (`#201 `_) 102 | * Contributors: Mabel Zhang 103 | 104 | 0.7.1 (2019-05-20) 105 | ------------------ 106 | * Disable rosout logging for the bridge (`#197 `_) 107 | * Handle launch_testing assertExitCodes correctly (`#193 `_) 108 | * Support field selection (`#174 `_) 109 | * Use interface kind names properly in ROS2 interface type names. (`#194 `_) 110 | * Contributors: Juan Rodriguez Hortala, Michel Hidalgo, ivanpauno 111 | 112 | 0.7.0 (2019-05-08) 113 | ------------------ 114 | * Adds interface type to ROS2 message type name. (`#191 `_) 115 | * fix build by passing options (`#192 `_) 116 | * changes to avoid deprecated API's (`#189 `_) 117 | * Corrected publish calls with shared_ptr signature, leftovers (`#190 `_) 118 | * Corrected publish calls with shared_ptr signature (`#188 `_) 119 | * Migrate launch tests to new launch_testing features & API (`#179 `_) 120 | * Some small fixes to the README (`#186 `_) 121 | * Fix the generator. (`#185 `_) 122 | * Merge pull request `#183 `_ from ros2/interface_specific_compilation_units 123 | * remove note about memory usage from README 124 | * split into interface specific compilation units 125 | * duplicate template before modifying it to track history 126 | * fix log messages (`#182 `_) 127 | * use safe_load instead of deprecated load (`#180 `_) 128 | * Merge pull request `#178 `_ from ros2/gonzalodepedro/fix-propagate-args-to-rcl-init 129 | * Allows propagations of cmd args to rclcpp::init 130 | * add section about DCO to CONTRIBUTING.md 131 | * Add launch along with launch_testing as test dependencies. (`#171 `_) 132 | * Switch to rclcpp logging and improve messages (`#167 `_) 133 | * invalidate wrong cached result for diagnostic_msgs (`#170 `_) 134 | * Drops legacy launch API usage. (`#163 `_) 135 | * export find_ros1_package cmake (`#164 `_) 136 | * ensure that the diagnostic_msgs package is from ROS 2 (`#169 `_) 137 | * Allow latching for ROS1 pub, and custom qos for ROS2 components (`#162 `_) 138 | * Allow external use of ros1_bridge library factories (`#160 `_) 139 | * Contributors: Chris Lalancette, Dirk Thomas, Gonzalo de Pedro, Gonzo, Karsten Knese, Michel Hidalgo, Mikael Arguedas, Paul Bovbel, William Woodall, ivanpauno 140 | 141 | 0.6.1 (2018-12-12) 142 | ------------------ 143 | * exclude ros1 nodelets (`#152 `_) 144 | * fix is_package_mapping check (`#151 `_) 145 | * Contributors: Dirk Thomas, Karsten Knese 146 | 147 | 0.6.0 (2018-12-08) 148 | ------------------ 149 | * expose convert function (`#146 `_) 150 | * support for custom field mapping for services (`#147 `_) 151 | * handle idl files correctly (`#145 `_) 152 | * Fix for actions subfolder introduction in ros2 message bridge (`#143 `_) 153 | * use new error handling API from rcutils (`#141 `_) 154 | * changed cmake message logger level (`#138 `_) 155 | * Contributors: Alberto Soragna, Dirk Thomas, Karsten Knese, Samuel Servulo, William Woodall 156 | 157 | 0.5.1 (2018-08-20) 158 | ------------------ 159 | * Merge pull request `#136 `_ from ros2/update_docs_135 160 | * update doc to reflect that any mapping combination is supported 161 | * rule can be a message mapping even if a field mapping is provided as well (`#135 `_) 162 | * Contributors: Mikael Arguedas 163 | 164 | 0.5.0 (2018-06-27) 165 | ------------------ 166 | * remove --build-tests which is an ament argument from colcon invocation 167 | * print service pairs as well (`#124 `_) 168 | * print message for all ROS 2 message pkgs (`#123 `_) 169 | * update README to use colcon and ROS Melodic (`#122 `_) 170 | * include module name which wasn't found in error message (`#121 `_) 171 | * use catkin_pkg to parse packages (`#119 `_) 172 | * migrate launch -> launch.legacy (`#117 `_) 173 | * Duplicate messages in bidirectional_bridge fix (`#113 `_) 174 | * Fix linter failures from includes (`#110 `_) 175 | * Map duration and time messages (`#106 `_) 176 | * clarify that all field must be listed explicitly (`#109 `_) 177 | * add an error message if the mapping rules are not a list (`#107 `_) 178 | * advise to ask questions on ROS answers 179 | * Contributors: ArkadiuszNiemiec, Dirk Thomas, Mikael Arguedas, Tully Foote, William Woodall, dhood 180 | 181 | 0.4.0 (2017-12-08) 182 | ------------------ 183 | * match topic name printed in console (`#102 `_) 184 | * Update for rclcpp namespace removals (`#101 `_) 185 | * cmake 3.10 compatibility: pass absolute path to file(GENERATE) function (`#100 `_) 186 | * depend on rosidl_interfaces_packages group (`#99 `_) 187 | * Fix building of ros1_bridge against newer roscpp. (`#98 `_) 188 | * Merge pull request `#97 `_ from ros2/ament_cmake_pytest 189 | * use ament_cmake_pytest instead of ament_cmake_nose 190 | * Merge pull request `#96 `_ from ros2/print_type_names 191 | * print bridged type names 192 | * Increase timeout waiting for server for ros2 client in tests (`#94 `_) 193 | * update style to match latest uncrustify (`#93 `_) 194 | * Contributors: Brian Gerkey, Chris Lalancette, Dirk Thomas, Esteve Fernandez, Hunter Allen, Jackie Kay, Karsten Knese, Mikael Arguedas, Morgan Quigley, Rafal Kozik, Rafał Kozik, Steven! Ragnarök, Tully Foote, William Woodall, dhood, gerkey 195 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ros1_bridge) 4 | 5 | # Default to C++14 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 14) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra) 11 | endif() 12 | 13 | find_package(rmw REQUIRED) 14 | 15 | find_package(ament_cmake REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rmw_implementation_cmake REQUIRED) 18 | find_package(std_msgs REQUIRED) 19 | 20 | # find ROS 1 packages 21 | set(cmake_extras_files cmake/find_ros1_package.cmake cmake/find_ros1_interface_packages.cmake) 22 | include(cmake/find_ros1_package.cmake) 23 | 24 | find_package(PkgConfig) 25 | if(NOT PKG_CONFIG_FOUND) 26 | message(WARNING "Failed to find PkgConfig, skipping...") 27 | # call ament_package() to avoid ament_tools treating this as a plain CMake pkg 28 | ament_package() 29 | return() 30 | endif() 31 | 32 | find_ros1_package(roscpp) 33 | if(NOT ros1_roscpp_FOUND) 34 | if(NOT ROS1_PACKAGES_UNAVAILABLE) 35 | # Only warn if ROS1 packages were expected to be available 36 | message(WARNING "Failed to find ROS 1 roscpp, skipping...") 37 | endif() 38 | # call ament_package() to avoid ament_tools treating this as a plain CMake pkg 39 | ament_package( 40 | CONFIG_EXTRAS ${cmake_extras_files} 41 | ) 42 | return() 43 | endif() 44 | 45 | find_ros1_package(std_msgs REQUIRED) 46 | 47 | # Dependency that we should only look for if ROS 1 is installed (it's not present on a ROS 2 48 | # system; see https://github.com/ros2/ros1_bridge/pull/331#issuecomment-1188111510) 49 | find_package(xmlrpcpp REQUIRED) 50 | 51 | # find ROS 1 packages with messages / services 52 | include(cmake/find_ros1_interface_packages.cmake) 53 | find_ros1_interface_packages(ros1_message_packages) 54 | 55 | set(prefixed_ros1_message_packages "") 56 | foreach(ros1_message_package ${ros1_message_packages}) 57 | # TODO(karsten1987): This is currently a workaround to work with ROS 2 classloader 58 | # rather than ROS 1 classloader. 59 | if(NOT "${ros1_message_package}" STREQUAL "nodelet") 60 | find_ros1_package(${ros1_message_package} REQUIRED) 61 | list(APPEND prefixed_ros1_message_packages "ros1_${ros1_message_package}") 62 | endif() 63 | endforeach() 64 | 65 | set(TEST_ROS1_BRIDGE FALSE) 66 | if(BUILD_TESTING) 67 | find_package(ament_lint_auto REQUIRED) 68 | find_package(diagnostic_msgs REQUIRED) 69 | find_ros1_package(diagnostic_msgs) 70 | find_ros1_package(roslaunch) 71 | ament_lint_auto_find_test_dependencies() 72 | if(ros1_diagnostic_msgs_FOUND AND ros1_roslaunch_FOUND) 73 | # ensure that the ROS 2 diagnostic_msgs was found by find_package 74 | # and not the ROS 1 package 75 | set(_ros1_diagnostic_msgs_DIR "${ros1_diagnostic_msgs_PREFIX}/share/diagnostic_msgs/cmake") 76 | if("${diagnostic_msgs_DIR}" STREQUAL "${_ros1_diagnostic_msgs_DIR}") 77 | # invalidate the cached result to retry finding the package next time 78 | unset(diagnostic_msgs_DIR CACHE) 79 | message(FATAL_ERROR "Failed to find ROS 2 package 'diagnostic_msgs'") 80 | endif() 81 | set(TEST_ROS1_BRIDGE TRUE) 82 | endif() 83 | 84 | find_package(ament_cmake_gtest REQUIRED) 85 | find_package(geometry_msgs REQUIRED) 86 | find_package(std_msgs REQUIRED) 87 | find_package(sensor_msgs REQUIRED) 88 | find_ros1_package(geometry_msgs) 89 | find_ros1_package(std_msgs) 90 | find_ros1_package(sensor_msgs) 91 | if(ros1_geometry_msgs_FOUND AND ros1_sensor_msgs_FOUND AND ros1_std_msgs_FOUND) 92 | ament_add_gtest(test_convert_generic test/test_convert_generic.cpp) 93 | ament_target_dependencies(test_convert_generic 94 | "geometry_msgs" 95 | "rclcpp" 96 | "ros1_roscpp" 97 | "ros1_sensor_msgs" 98 | "ros1_std_msgs" 99 | "ros1_geometry_msgs" 100 | "sensor_msgs" 101 | "std_msgs" 102 | ) 103 | target_link_libraries(test_convert_generic ${PROJECT_NAME}) 104 | 105 | ament_add_gtest(test_ros2_to_ros1_serialization test/test_ros2_to_ros1_serialization.cpp) 106 | ament_target_dependencies(test_ros2_to_ros1_serialization 107 | "geometry_msgs" 108 | "rclcpp" 109 | "ros1_roscpp" 110 | "ros1_sensor_msgs" 111 | "ros1_std_msgs" 112 | "ros1_geometry_msgs" 113 | "sensor_msgs" 114 | "std_msgs" 115 | ) 116 | target_link_libraries(test_ros2_to_ros1_serialization ${PROJECT_NAME}) 117 | endif() 118 | 119 | endif() 120 | 121 | ament_export_include_directories(include) 122 | ament_export_libraries(${PROJECT_NAME}) 123 | 124 | ament_python_install_package(${PROJECT_NAME}) 125 | 126 | ament_package( 127 | CONFIG_EXTRAS ${cmake_extras_files} 128 | ) 129 | 130 | set(generated_path "${CMAKE_BINARY_DIR}/generated") 131 | set(generated_files "${generated_path}/get_factory.cpp") 132 | list(APPEND generated_files "${generated_path}/get_mappings.cpp") 133 | 134 | # generate per interface compilation units to keep the memory usage low 135 | ament_index_get_resources(ros2_interface_packages "rosidl_interfaces") 136 | # actionlib_msgs is deprecated, but we will quiet the warning until the bridge has support for 137 | # ROS actions: https://github.com/ros2/design/issues/195 138 | set(actionlib_msgs_DEPRECATED_QUIET TRUE) 139 | foreach(package_name ${ros2_interface_packages}) 140 | find_package(${package_name} QUIET REQUIRED) 141 | message(STATUS "Found ${package_name}: ${${package_name}_VERSION} (${${package_name}_DIR})") 142 | if(NOT "${package_name}" STREQUAL "builtin_interfaces") 143 | list(APPEND generated_files "${generated_path}/${package_name}_factories.cpp") 144 | list(APPEND generated_files "${generated_path}/${package_name}_factories.hpp") 145 | foreach(interface_file ${${package_name}_IDL_FILES}) 146 | file(TO_CMAKE_PATH "${interface_file}" interface_name) 147 | get_filename_component(interface_basename "${interface_name}" NAME_WE) 148 | # skipping actions and request and response messages of services 149 | if(NOT "${interface_name}" MATCHES "^(msg|srv)/" OR "${interface_basename}" MATCHES "_(Request|Response)$") 150 | continue() 151 | endif() 152 | string(REPLACE "/" "__" interface_name "${interface_name}") 153 | get_filename_component(interface_name "${interface_name}" NAME_WE) 154 | list(APPEND generated_files "${generated_path}/${package_name}__${interface_name}__factories.cpp") 155 | endforeach() 156 | endif() 157 | endforeach() 158 | 159 | set(target_dependencies 160 | "bin/ros1_bridge_generate_factories" 161 | "resource/get_factory.cpp.em" 162 | "resource/get_mappings.cpp.em" 163 | "resource/interface_factories.cpp.em" 164 | "resource/pkg_factories.cpp.em" 165 | "resource/pkg_factories.hpp.em" 166 | "ros1_bridge/__init__.py") 167 | 168 | find_package(Python3 REQUIRED COMPONENTS Interpreter) 169 | 170 | add_custom_command( 171 | OUTPUT ${generated_files} 172 | COMMAND Python3::Interpreter 173 | ARGS bin/ros1_bridge_generate_factories 174 | --output-path "${generated_path}" --template-dir resource 175 | DEPENDS ${target_dependencies} 176 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 177 | COMMENT "Generating factories for interface types") 178 | 179 | if(NOT WIN32) 180 | # ignore warning in ROS 1 message headers 181 | set_source_files_properties(${generated_files} 182 | PROPERTIES COMPILE_FLAGS "-Wno-unused-parameter") 183 | endif() 184 | 185 | include_directories(include ${generated_path}) 186 | 187 | function(custom_executable target) 188 | cmake_parse_arguments( 189 | ARG "ROS1_DEPENDENCIES" "" "TARGET_DEPENDENCIES" ${ARGN}) 190 | 191 | add_executable(${target} 192 | ${ARG_UNPARSED_ARGUMENTS}) 193 | ament_target_dependencies(${target} 194 | "rclcpp" 195 | ${ARG_TARGET_DEPENDENCIES}) 196 | if(ARG_ROS1_DEPENDENCIES) 197 | ament_target_dependencies(${target} 198 | "ros1_roscpp" 199 | "ros1_std_msgs") 200 | endif() 201 | if(ARG_DEPENDENCIES) 202 | add_dependencies(${target} ${ARG_DEPENDENCIES}) 203 | endif() 204 | 205 | install(TARGETS ${target} 206 | DESTINATION lib/${PROJECT_NAME}) 207 | endfunction() 208 | 209 | custom_executable(simple_bridge_1_to_2 "src/simple_bridge_1_to_2.cpp" 210 | ROS1_DEPENDENCIES 211 | TARGET_DEPENDENCIES "std_msgs") 212 | custom_executable(simple_bridge_2_to_1 "src/simple_bridge_2_to_1.cpp" 213 | ROS1_DEPENDENCIES 214 | TARGET_DEPENDENCIES "std_msgs") 215 | 216 | custom_executable(simple_bridge "src/simple_bridge.cpp" 217 | ROS1_DEPENDENCIES 218 | TARGET_DEPENDENCIES "std_msgs") 219 | 220 | add_library(${PROJECT_NAME} SHARED 221 | "src/builtin_interfaces_factories.cpp" 222 | "src/convert_builtin_interfaces.cpp" 223 | "src/bridge.cpp" 224 | ${generated_files}) 225 | ament_target_dependencies(${PROJECT_NAME} 226 | ${prefixed_ros1_message_packages} 227 | ${ros2_interface_packages} 228 | "rclcpp" 229 | "ros1_roscpp" 230 | "ros1_std_msgs") 231 | 232 | install(TARGETS ${PROJECT_NAME} 233 | ARCHIVE DESTINATION lib 234 | LIBRARY DESTINATION lib 235 | RUNTIME DESTINATION bin) 236 | 237 | custom_executable(static_bridge 238 | "src/static_bridge.cpp" 239 | ROS1_DEPENDENCIES 240 | TARGET_DEPENDENCIES ${ros2_interface_packages}) 241 | target_link_libraries(static_bridge 242 | ${PROJECT_NAME}) 243 | 244 | custom_executable(parameter_bridge 245 | "src/parameter_bridge.cpp" 246 | ROS1_DEPENDENCIES 247 | TARGET_DEPENDENCIES ${message_packages}) 248 | target_link_libraries(parameter_bridge 249 | ${PROJECT_NAME}) 250 | 251 | custom_executable(dynamic_bridge 252 | "src/dynamic_bridge.cpp" 253 | ROS1_DEPENDENCIES 254 | TARGET_DEPENDENCIES ${ros2_interface_packages}) 255 | target_link_libraries(dynamic_bridge 256 | ${PROJECT_NAME}) 257 | 258 | if(TEST_ROS1_BRIDGE) 259 | custom_executable(test_ros2_client_cpp "test/test_ros2_client.cpp") 260 | ament_target_dependencies("test_ros2_client_cpp${target_suffix}" "ros1_roscpp" "diagnostic_msgs") 261 | custom_executable(test_ros2_server_cpp "test/test_ros2_server.cpp") 262 | ament_target_dependencies("test_ros2_server_cpp${target_suffix}" "ros1_roscpp" "diagnostic_msgs") 263 | 264 | set(TEST_BRIDGE_DYNAMIC_BRIDGE "$") 265 | set(TEST_BRIDGE_ROS2_CLIENT "$") 266 | set(TEST_BRIDGE_ROS2_SERVER "$") 267 | endif() 268 | 269 | macro(targets) 270 | set(TEST_BRIDGE_RMW ${rmw_implementation}) 271 | 272 | configure_file( 273 | test/test_topics_across_dynamic_bridge.py.in 274 | test_topics_across_dynamic_bridge${target_suffix}.py.genexp 275 | @ONLY 276 | ) 277 | file(GENERATE 278 | OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}_$.py" 279 | INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}.py.genexp" 280 | ) 281 | add_launch_test( 282 | "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}_$.py" 283 | TARGET test_topics_across_dynamic_bridge${target_suffix} 284 | ENV RMW_IMPLEMENTATION=${rmw_implementation} 285 | TIMEOUT 60) 286 | 287 | configure_file( 288 | test/test_services_across_dynamic_bridge.py.in 289 | test_services_across_dynamic_bridge${target_suffix}.py.genexp 290 | @ONLY 291 | ) 292 | file(GENERATE 293 | OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}_$.py" 294 | INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}.py.genexp" 295 | ) 296 | add_launch_test( 297 | "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}_$.py" 298 | TARGET test_services_across_dynamic_bridge${target_suffix} 299 | ENV RMW_IMPLEMENTATION=${rmw_implementation} 300 | TIMEOUT 60) 301 | endmacro() 302 | 303 | if(TEST_ROS1_BRIDGE) 304 | find_package(launch_testing_ament_cmake REQUIRED) 305 | 306 | add_executable(test_ros1_client "test/test_ros1_client.cpp") 307 | ament_target_dependencies(test_ros1_client "ros1_diagnostic_msgs" "ros1_roscpp") 308 | add_executable(test_ros1_server "test/test_ros1_server.cpp") 309 | ament_target_dependencies(test_ros1_server "ros1_diagnostic_msgs" "ros1_roscpp") 310 | set(TEST_BRIDGE_ROS1_ENV "${ros1_roslaunch_PREFIX}/env.sh") 311 | set(TEST_BRIDGE_ROSCORE "${ros1_roslaunch_PREFIX}/bin/roscore") 312 | set(TEST_BRIDGE_ROS1_CLIENT "$") 313 | set(TEST_BRIDGE_ROS1_SERVER "$") 314 | 315 | call_for_each_rmw_implementation(targets) 316 | endif() 317 | 318 | install( 319 | PROGRAMS bin/ros1_bridge_generate_factories 320 | DESTINATION lib/${PROJECT_NAME}/generate_factories 321 | ) 322 | install( 323 | DIRECTORY cmake 324 | DESTINATION share/${PROJECT_NAME} 325 | ) 326 | install( 327 | DIRECTORY include/ 328 | DESTINATION include 329 | ) 330 | install( 331 | DIRECTORY resource 332 | DESTINATION share/${PROJECT_NAME} 333 | ) 334 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | # This file was generated by https://github.com/audrow/update-ros2-repos 2 | * @methylDragon @quarkytale @gbiggs 3 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | 15 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 16 | line to commit messages to certify that they have the right to submit 17 | the code they are contributing to the project according to the 18 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 19 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Bridge communication between ROS 1 and ROS 2 2 | 3 | This package provides a network bridge which enables the exchange of messages between ROS 1 and ROS 2. 4 | 5 | The bridge is currently implemented in C++ as at the time the Python API for ROS 2 had not been developed. 6 | Because of this its support is limited to only the message/service types available at compile time of the bridge. 7 | The bridge provided with the prebuilt ROS 2 binaries includes support for common ROS interfaces (messages/services), such as the interface packages listed in the [ros2/common_interfaces repository](https://github.com/ros2/common_interfaces) and `tf2_msgs`. 8 | See [the documentation](doc/index.rst) for more details on how ROS 1 and ROS 2 interfaces are associated with each other. 9 | If you would like to use a bridge with other interfaces (including your own custom types), you will have to build the bridge from source (instructions below), after building and sourcing your custom types in separate ROS 1 and ROS 2 workspaces. 10 | See [the documentation](doc/index.rst) for an example setup. 11 | 12 | For efficiency reasons, topics will only be bridged when matching publisher-subscriber pairs are active for a topic on either side of the bridge. 13 | As a result using `ros2 topic echo ` doesn't work but fails with an error message `Could not determine the type for the passed topic` if no other subscribers are present since the dynamic bridge hasn't bridged the topic yet. 14 | As a workaround the topic type can be specified explicitly `ros2 topic echo ` which triggers the bridging of the topic since the `echo` command represents the necessary subscriber. 15 | On the ROS 1 side `rostopic echo` doesn't have an option to specify the topic type explicitly. 16 | Therefore it can't be used with the dynamic bridge if no other subscribers are present. 17 | As an alternative you can use the `--bridge-all-2to1-topics` option to bridge all ROS 2 topics to ROS 1 so that tools such as `rostopic echo`, `rostopic list` and `rqt` will see the topics even if there are no matching ROS 1 subscribers. 18 | Run `ros2 run ros1_bridge dynamic_bridge -- --help` for more options. 19 | 20 | ## Supported ROS and Ubuntu Versions 21 | 22 | > ⚠️ **Important Compatibility Notice** 23 | 24 | - `ros1_bridge` **requires ROS 1**, which has reached [end-of-life (EOL)](https://www.ros.org/reps/rep-0003.html#noetic-ninjemys-may-2020-may-2025) as of **May 2025** for ROS Noetic. 25 | - Ubuntu **24.04 LTS** does **not support ROS 1**, and therefore is **not compatible** with `ros1_bridge`. 26 | 27 | | Ubuntu Version | Supported ROS 1 Versions | Supported ROS 2 Versions | `ros1_bridge` Support | 28 | |----------------|----------------|---------------------------------------|----------------------------------| 29 | | 20.04 (Focal) | Noetic Ninjemys | Foxy Fitzroy (EOL), Galactic Geochelone (EOL), Humble Hawksbill | ✅ Full support | 30 | | 22.04 (Jammy) | ⚠️ Partial (unsupported officially) | Humble Hawksbill, Iron Irwini | ⚠️ Requires building from source | 31 | | 24.04 (Noble) | ❌ Not available | Jazzy Jalisco, Kilted Kaiju | ❌ Not supported | 32 | 33 | To use `ros1_bridge`, you must use a system where both ROS 1 and ROS 2 are installable and buildable. Mixing ROS distributions across unsupported Ubuntu versions is **not recommended** and may lead to broken builds or missing dependencies. 34 | 35 | ## Prerequisites 36 | 37 | In order to run the bridge you need to either: 38 | 39 | * get [prebuilt binaries](https://github.com/ros2/ros2/releases) or 40 | * build the bridge as well as the other ROS 2 packages from source. 41 | 42 | After that you can run both examples described below. 43 | 44 | For all examples you need to source the environment of the install space where the bridge was built or unpacked to. 45 | Additionally you will need to either source the ROS 1 environment or at least set the `ROS_MASTER_URI` and run a `roscore`. 46 | 47 | The following ROS 1 packages are required to build and use the bridge: 48 | * `catkin` 49 | * `roscpp` 50 | * `roslaunch` (for `roscore` executable) 51 | * `rosmsg` 52 | * `std_msgs` 53 | * as well as the Python package `rospkg` 54 | 55 | To run the following examples you will also need these ROS 1 packages: 56 | * `rosbash` (for `rosrun` executable) 57 | * `roscpp_tutorials` 58 | * `rospy_tutorials` 59 | * `rostopic` 60 | * `rqt_image_view` 61 | 62 | ### Prerequisites for the examples in this file 63 | 64 | In order to make the examples below portable between versions of ROS, we define two environment variables, `ROS1_INSTALL_PATH` and `ROS2_INSTALL_PATH`. 65 | These are defined as the paths to the installation location of their respective ROS versions. 66 | 67 | If you installed Noetic in the default location, then the definition of `ROS1_INSTALL_PATH` will be `/opt/ros/noetic`. 68 | 69 | Building the bridge as described below requires you to build all of ROS 2. 70 | We assume that you have downloaded it to `~/ros2_rolling`, and that is where you plan on building it. 71 | In this case, `ROS2_INSTALL_PATH` will be defined as `~/ros2_rolling/install`. 72 | 73 | If you've chosen to install either or both versions of ROS somewhere else, you will need adjust the definitions below to match your installation paths. 74 | 75 | Because these definitions are used continuously throughout this page, it is useful to add the following lines to your shell startup file (`~/.bashrc` if you are using `bash`, `~/.zshrc` if you are using `zsh`). 76 | Modify these definitions as appropriate for the versions of ROS that you're using, and for the shell that you're using. 77 | 78 | 79 | ```bash 80 | export ROS1_INSTALL_PATH=/opt/ros/noetic 81 | export ROS2_INSTALL_PATH=~/ros2_rolling/install 82 | ``` 83 | 84 | Note that no trailing '/' character is used in either definition. 85 | If you have problems involving paths, please verify that you have the correct path to the installation location, and that you do not have a trailing '/' in either definition. 86 | 87 | ### Building the bridge from source 88 | 89 | Before continuing you should have the prerequisites for building ROS 2 from source installed following [these instructions](https://github.com/ros2/ros2/wiki/Installation). 90 | 91 | In the past, building this package required patches to ROS 1, but in the latest releases that is no longer the case. 92 | If you run into trouble first make sure you have at least version `1.11.16` of `ros_comm` and `rosbag`. 93 | 94 | The bridge uses `pkg-config` to find ROS 1 packages. 95 | ROS 2 packages are found through CMake using `find_package()`. 96 | Therefore the `CMAKE_PREFIX_PATH` must not contain paths from ROS 1 which would overlay ROS 2 packages. 97 | 98 | Here are the steps for Linux and OSX. 99 | 100 | You should first build everything but the ROS 1 bridge with normal colcon arguments. 101 | We don't recommend having your ROS 1 environment sourced during this step as it can add other libraries to the path. 102 | 103 | 104 | ```bash 105 | colcon build --symlink-install --packages-skip ros1_bridge 106 | ``` 107 | 108 | Next you need to source the ROS 1 environment. 109 | If you set the `ROS1_INSTALL_PATH` environment variable as described above, then the following will source the correct `setup.bash` file. 110 | 111 | 112 | ```bash 113 | source ${ROS1_INSTALL_PATH}/setup.bash 114 | # Or, on OSX, something like: 115 | # . ~/ros_catkin_ws/install_isolated/setup.bash 116 | ``` 117 | 118 | The bridge will be built with support for any message/service packages that are on your path and have an associated mapping between ROS 1 and ROS 2. 119 | Therefore you must add any ROS 1 or ROS 2 workspaces that have message/service packages that you want to be bridged to your path before building the bridge. 120 | This can be done by adding explicit dependencies on the message/service packages to the `package.xml` of the bridge, so that `colcon` will add them to the path before it builds the bridge. 121 | Alternatively you can do it manually by sourcing the relevant workspaces yourself, e.g.: 122 | 123 | 124 | ```bash 125 | # You have already sourced your ROS installation. 126 | # Source your ROS 2 installation: 127 | source ${ROS2_INSTALL_PATH}/setup.bash 128 | # And if you have a ROS 1 overlay workspace, something like: 129 | # . /setup.bash 130 | # And if you have a ROS 2 overlay workspace, something like: 131 | # . /local_setup.bash 132 | ``` 133 | 134 | Then build just the ROS 1 bridge: 135 | 136 | 137 | ```bash 138 | colcon build --symlink-install --packages-select ros1_bridge --cmake-force-configure 139 | ``` 140 | 141 | *Note:* If you are building on a memory constrained system you might want to limit the number of parallel jobs by setting e.g. the environment variable `MAKEFLAGS=-j1`. 142 | 143 | 144 | ## Example 1: run the bridge and the example talker and listener 145 | 146 | The talker and listener can be either a ROS 1 or a ROS 2 node. 147 | The bridge will pass the message along transparently. 148 | 149 | *Note:* When you are running these demos make sure to only source the indicated workspaces. 150 | You will get errors from most tools if they have both workspaces in their environment. 151 | 152 | 153 | ### Example 1a: ROS 1 talker and ROS 2 listener 154 | 155 | First we start a ROS 1 `roscore`: 156 | 157 | 158 | ```bash 159 | # Shell A (ROS 1 only): 160 | source ${ROS1_INSTALL_PATH}/setup.bash 161 | # Or, on OSX, something like: 162 | # . ~/ros_catkin_ws/install_isolated/setup.bash 163 | roscore 164 | ``` 165 | 166 | --- 167 | 168 | Then we start the dynamic bridge which will watch the available ROS 1 and ROS 2 topics. 169 | Once a *matching* topic has been detected it starts to bridge the messages on this topic. 170 | 171 | 172 | ```bash 173 | # Shell B (ROS 1 + ROS 2): 174 | # Source ROS 1 first: 175 | source ${ROS1_INSTALL_PATH}/setup.bash 176 | # Or, on OSX, something like: 177 | # . ~/ros_catkin_ws/install_isolated/setup.bash 178 | # Source ROS 2 next: 179 | source ${ROS2_INSTALL_PATH}/setup.bash 180 | # For example: 181 | # . /opt/ros/dashing/setup.bash 182 | export ROS_MASTER_URI=http://localhost:11311 183 | ros2 run ros1_bridge dynamic_bridge 184 | ``` 185 | 186 | The program will start outputting the currently available topics in ROS 1 and ROS 2 in a regular interval. 187 | 188 | --- 189 | 190 | Now we start the ROS 1 talker. 191 | 192 | 193 | ```bash 194 | # Shell C: 195 | source ${ROS1_INSTALL_PATH}/setup.bash 196 | # Or, on OSX, something like: 197 | # . ~/ros_catkin_ws/install_isolated/setup.bash 198 | rosrun rospy_tutorials talker 199 | ``` 200 | 201 | The ROS 1 node will start printing the published messages to the console. 202 | 203 | --- 204 | 205 | Now we start the ROS 2 listener from the `demo_nodes_cpp` ROS 2 package. 206 | 207 | 208 | ```bash 209 | # Shell D: 210 | source ${ROS2_INSTALL_PATH}/setup.bash 211 | ros2 run demo_nodes_cpp listener 212 | ``` 213 | 214 | The ROS 2 node will start printing the received messages to the console. 215 | 216 | When looking at the output in *shell B* there will be a line stating that the bridge for this topic has been created: 217 | 218 | 219 | ```bash 220 | created 1to2 bridge for topic '/chatter' with ROS 1 type 'std_msgs/String' and ROS 2 type 'std_msgs/String' 221 | ``` 222 | 223 | At the end stop all programs with `Ctrl-C`. 224 | Once you stop either the talker or the listener in *shell B* a line will be stating that the bridge has been torn down: 225 | 226 | 227 | ```bash 228 | removed 1to2 bridge for topic '/chatter' 229 | ``` 230 | 231 | The screenshot shows all the shell windows and their expected content: 232 | 233 | ![ROS 1 talker and ROS 2 listener](doc/ros1_talker_ros2_listener.png) 234 | 235 | 236 | ### Example 1b: ROS 2 talker and ROS 1 listener 237 | 238 | The steps are very similar to the previous example and therefore only the commands are described. 239 | 240 | 241 | ```bash 242 | # Shell A: 243 | source ${ROS1_INSTALL_PATH}/setup.bash 244 | # Or, on OSX, something like: 245 | # . ~/ros_catkin_ws/install_isolated/setup.bash 246 | roscore 247 | ``` 248 | 249 | --- 250 | 251 | 252 | ```bash 253 | # Shell B: 254 | source ${ROS1_INSTALL_PATH}/setup.bash 255 | # Or, on OSX, something like: 256 | # . ~/ros_catkin_ws/install_isolated/setup.bash 257 | source ${ROS2_INSTALL_PATH}/setup.bash 258 | export ROS_MASTER_URI=http://localhost:11311 259 | ros2 run ros1_bridge dynamic_bridge 260 | ``` 261 | 262 | --- 263 | 264 | Now we start the ROS 2 talker from the `demo_nodes_py` ROS 2 package. 265 | 266 | ```bash 267 | # Shell C: 268 | source ${ROS2_INSTALL_PATH}/setup.bash 269 | ros2 run demo_nodes_py talker 270 | ``` 271 | 272 | --- 273 | 274 | Now we start the ROS 1 listener. 275 | 276 | ```bash 277 | # Shell D: 278 | source ${ROS1_INSTALL_PATH}/setup.bash 279 | # Or, on OSX, something like: 280 | # . ~/ros_catkin_ws/install_isolated/setup.bash 281 | rosrun roscpp_tutorials listener 282 | ``` 283 | 284 | ## Example 2: run the bridge and exchange images 285 | 286 | The second example will demonstrate the bridge passing along bigger and more complicated messages. 287 | A ROS 2 node is publishing images retrieved from a camera and on the ROS 1 side we use `rqt_image_view` to render the images in a GUI. 288 | And a ROS 1 publisher can send a message to toggle an option in the ROS 2 node. 289 | 290 | First we start a ROS 1 `roscore` and the bridge: 291 | 292 | ```bash 293 | # Shell A: 294 | source ${ROS1_INSTALL_PATH}/setup.bash 295 | # Or, on OSX, something like: 296 | # . ~/ros_catkin_ws/install_isolated/setup.bash 297 | roscore 298 | ``` 299 | 300 | ```bash 301 | # Shell B: 302 | source ${ROS1_INSTALL_PATH}/setup.bash 303 | # Or, on OSX, something like: 304 | # . ~/ros_catkin_ws/install_isolated/setup.bash 305 | source ${ROS2_INSTALL_PATH}/install/setup.bash 306 | export ROS_MASTER_URI=http://localhost:11311 307 | ros2 run ros1_bridge dynamic_bridge 308 | ``` 309 | 310 | --- 311 | 312 | Now we start the ROS 1 GUI: 313 | 314 | ```bash 315 | # Shell C: 316 | source ${ROS1_INSTALL_PATH}/setup.bash 317 | # Or, on OSX, something like: 318 | # . ~/ros_catkin_ws/install_isolated/setup.bash 319 | rqt_image_view /image 320 | ``` 321 | 322 | --- 323 | 324 | Now we start the ROS 2 image publisher from the `image_tools` ROS 2 package: 325 | ```bash 326 | # Shell D: 327 | source ${ROS2_INSTALL_PATH}/install/setup.bash 328 | ros2 run image_tools cam2image 329 | ``` 330 | 331 | You should see the current images in `rqt_image_view` which are coming from the ROS 2 node `cam2image` and are being passed along by the bridge. 332 | 333 | --- 334 | 335 | To exercise the bridge in the opposite direction at the same time you can publish a message to the ROS 2 node from ROS 1. 336 | By publishing either `true` or `false` to the `flip_image` topic, the camera node will conditionally flip the image before sending it. 337 | You can either use the `Message Publisher` plugin in `rqt` to publish a `std_msgs/Bool` message on the topic `flip_image`, or run one of the two following `rostopic` commands: 338 | 339 | 340 | ```bash 341 | # Shell E: 342 | source ${ROS1_INSTALL_PATH}/setup.bash 343 | # Or, on OSX, something like: 344 | # . ~/ros_catkin_ws/install_isolated/setup.bash 345 | rostopic pub -r 1 /flip_image std_msgs/Bool "{data: true}" 346 | rostopic pub -r 1 /flip_image std_msgs/Bool "{data: false}" 347 | ``` 348 | 349 | The screenshot shows all the shell windows and their expected content (it was taken when Indigo was supported - you should use Melodic): 350 | 351 | ![ROS 2 camera and ROS 1 rqt](doc/ros2_camera_ros1_rqt.png) 352 | 353 | ## Example 3: run the bridge for AddTwoInts service 354 | 355 | In this example we will bridge a service TwoInts from 356 | [ros/roscpp_tutorials](https://github.com/ros/ros_tutorials) and AddTwoInts from 357 | [ros2/roscpp_examples](https://github.com/ros2/examples). 358 | 359 | While building, ros1_bridge looks for all installed ROS and ROS2 services. 360 | Found services are matched by comparing package name, service name and fields in a request and a response. 361 | If all names are the same in ROS and ROS2 service, the bridge will be created. 362 | It is also possible to pair services manually by creating a yaml file that will include names of corresponding services. 363 | You can find more information [here](doc/index.rst). 364 | 365 | So to make this example work, please make sure that the roscpp_tutorials package 366 | is installed on your system and the environment is set up correctly while you build ros1_bridge. 367 | 368 | Launch ROS master 369 | 370 | ```bash 371 | # Shell A: 372 | source ${ROS1_INSTALL_PATH}/setup.bash 373 | roscore -p 11311 374 | ``` 375 | 376 | Launch dynamic_bridge: 377 | 378 | ```bash 379 | # Shell B: 380 | source ${ROS1_INSTALL_PATH}/setup.bash 381 | source ${ROS2_INSTALL_PATH}/setup.bash 382 | export ROS_MASTER_URI=http://localhost:11311 383 | ros2 run ros1_bridge dynamic_bridge 384 | ``` 385 | 386 | Launch TwoInts server: 387 | 388 | ```bash 389 | # Shell C: 390 | source ${ROS1_INSTALL_PATH}/setup.bash 391 | export ROS_MASTER_URI=http://localhost:11311 392 | rosrun roscpp_tutorials add_two_ints_server 393 | ``` 394 | 395 | Launch AddTwoInts client: 396 | 397 | ```bash 398 | # Shell D: 399 | source ${ROS2_INSTALL_PATH}/setup.bash 400 | ros2 run demo_nodes_cpp add_two_ints_client 401 | ``` 402 | 403 | ## Example 4: bridge only selected topics and services 404 | This example expands on example 3 by selecting a subset of topics and services to be bridged. 405 | This is handy when, for example, you have a system that runs most of it's stuff in either ROS 1 or ROS 2 but needs a few nodes from the 'opposite' version of ROS. 406 | Where the `dynamic_bridge` bridges all topics and service, the `parameter_bridge` uses the ROS 1 parameter server to choose which topics and services are bridged. 407 | **Note**: The service bridge is **monodirectional**. You must use either `services_2_to_1` and/or `services_1_to_2` to bridge ROS 2 -> ROS 1 or ROS 1 -> ROS 2 services accordingly. 408 | For example, to bridge only the `/chatter` topic bidirectionally, and the `/add_two_ints service` from ROS 2 to ROS 1 only, create this configuration file, `bridge.yaml`: 409 | 410 | ```yaml 411 | topics: 412 | - 413 | topic: /chatter # Topic name on both ROS 1 and ROS 2 414 | type: std_msgs/msg/String # Type of topic to bridge 415 | queue_size: 1 # Queue size 416 | services_2_to_1: 417 | - 418 | service: /add_two_ints # ROS 1 service name 419 | type: roscpp_tutorials/TwoInts # The ROS 1 service type name 420 | ``` 421 | 422 | Start a ROS 1 roscore: 423 | 424 | ```bash 425 | # Shell A (ROS 1 only): 426 | source ${ROS1_INSTALL_PATH}/setup.bash 427 | # Or, on OSX, something like: 428 | # . ~/ros_catkin_ws/install_isolated/setup.bash 429 | roscore 430 | ``` 431 | 432 | Then load the bridge.yaml config file and start the talker to publish on the `/chatter` topic: 433 | 434 | ```bash 435 | Shell B: (ROS 1 only): 436 | source ${ROS1_INSTALL_PATH}/setup.bash 437 | # Or, on OSX, something like: 438 | # . ~/ros_catkin_ws/install_isolated/setup.bash 439 | rosparam load bridge.yaml 440 | 441 | rosrun rospy_tutorials talker 442 | ``` 443 | 444 | ```bash 445 | Shell C: (ROS 1 only): 446 | source ${ROS1_INSTALL_PATH}/setup.bash 447 | # Or, on OSX, something like: 448 | # . ~/ros_catkin_ws/install_isolated/setup.bash 449 | 450 | rosrun roscpp_tutorials add_two_ints_server 451 | ``` 452 | 453 | Then, in a few ROS 2 terminals: 454 | 455 | ```bash 456 | # Shell D: 457 | source ${ROS2_INSTALL_PATH}/setup.bash 458 | ros2 run ros1_bridge parameter_bridge 459 | ``` 460 | 461 | If all is well, the logging shows it is creating bridges for the topic and service and you should be able to call the service and listen to the ROS 1 talker from ROS 2: 462 | 463 | ```bash 464 | # Shell E: 465 | source ${ROS2_INSTALL_PATH}/setup.bash 466 | ros2 run demo_nodes_cpp listener 467 | ``` 468 | This should start printing text like `I heard: [hello world ...]` with a timestamp. 469 | 470 | ```bash 471 | # Shell F: 472 | source ${ROS2_INSTALL_PATH}/setup.bash 473 | ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 1, b: 2}" 474 | ``` 475 | If all is well, the output should contain `example_interfaces.srv.AddTwoInts_Response(sum=3)` 476 | 477 | ### Parametrizing Quality of Service 478 | An advantage of ROS 2 over ROS 1 is the possibility to define different Quality of Service settings per topic. 479 | The parameter bridge optionally allows for this as well. 480 | For some topics, like `/tf_static` this is actually required, as this is a latching topic in ROS 1. 481 | In ROS 2 with the `parameter_bridge`, this requires that topic to be configured as such: 482 | 483 | ```yaml 484 | topics: 485 | - 486 | topic: /tf_static 487 | type: tf2_msgs/msg/TFMessage 488 | queue_size: 1 489 | qos: 490 | history: keep_all 491 | durability: transient_local 492 | ``` 493 | 494 | All other QoS options (as documented here in https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html) are available: 495 | 496 | ```yaml 497 | topics: 498 | - 499 | topic: /some_ros1_topic 500 | type: std_msgs/msg/String 501 | queue_size: 1 502 | qos: 503 | history: keep_last # OR keep_all, then you can omit `depth` parameter below 504 | depth: 10 # Only required when history == keep_last 505 | reliability: reliable # OR best_effort 506 | durability: transient_local # OR volatile 507 | deadline: 508 | secs: 10 509 | nsecs: 2345 510 | lifespan: 511 | secs: 20 512 | nsecs: 3456 513 | liveliness: liveliness_system_default # Values from https://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html, eg. LIVELINESS_AUTOMATIC 514 | liveliness_lease_duration: 515 | secs: 40 516 | nsecs: 5678 517 | ``` 518 | 519 | Note that the `qos` section can be omitted entirely and options not set are left default. 520 | -------------------------------------------------------------------------------- /bin/ros1_bridge_generate_factories: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import os 5 | import sys 6 | 7 | # add ros1_bridge to the Python path 8 | ros1_bridge_root = os.path.join(os.path.dirname(__file__), '..') 9 | sys.path.insert(0, os.path.abspath(ros1_bridge_root)) 10 | 11 | from ros1_bridge import generate_cpp 12 | 13 | 14 | def main(argv=sys.argv[1:]): 15 | parser = argparse.ArgumentParser( 16 | description='Generate C++ template specializations for all ROS message types.', 17 | formatter_class=argparse.ArgumentDefaultsHelpFormatter) 18 | parser.add_argument( 19 | '--output-path', 20 | required=True, 21 | help='The basepath of the generated C++ files') 22 | parser.add_argument( 23 | '--template-dir', 24 | required=True, 25 | help='The location of the template files') 26 | args = parser.parse_args(argv) 27 | 28 | try: 29 | return generate_cpp( 30 | args.output_path, 31 | args.template_dir, 32 | ) 33 | except RuntimeError as e: 34 | print(str(e), file=sys.stderr) 35 | return 1 36 | 37 | 38 | if __name__ == '__main__': 39 | sys.exit(main()) 40 | -------------------------------------------------------------------------------- /cmake/find_ros1_interface_packages.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | function(find_ros1_interface_packages var) 16 | # rosmsg/rossrv require the ROS 1 packages to be first in the PYTHONPATH 17 | # therefore we temporarily remove every ROS 2 path from the PYTHONPATH 18 | file(TO_CMAKE_PATH "$ENV{AMENT_PREFIX_PATH}" AMENT_PREFIX_PATH) 19 | set(PYTHONPATH "$ENV{PYTHONPATH}") 20 | file(TO_CMAKE_PATH "${PYTHONPATH}" CMAKE_PYTHONPATH) 21 | set(PYTHONPATH_WITHOUT_ROS2 "") 22 | foreach(python_path IN LISTS CMAKE_PYTHONPATH) 23 | set(match FALSE) 24 | foreach(ament_prefix_path IN LISTS AMENT_PREFIX_PATH) 25 | if(python_path MATCHES "^${ament_prefix_path}/") 26 | set(match TRUE) 27 | break() 28 | endif() 29 | endforeach() 30 | if(NOT match) 31 | list(APPEND PYTHONPATH_WITHOUT_ROS2 "${python_path}") 32 | endif() 33 | endforeach() 34 | file(TO_NATIVE_PATH "${PYTHONPATH_WITHOUT_ROS2}" PYTHONPATH_WITHOUT_ROS2) 35 | if(NOT WIN32) 36 | string(REPLACE ";" ":" PYTHONPATH_WITHOUT_ROS2 "${PYTHONPATH_WITHOUT_ROS2}") 37 | endif() 38 | set(ENV{PYTHONPATH} "${PYTHONPATH_WITHOUT_ROS2}") 39 | 40 | # find all known ROS1 message/service packages 41 | execute_process( 42 | COMMAND "rosmsg" "list" 43 | OUTPUT_VARIABLE rosmsg_output 44 | ERROR_VARIABLE rosmsg_error 45 | ) 46 | execute_process( 47 | COMMAND "rossrv" "list" 48 | OUTPUT_VARIABLE rossrv_output 49 | ERROR_VARIABLE rossrv_error 50 | ) 51 | 52 | # restore PYTHONPATH 53 | set(ENV{PYTHONPATH} ${PYTHONPATH}) 54 | 55 | if(NOT rosmsg_error STREQUAL "" OR NOT rossrv_error STREQUAL "") 56 | message(FATAL_ERROR "${rosmsg_error}\n${rossrv_error}\nFailed to call rosmsg/rossrv") 57 | endif() 58 | 59 | set(ros1_message_packages "") 60 | string(REPLACE "\n" ";" ros1_interfaces "${rosmsg_output}\n${rossrv_output}") 61 | foreach(interface_package ${ros1_interfaces}) 62 | string(REGEX REPLACE "/.*$" "" interface_package "${interface_package}") 63 | list_append_unique(ros1_message_packages ${interface_package}) 64 | endforeach() 65 | set(${var} ${ros1_message_packages} PARENT_SCOPE) 66 | endfunction() 67 | -------------------------------------------------------------------------------- /cmake/find_ros1_package.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | macro(find_ros1_package name) 16 | cmake_parse_arguments(_ARG "REQUIRED" "" "" ${ARGN}) 17 | if(_ARG_UNPARSED_ARGUMENTS) 18 | message(FATAL_ERROR 19 | "find_ros1_package() called with unused arguments: " 20 | "${ARG_UNPARSED_ARGUMENTS}") 21 | endif() 22 | 23 | find_package(PkgConfig REQUIRED) 24 | # the error message of pkg_check_modules for not found required modules 25 | # doesn't include the module name which is not helpful 26 | pkg_check_modules(ros1_${name} ${name}) 27 | if(NOT ros1_${name}_FOUND) 28 | if(_ARG_REQUIRED) 29 | message(FATAL_ERROR 30 | "find_ros1_package() failed to find '${name}' using " 31 | "pkg_check_modules()") 32 | endif() 33 | else() 34 | set(_libraries "${ros1_${name}_LIBRARIES}") 35 | # Prior to catkin 0.7.7, dependent libraries in the pkg-config file were always generated 36 | # in the form "-l:/absolute/path/to/library". Because of the leading "-l", this made 37 | # them show up in ${ros1_${name}_LIBRARIES}. catkin 0.7.7 and later has changed this to 38 | # just be an absolute path (of the form "/absolute/path/to/library"), so we now have to 39 | # look through the LDFLAGS_OTHER and add those to the libraries that we need to link with. 40 | foreach(_flag ${ros1_${name}_LDFLAGS_OTHER}) 41 | if(IS_ABSOLUTE ${_flag}) 42 | list(APPEND _libraries "${_flag}") 43 | endif() 44 | endforeach() 45 | set(_library_dirs "${ros1_${name}_LIBRARY_DIRS}") 46 | set(ros1_${name}_LIBRARIES "") 47 | set(ros1_${name}_LIBRARY_DIRS "") 48 | foreach(_library ${_libraries}) 49 | string(SUBSTRING "${_library}" 0 1 _prefix) 50 | if("${_prefix} " STREQUAL ": ") 51 | string(SUBSTRING "${_library}" 1 -1 _rest) 52 | list(APPEND ros1_${name}_LIBRARIES ${_rest}) 53 | elseif(IS_ABSOLUTE ${_library}) 54 | list(APPEND ros1_${name}_LIBRARIES ${_library}) 55 | else() 56 | set(_lib "${_library}-NOTFOUND") 57 | set(_lib_path "") 58 | # since the path where the library is found is not returned 59 | # this has to be done for each path separately 60 | foreach(_path ${_library_dirs}) 61 | find_library(_lib ${_library} 62 | PATHS ${_path} 63 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 64 | if(_lib) 65 | set(_lib_path ${_path}) 66 | break() 67 | endif() 68 | endforeach() 69 | if(_lib) 70 | list(APPEND ros1_${name}_LIBRARIES ${_lib}) 71 | list_append_unique(ros1_${name}_LIBRARY_DIRS ${_lib_path}) 72 | else() 73 | # as a fall back try to search globally 74 | find_library(_lib ${_library}) 75 | if(NOT _lib) 76 | message(FATAL_ERROR "pkg-config module '${name}' failed to find library '${_library}'") 77 | endif() 78 | list(APPEND ros1_${name}_LIBRARIES ${_lib}) 79 | endif() 80 | endif() 81 | endforeach() 82 | endif() 83 | endmacro() 84 | -------------------------------------------------------------------------------- /doc/index.rst: -------------------------------------------------------------------------------- 1 | ros1_bridge 2 | =========== 3 | 4 | The bridge is a "dual homed" program which acts as a ROS 1 node as well as a ROS 2 node at the same time. 5 | It can therefore subscribe to messages in one ROS version and publish them into the other ROS version. 6 | 7 | The specific messages it is able to convert between ROS 1 and ROS 2 are determined at compile time. 8 | Automatic mapping rules are being applied based on the names. 9 | Additionally custom mapping rules can be provided. 10 | 11 | 12 | How are ROS 1 and 2 messages associated with each other? 13 | -------------------------------------------------------- 14 | 15 | Automatic mapping between ROS 1 and 2 messages is performed based on the names. 16 | 17 | In the first step ROS 1 packages ending with ``_msgs`` are associated with ROS 2 packages ending in either ``_msgs`` or ``_interfaces``. 18 | 19 | In the second step messages with the same name are associated with each other. 20 | 21 | In the third step fields are associated with each other. 22 | If one of the two associated messages has fields which is are not part of the other message they are being ignored. 23 | If both messages have fields the other message does not contain it is assumed that the mapping is incomplete and no association is established. 24 | 25 | If the package name, message name, and all field names and types are the same between a ROS 1 package and a ROS 2 package, and they satisfy the package suffix condition above, then the messages will automatically be associated with each other without additional specification. 26 | The bridge needs to be recompiled, after the ROS 1 and ROS 2 packages have been compiled and sourced, each in a separate workspace, for a total of three workspaces. 27 | 28 | How can I specify custom mapping rule for messages? 29 | --------------------------------------------------- 30 | 31 | Additional mapping rules can be provided by a ROS 2 package using a ``yaml`` file. 32 | Each mapping rule can have one of three types: 33 | 34 | 1. A package mapping rule is defined by: 35 | 36 | - a ``ros1_package_name`` 37 | - a ``ros2_package_name`` (which, by default, must be the same as the ROS 2 package this mapping rule is defined in) 38 | 39 | 2. A message mapping rule is defined by the attributes of a package mapping rule and: 40 | 41 | - a ``ros1_message_name`` 42 | - a ``ros2_message_name`` 43 | 44 | 3. A field mapping rule is defined by the attributes of a message mapping rule and: 45 | 46 | - a dictionary ``fields_1_to_2`` mapping ROS 1 field selections to ROS 2 field selections. 47 | A field selection is a sequence of field names separated by ``.``, that specifies the path to a field starting from a message. 48 | For example starting from the message ``rosgraph_msgs/Log`` the field selection ``header.stamp`` specifies a 49 | path that goes through the field ``header`` of ``rosgraph_msgs/Log``, that has a message of type ``std_msgs/Header``, 50 | and ending up in the field ``stamp`` of ``std_msgs/Header``, that has type ``time``. 51 | All fields must be listed explicitly - not listed fields are not mapped implicitly when their names match. 52 | 53 | Each mapping rule file contains a list of mapping rules. 54 | 55 | How are ROS 1 and 2 services associated with each other? 56 | -------------------------------------------------------- 57 | 58 | Automatic mapping between ROS 1 and 2 services is performed similar to messages. 59 | Except that currently different field names are not supported. 60 | 61 | How can I specify custom mapping rule for services? 62 | --------------------------------------------------- 63 | 64 | In case of services, each mapping rule can have one of two types: 65 | 66 | 1. A package mapping rule is defined by: 67 | 68 | - a ``ros1_package_name`` 69 | - a ``ros2_package_name`` (which, by default, must be the same as the ROS 2 package this mapping rule is defined in) 70 | 71 | 2. A service name mapping rule is defined by the attributes of a package mapping rule and: 72 | 73 | - a ``ros1_service_name`` 74 | - a ``ros2_service_name`` 75 | 76 | A custom field mapping is currently not supported for services. 77 | 78 | How can I install mapping rule files? 79 | ------------------------------------- 80 | 81 | The mapping rule files must be exported in the ROS 2 package's ``package.xml`` in order to be processed by this package:: 82 | 83 | 84 | 85 | 86 | 87 | The yaml files must also be installed in the ROS 2 package's ``CMakeLists.txt``:: 88 | 89 | install( 90 | FILES my_mapping_rules.yaml 91 | DESTINATION share/${PROJECT_NAME}) 92 | 93 | 94 | Example mapping rules file 95 | -------------------------- 96 | 97 | Example package mapping rule matching all messages with the same names and fields within a pair of packages:: 98 | 99 | - 100 | ros1_package_name: 'ros1_pkg_name' 101 | ros2_package_name: 'ros2_pkg_name' 102 | 103 | Example message mapping rule matching a specific pair of messages with the same fields:: 104 | 105 | - 106 | ros1_package_name: 'ros1_pkg_name' 107 | ros1_message_name: 'ros1_msg_name' 108 | ros2_package_name: 'ros2_pkg_name' 109 | ros2_message_name: 'ros2_msg_name' 110 | 111 | Example message field mapping rule a specific pair of messages with a custom mapping between the fields:: 112 | 113 | - 114 | ros1_package_name: 'ros1_pkg_name' 115 | ros1_message_name: 'ros1_msg_name' 116 | ros2_package_name: 'ros2_pkg_name' 117 | ros2_message_name: 'ros2_msg_name' 118 | fields_1_to_2: 119 | foo: 'foo' 120 | ros1_bar: 'ros2_bar' 121 | 122 | Example service mapping rule matching all services with the same names and fields within a pair of packages:: 123 | 124 | - 125 | ros1_package_name: 'ros1_pkg_name' 126 | ros2_package_name: 'ros2_pkg_name' 127 | 128 | Example service mapping rule matching a specific pair of services with the same fields:: 129 | 130 | - 131 | ros1_package_name: 'ros1_pkg_name' 132 | ros1_service_name: 'ros1_srv_name' 133 | ros2_package_name: 'ros2_pkg_name' 134 | ros2_service_name: 'ros2_srv_name' 135 | 136 | Example service mapping rule matching a specific pair of services with a custom mapping between the fields. 137 | The mapping can optionally only define ``request_fields_1_to_2`` or ``response_fields_1_to_2`` if the other part has the same fields:: 138 | 139 | - 140 | ros1_package_name: 'ros1_pkg_name' 141 | ros1_service_name: 'ros1_srv_name' 142 | ros2_package_name: 'ros2_pkg_name' 143 | ros2_service_name: 'ros2_srv_name' 144 | request_fields_1_to_2: 145 | foo: 'foo' 146 | ros1_bar: 'ros2_bar' 147 | response_fields_1_to_2: 148 | foo: 'foo' 149 | ros1_bar: 'ros2_bar' 150 | 151 | 152 | How does the bridge know about custom interfaces? 153 | ------------------------------------------------- 154 | 155 | The ROS 1 and ROS 2 packages need to be in separate workspaces, so that each workspace can be sourced with its correponding ROS version. 156 | The bridge should be in its own workspace, as it will need to source both ROS 1 and ROS 2 versions. 157 | 158 | Example workspace setup 159 | ----------------------- 160 | 161 | Here we will call the ROS 1 workspace ``ros1_msgs_ws``, the ROS 2 workspace ``ros2_msgs_ws``, the workspace containing the bridge ``bridge_ws``. 162 | For simplification, we will use matching names for the packages, messages, and fields of the custom interfaces on the ROS 1 and ROS 2 sides. 163 | We will call the actual package ``bridge_msgs`` in both the ROS 1 and the ROS 2 workspaces. 164 | That is, the name defined in ``CMakeLists.txt`` and ``package.xml`` in the package. 165 | 166 | The directory layout looks like this:: 167 | 168 | . 169 | ├─ ros1_msgs_ws 170 | │ └─ src 171 | │ └─ bridge_msgs 172 | │ └─ msg 173 | │ └─ JointCommand.msg 174 | ├─ ros2_msgs_ws 175 | │ └─ src 176 | │ └─ bridge_msgs 177 | │ ├─ msg 178 | │ │ └─ JointCommand.msg 179 | │ └─ # YAML file if your custom interfaces have non-matching names 180 | └─ bridge_ws 181 | └─ src 182 | └─ ros1_bridge 183 | 184 | The content of JointCommand.msg:: 185 | 186 | float64 position 187 | 188 | The workspaces can be compiled as follows. 189 | 190 | First, build the ROS 1 messages:: 191 | 192 | # Shell 1 (ROS 1) 193 | . /opt/ros/melodic/setup.bash 194 | # Or, on OSX, something like: 195 | # . ~/ros_catkin_ws/install_isolated/setup.bash 196 | cd /ros1_msgs_ws 197 | catkin_make_isolated --install 198 | 199 | Then build the ROS 2 messages:: 200 | 201 | # Shell 2 (ROS 2) 202 | . /opt/ros/crystal/setup.bash 203 | cd /ros2_msgs_ws 204 | colcon build --packages-select bridge_msgs 205 | 206 | Then build the bridge:: 207 | 208 | # Shell 3 (ROS 1 and ROS 2) 209 | . /opt/ros/melodic/setup.bash 210 | . /opt/ros/crystal/setup.bash 211 | . /ros1_msgs_ws/install_isolated/setup.bash 212 | . /ros2_msgs_ws/install/local_setup.bash 213 | cd /bridge_ws 214 | colcon build --packages-select ros1_bridge --cmake-force-configure 215 | 216 | Verify the custom types were recognized by the bridge, by printing all pairs of bridged types. 217 | The custom types should be listed:: 218 | 219 | ros2 run ros1_bridge dynamic_bridge --print-pairs 220 | 221 | Run the bridge, reusing shells from above:: 222 | 223 | # Shell 1 (ROS 1) 224 | roscore 225 | 226 | # Shell 2 (ROS 2) 227 | . /ros2_msgs_ws/install/local_setup.bash 228 | ros2 topic pub /joint_command bridge_msgs/JointCommand "{position: 0.123}" 229 | 230 | # Shell 3 (ROS 1 and ROS 2) 231 | . /bridge_ws/install/local_setup.bash 232 | ros2 run ros1_bridge dynamic_bridge --bridge-all-topics 233 | 234 | # Shell 4 (ROS 1) 235 | . /opt/ros/melodic/setup.bash 236 | . /ros1_msgs_ws/install_isolated/setup.bash 237 | # Verify the topic is listed 238 | rostopic list 239 | rostopic echo /joint_command 240 | 241 | 242 | How can I define a mapping rules for a foreign package, from a foreign package? 243 | ------------------------------------------------------------------------------- 244 | 245 | In the previous sections, it was stated that the ``ros2_package_name`` mapping rule must be the same as the ROS 2 package the mapping rule was defined in. 246 | While this is **recommended** to prevent conflicting and/or duplicate rules, it is possible to override the check that enforces this with the ``enable_foreign_mappings`` field. 247 | 248 | This will mean that, for every package mapping rule defined in the ``yaml`` file, the check for ROS 2 package name equality will be skipped. 249 | Again, note that this is a dark art that should be wielded with responsibility, please be very careful with this! 250 | If there are conflicting mapping rules, the last one that is parsed in sorting order is used! 251 | This is usually hard to predict, so be very careful! 252 | 253 | With ``enable_foreign_mappings`` set to ``true``, you can then specify mapping rules for ROS 2 packages that are not the same as the package your mapping rules file resides in:: 254 | 255 | - 256 | enable_foreign_mappings: true 257 | ros1_package_name: 'ros1_pkg_name' 258 | ros1_service_name: 'ros1_srv_name' 259 | ros2_package_name: 'ros2_FOREIGN_pkg_name' # the package with the message definition 260 | ros2_service_name: 'ros2_srv_name' 261 | 262 | You must also make the ``ros1_bridge_foreign_mapping`` ament resource available to the index in the mapping package's ``CMakeLists.txt``. 263 | A good place to put the line is to do this is before the install rule for your mapping rules, like so:: 264 | 265 | - 266 | ament_index_register_resource("ros1_bridge_foreign_mapping") 267 | install( 268 | FILES YOUR_MAPPING_RULE_FILE.yaml 269 | DESTINATION share/${PROJECT_NAME}) 270 | 271 | **Note**: In this case, the package name you should put in ```ros2_package_name`` is the name of the package with the message definition. 272 | In effect, it'll be a foreign package, relative to the mapping package you're defining your mapping rules in. 273 | 274 | An example directory layout looks like this:: 275 | 276 | . 277 | ├─ ros1_msgs_ws 278 | │ └─ src 279 | │ └─ ros1_bridge_msgs 280 | │ └─ msg 281 | │ └─ ros1_msg_name.msg 282 | ├─ ros2_msgs_ws 283 | │ └─ src 284 | │ └─ ros2_bridge_msgs 285 | │ │ ├─ msg 286 | │ │ │ └─ ros2_msg_name.msg 287 | │ └─ ros2_bridge_mappings 288 | │ └─ # YAML file that defines mapping rules for bridge_msgs, or some other foreign msg package 289 | └─ bridge_ws 290 | └─ src 291 | └─ ros1_bridge 292 | 293 | In the above example, the mapping rule would look like this:: 294 | 295 | - 296 | enable_foreign_mappings: true 297 | ros1_package_name: 'ros1_bridge_msgs' 298 | ros1_message_name: 'ros1_msg_name' 299 | ros2_package_name: 'ros2_bridge_msgs' # this is a foreign package, relative to ros2_bridge_mappings! 300 | ros2_message_name: 'ros2_msg_name' 301 | 302 | 303 | Known Issues 304 | ------------ 305 | 306 | - Currently, ``--bridge-all-topics`` may be needed to bridge correctly. 307 | This has been `tested and reported `_ to be required when at least one custom message type is being published in ROS 2 and subscribed in ROS 1. 308 | It has also been informally reported to be needed for built-in message types under the same condition. 309 | Once the mapping is established with the ROS 1 master, it may be possible to rerun the bridge without ``--bridge-all-topics``, in order to selectively bridge topics. 310 | However, this is not guaranteed. 311 | - ``/rosout`` logging, which maps from ``rosgraph_msgs/Log`` to ``rcl_interfaces/Log``, requires `field selection `_. 312 | This `works with OpenSplice and Connext `_, but `not with Fast-RTPS `_. 313 | For it to work with Fast-RTPS, `this bug `_ needs to be fixed. 314 | As a workaround, run the subscriber with ``--disable-rosout-logs``. 315 | -------------------------------------------------------------------------------- /doc/ros1_talker_ros2_listener.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/ros1_bridge/6c5ad167139ae64a42c8a202614f7c84004081ee/doc/ros1_talker_ros2_listener.png -------------------------------------------------------------------------------- /doc/ros2_camera_ros1_rqt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/ros1_bridge/6c5ad167139ae64a42c8a202614f7c84004081ee/doc/ros2_camera_ros1_rqt.png -------------------------------------------------------------------------------- /include/ros1_bridge/bridge.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ROS1_BRIDGE__BRIDGE_HPP_ 16 | #define ROS1_BRIDGE__BRIDGE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | // include ROS 1 23 | #include "ros/node_handle.h" 24 | 25 | // include ROS 2 26 | #include "rclcpp/node.hpp" 27 | 28 | #include "ros1_bridge/factory_interface.hpp" 29 | 30 | namespace ros1_bridge 31 | { 32 | 33 | struct Bridge1to2Handles 34 | { 35 | ros::Subscriber ros1_subscriber; 36 | rclcpp::PublisherBase::SharedPtr ros2_publisher; 37 | }; 38 | 39 | struct Bridge2to1Handles 40 | { 41 | rclcpp::SubscriptionBase::SharedPtr ros2_subscriber; 42 | ros::Publisher ros1_publisher; 43 | }; 44 | 45 | struct BridgeHandles 46 | { 47 | Bridge1to2Handles bridge1to2; 48 | Bridge2to1Handles bridge2to1; 49 | }; 50 | 51 | bool 52 | get_1to2_mapping( 53 | const std::string & ros1_type_name, 54 | std::string & ros2_type_name); 55 | 56 | bool 57 | get_2to1_mapping( 58 | const std::string & ros2_type_name, 59 | std::string & ros1_type_name); 60 | 61 | std::multimap 62 | get_all_message_mappings_2to1(); 63 | 64 | std::multimap 65 | get_all_service_mappings_2to1(); 66 | 67 | std::shared_ptr 68 | get_factory( 69 | const std::string & ros1_type_name, 70 | const std::string & ros2_type_name); 71 | 72 | std::unique_ptr 73 | get_service_factory(const std::string &, const std::string &, const std::string &); 74 | 75 | Bridge1to2Handles 76 | create_bridge_from_1_to_2( 77 | ros::NodeHandle ros1_node, 78 | rclcpp::Node::SharedPtr ros2_node, 79 | const std::string & ros1_type_name, 80 | const std::string & ros1_topic_name, 81 | size_t subscriber_queue_size, 82 | const std::string & ros2_type_name, 83 | const std::string & ros2_topic_name, 84 | size_t publisher_queue_size); 85 | 86 | Bridge1to2Handles 87 | create_bridge_from_1_to_2( 88 | ros::NodeHandle ros1_node, 89 | rclcpp::Node::SharedPtr ros2_node, 90 | const std::string & ros1_type_name, 91 | const std::string & ros1_topic_name, 92 | size_t subscriber_queue_size, 93 | const std::string & ros2_type_name, 94 | const std::string & ros2_topic_name, 95 | const rclcpp::QoS & publisher_qos); 96 | 97 | Bridge2to1Handles 98 | create_bridge_from_2_to_1( 99 | rclcpp::Node::SharedPtr ros2_node, 100 | ros::NodeHandle ros1_node, 101 | const std::string & ros2_type_name, 102 | const std::string & ros2_topic_name, 103 | size_t subscriber_queue_size, 104 | const std::string & ros1_type_name, 105 | const std::string & ros1_topic_name, 106 | size_t publisher_queue_size, 107 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr); 108 | 109 | Bridge2to1Handles 110 | create_bridge_from_2_to_1( 111 | rclcpp::Node::SharedPtr ros2_node, 112 | ros::NodeHandle ros1_node, 113 | const std::string & ros2_type_name, 114 | const std::string & ros2_topic_name, 115 | const rclcpp::QoS & subscriber_qos, 116 | const std::string & ros1_type_name, 117 | const std::string & ros1_topic_name, 118 | size_t publisher_queue_size, 119 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr); 120 | 121 | BridgeHandles 122 | create_bidirectional_bridge( 123 | ros::NodeHandle ros1_node, 124 | rclcpp::Node::SharedPtr ros2_node, 125 | const std::string & ros1_type_name, 126 | const std::string & ros2_type_name, 127 | const std::string & topic_name, 128 | size_t queue_size = 10); 129 | 130 | BridgeHandles 131 | create_bidirectional_bridge( 132 | ros::NodeHandle ros1_node, 133 | rclcpp::Node::SharedPtr ros2_node, 134 | const std::string & ros1_type_name, 135 | const std::string & ros2_type_name, 136 | const std::string & topic_name, 137 | size_t queue_size, 138 | const rclcpp::QoS & publisher_qos); 139 | 140 | } // namespace ros1_bridge 141 | 142 | #endif // ROS1_BRIDGE__BRIDGE_HPP_ 143 | -------------------------------------------------------------------------------- /include/ros1_bridge/builtin_interfaces_factories.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_ 16 | #define ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_ 17 | 18 | #include 19 | 20 | // include ROS 1 messages 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | // include ROS 2 messages 29 | #include 30 | #include 31 | #include 32 | 33 | #include "ros1_bridge/factory.hpp" 34 | 35 | namespace ros1_bridge 36 | { 37 | 38 | std::shared_ptr 39 | get_factory_builtin_interfaces( 40 | const std::string & ros1_type_name, 41 | const std::string & ros2_type_name); 42 | 43 | // conversion functions for available interfaces 44 | 45 | template<> 46 | void 47 | Factory< 48 | std_msgs::Duration, 49 | builtin_interfaces::msg::Duration 50 | >::convert_1_to_2( 51 | const std_msgs::Duration & ros1_msg, 52 | builtin_interfaces::msg::Duration & ros2_msg); 53 | 54 | template<> 55 | void 56 | Factory< 57 | std_msgs::Duration, 58 | builtin_interfaces::msg::Duration 59 | >::convert_2_to_1( 60 | const builtin_interfaces::msg::Duration & ros2_msg, 61 | std_msgs::Duration & ros1_msg); 62 | 63 | template<> 64 | void 65 | Factory< 66 | std_msgs::Duration, 67 | builtin_interfaces::msg::Duration 68 | >::internal_stream_translate_helper( 69 | ros::serialization::OStream & stream, 70 | const builtin_interfaces::msg::Duration & msg); 71 | 72 | template<> 73 | void 74 | Factory< 75 | std_msgs::Duration, 76 | builtin_interfaces::msg::Duration 77 | >::internal_stream_translate_helper( 78 | ros::serialization::IStream & stream, 79 | builtin_interfaces::msg::Duration & msg); 80 | 81 | template<> 82 | void 83 | Factory< 84 | std_msgs::Duration, 85 | builtin_interfaces::msg::Duration 86 | >::internal_stream_translate_helper( 87 | ros::serialization::LStream & stream, 88 | const builtin_interfaces::msg::Duration & msg); 89 | 90 | template<> 91 | void 92 | Factory< 93 | std_msgs::Time, 94 | builtin_interfaces::msg::Time 95 | >::convert_1_to_2( 96 | const std_msgs::Time & ros1_msg, 97 | builtin_interfaces::msg::Time & ros2_msg); 98 | 99 | template<> 100 | void 101 | Factory< 102 | std_msgs::Time, 103 | builtin_interfaces::msg::Time 104 | >::convert_2_to_1( 105 | const builtin_interfaces::msg::Time & ros2_msg, 106 | std_msgs::Time & ros1_msg); 107 | 108 | template<> 109 | void 110 | Factory< 111 | std_msgs::Time, 112 | builtin_interfaces::msg::Time 113 | >::internal_stream_translate_helper( 114 | ros::serialization::OStream & stream, 115 | const builtin_interfaces::msg::Time & msg); 116 | 117 | template<> 118 | void 119 | Factory< 120 | std_msgs::Time, 121 | builtin_interfaces::msg::Time 122 | >::internal_stream_translate_helper( 123 | ros::serialization::IStream & stream, 124 | builtin_interfaces::msg::Time & msg); 125 | 126 | template<> 127 | void 128 | Factory< 129 | std_msgs::Time, 130 | builtin_interfaces::msg::Time 131 | >::internal_stream_translate_helper( 132 | ros::serialization::LStream & stream, 133 | const builtin_interfaces::msg::Time & msg); 134 | 135 | } // namespace ros1_bridge 136 | 137 | #endif // ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_ 138 | -------------------------------------------------------------------------------- /include/ros1_bridge/convert_builtin_interfaces.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ROS1_BRIDGE__CONVERT_BUILTIN_INTERFACES_HPP_ 16 | #define ROS1_BRIDGE__CONVERT_BUILTIN_INTERFACES_HPP_ 17 | 18 | #include "ros1_bridge/convert_decl.hpp" 19 | 20 | // include ROS 1 builtin messages 21 | #include "ros/duration.h" 22 | #include "ros/time.h" 23 | 24 | // include ROS 2 builtin messages 25 | #include "builtin_interfaces/msg/duration.hpp" 26 | #include "builtin_interfaces/msg/time.hpp" 27 | 28 | namespace ros1_bridge 29 | { 30 | 31 | template<> 32 | void 33 | convert_1_to_2( 34 | const ros::Duration & ros1_type, 35 | builtin_interfaces::msg::Duration & ros2_msg); 36 | 37 | template<> 38 | void 39 | convert_2_to_1( 40 | const builtin_interfaces::msg::Duration & ros2_msg, 41 | ros::Duration & ros1_type); 42 | 43 | template 44 | void 45 | internal_stream_translate_helper( 46 | STREAM_T & stream, 47 | const builtin_interfaces::msg::Duration & msg); 48 | 49 | template 50 | void 51 | internal_stream_translate_helper( 52 | STREAM_T & stream, 53 | builtin_interfaces::msg::Duration & msg); 54 | 55 | 56 | template<> 57 | void 58 | convert_1_to_2( 59 | const ros::Time & ros1_type, 60 | builtin_interfaces::msg::Time & ros2_msg); 61 | 62 | template<> 63 | void 64 | convert_2_to_1( 65 | const builtin_interfaces::msg::Time & ros2_msg, 66 | ros::Time & ros1_type); 67 | 68 | template 69 | void 70 | internal_stream_translate_helper( 71 | STREAM_T & stream, 72 | const builtin_interfaces::msg::Time & msg); 73 | 74 | template 75 | void 76 | internal_stream_translate_helper( 77 | STREAM_T & stream, 78 | builtin_interfaces::msg::Time & msg); 79 | 80 | } // namespace ros1_bridge 81 | 82 | #endif // ROS1_BRIDGE__CONVERT_BUILTIN_INTERFACES_HPP_ 83 | -------------------------------------------------------------------------------- /include/ros1_bridge/convert_decl.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ROS1_BRIDGE__CONVERT_DECL_HPP_ 16 | #define ROS1_BRIDGE__CONVERT_DECL_HPP_ 17 | 18 | namespace ros1_bridge 19 | { 20 | 21 | template 22 | void 23 | convert_1_to_2( 24 | const ROS1_T & ros1_msg, 25 | ROS2_T & ros2_msg); 26 | 27 | template 28 | void 29 | convert_2_to_1( 30 | const ROS2_T & ros2_msg, 31 | ROS1_T & ros1_msg); 32 | 33 | template 34 | void 35 | internal_stream_translate_helper( 36 | STREAM_T & out_stream, 37 | const ROS2_T & msg); 38 | 39 | } // namespace ros1_bridge 40 | 41 | #endif // ROS1_BRIDGE__CONVERT_DECL_HPP_ 42 | -------------------------------------------------------------------------------- /include/ros1_bridge/factory.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ROS1_BRIDGE__FACTORY_HPP_ 16 | #define ROS1_BRIDGE__FACTORY_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "rmw/rmw.h" 25 | #include "rclcpp/rclcpp.hpp" 26 | 27 | // include ROS 1 message event 28 | #include "ros/message.h" 29 | 30 | #include "rcutils/logging_macros.h" 31 | 32 | #include "ros1_bridge/factory_interface.hpp" 33 | 34 | namespace ros1_bridge 35 | { 36 | 37 | template 38 | class Factory : public FactoryInterface 39 | { 40 | public: 41 | Factory( 42 | const std::string & ros1_type_name, const std::string & ros2_type_name) 43 | : ros1_type_name_(ros1_type_name), 44 | ros2_type_name_(ros2_type_name) 45 | { 46 | ts_lib_ = rclcpp::get_typesupport_library(ros2_type_name, "rosidl_typesupport_cpp"); 47 | if (static_cast(ts_lib_)) { 48 | type_support_ = rclcpp::get_typesupport_handle( 49 | ros2_type_name, "rosidl_typesupport_cpp", 50 | *ts_lib_); 51 | } 52 | } 53 | 54 | ros::Publisher 55 | create_ros1_publisher( 56 | ros::NodeHandle node, 57 | const std::string & topic_name, 58 | size_t queue_size, 59 | bool latch = false) 60 | { 61 | return node.advertise(topic_name, queue_size, latch); 62 | } 63 | 64 | rclcpp::PublisherBase::SharedPtr 65 | create_ros2_publisher( 66 | rclcpp::Node::SharedPtr node, 67 | const std::string & topic_name, 68 | size_t queue_size) 69 | { 70 | return node->create_publisher(topic_name, rclcpp::QoS(rclcpp::KeepLast(queue_size))); 71 | } 72 | 73 | rclcpp::PublisherBase::SharedPtr 74 | create_ros2_publisher( 75 | rclcpp::Node::SharedPtr node, 76 | const std::string & topic_name, 77 | const rmw_qos_profile_t & qos_profile) 78 | { 79 | auto qos = rclcpp::QoS(rclcpp::KeepAll()); 80 | qos.get_rmw_qos_profile() = qos_profile; 81 | return create_ros2_publisher(node, topic_name, qos); 82 | } 83 | 84 | rclcpp::PublisherBase::SharedPtr 85 | create_ros2_publisher( 86 | rclcpp::Node::SharedPtr node, 87 | const std::string & topic_name, 88 | const rclcpp::QoS & qos) 89 | { 90 | return node->create_publisher(topic_name, qos); 91 | } 92 | 93 | ros::Subscriber 94 | create_ros1_subscriber( 95 | ros::NodeHandle node, 96 | const std::string & topic_name, 97 | size_t queue_size, 98 | rclcpp::PublisherBase::SharedPtr ros2_pub, 99 | rclcpp::Logger logger) 100 | { 101 | // workaround for https://github.com/ros/roscpp_core/issues/22 to get the connection header 102 | ros::SubscribeOptions ops; 103 | ops.topic = topic_name; 104 | ops.queue_size = queue_size; 105 | ops.md5sum = ros::message_traits::md5sum(); 106 | ops.datatype = ros::message_traits::datatype(); 107 | ops.helper = ros::SubscriptionCallbackHelperPtr( 108 | new ros::SubscriptionCallbackHelperT &>( 109 | boost::bind( 110 | &Factory::ros1_callback, 111 | boost::placeholders::_1, ros2_pub, ros1_type_name_, ros2_type_name_, logger))); 112 | return node.subscribe(ops); 113 | } 114 | 115 | rclcpp::SubscriptionBase::SharedPtr 116 | create_ros2_subscriber( 117 | rclcpp::Node::SharedPtr node, 118 | const std::string & topic_name, 119 | size_t queue_size, 120 | ros::Publisher ros1_pub, 121 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) 122 | { 123 | auto qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); 124 | return create_ros2_subscriber(node, topic_name, qos, ros1_pub, ros2_pub); 125 | } 126 | 127 | rclcpp::SubscriptionBase::SharedPtr 128 | create_ros2_subscriber( 129 | rclcpp::Node::SharedPtr node, 130 | const std::string & topic_name, 131 | const rmw_qos_profile_t & qos, 132 | ros::Publisher ros1_pub, 133 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) 134 | { 135 | auto rclcpp_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)); 136 | rclcpp_qos.get_rmw_qos_profile() = qos; 137 | return create_ros2_subscriber( 138 | node, topic_name, rclcpp_qos, ros1_pub, ros2_pub); 139 | } 140 | 141 | rclcpp::SubscriptionBase::SharedPtr 142 | create_ros2_subscriber( 143 | rclcpp::Node::SharedPtr node, 144 | const std::string & topic_name, 145 | const rclcpp::QoS & qos, 146 | ros::Publisher ros1_pub, 147 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) 148 | { 149 | std::function< 150 | void(const typename ROS2_T::SharedPtr msg, const rclcpp::MessageInfo & msg_info)> callback; 151 | callback = std::bind( 152 | &Factory::ros2_callback, std::placeholders::_1, std::placeholders::_2, 153 | ros1_pub, ros1_type_name_, ros2_type_name_, node->get_logger(), ros2_pub); 154 | rclcpp::SubscriptionOptions options; 155 | options.ignore_local_publications = true; 156 | return node->create_subscription( 157 | topic_name, qos, callback, options); 158 | } 159 | 160 | void convert_1_to_2(const void * ros1_msg, void * ros2_msg) const override 161 | { 162 | auto typed_ros1_msg = static_cast(ros1_msg); 163 | auto typed_ros2_msg = static_cast(ros2_msg); 164 | convert_1_to_2(*typed_ros1_msg, *typed_ros2_msg); 165 | } 166 | 167 | void convert_2_to_1(const void * ros2_msg, void * ros1_msg) const override 168 | { 169 | auto typed_ros2_msg = static_cast(ros2_msg); 170 | auto typed_ros1_msg = static_cast(ros1_msg); 171 | convert_2_to_1(*typed_ros2_msg, *typed_ros1_msg); 172 | } 173 | 174 | protected: 175 | static 176 | void ros1_callback( 177 | const ros::MessageEvent & ros1_msg_event, 178 | rclcpp::PublisherBase::SharedPtr ros2_pub, 179 | const std::string & ros1_type_name, 180 | const std::string & ros2_type_name, 181 | rclcpp::Logger logger) 182 | { 183 | typename rclcpp::Publisher::SharedPtr typed_ros2_pub; 184 | typed_ros2_pub = 185 | std::dynamic_pointer_cast>(ros2_pub); 186 | 187 | if (!typed_ros2_pub) { 188 | throw std::runtime_error( 189 | "Invalid type " + ros2_type_name + " for ROS 2 publisher " + 190 | ros2_pub->get_topic_name()); 191 | } 192 | 193 | const boost::shared_ptr & connection_header = 194 | ros1_msg_event.getConnectionHeaderPtr(); 195 | if (!connection_header) { 196 | RCLCPP_WARN( 197 | logger, "Dropping ROS 1 message %s without connection header", ros1_type_name.c_str()); 198 | return; 199 | } 200 | 201 | std::string key = "callerid"; 202 | if (connection_header->find(key) != connection_header->end()) { 203 | if (connection_header->at(key) == "/ros_bridge") { 204 | return; 205 | } 206 | } 207 | 208 | const boost::shared_ptr & ros1_msg = ros1_msg_event.getConstMessage(); 209 | 210 | auto ros2_msg = std::make_unique(); 211 | convert_1_to_2(*ros1_msg, *ros2_msg); 212 | RCLCPP_INFO_ONCE( 213 | logger, "Passing message from ROS 1 %s to ROS 2 %s (showing msg only once per type)", 214 | ros1_type_name.c_str(), ros2_type_name.c_str()); 215 | typed_ros2_pub->publish(std::move(ros2_msg)); 216 | } 217 | 218 | static 219 | void ros2_callback( 220 | typename ROS2_T::SharedPtr ros2_msg, 221 | const rclcpp::MessageInfo & msg_info, 222 | ros::Publisher ros1_pub, 223 | const std::string & ros1_type_name, 224 | const std::string & ros2_type_name, 225 | rclcpp::Logger logger, 226 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) 227 | { 228 | if (ros2_pub) { 229 | bool result = false; 230 | auto ret = rmw_compare_gids_equal( 231 | &msg_info.get_rmw_message_info().publisher_gid, 232 | &ros2_pub->get_gid(), 233 | &result); 234 | if (ret == RMW_RET_OK) { 235 | if (result) { // message GID equals to bridge's ROS2 publisher GID 236 | return; // do not publish messages from bridge itself 237 | } 238 | } else { 239 | auto msg = std::string("Failed to compare gids: ") + rmw_get_error_string().str; 240 | rmw_reset_error(); 241 | throw std::runtime_error(msg); 242 | } 243 | } 244 | 245 | void * ptr = ros1_pub; 246 | if (ptr == 0) { 247 | RCLCPP_WARN_ONCE( 248 | logger, 249 | "Message from ROS 2 %s failed to be passed to ROS 1 %s because the " 250 | "ROS 1 publisher is invalid (showing msg only once per type)", 251 | ros2_type_name.c_str(), ros1_type_name.c_str()); 252 | return; 253 | } 254 | 255 | ROS1_T ros1_msg; 256 | convert_2_to_1(*ros2_msg, ros1_msg); 257 | RCLCPP_INFO_ONCE( 258 | logger, "Passing message from ROS 2 %s to ROS 1 %s (showing msg only once per type)", 259 | ros2_type_name.c_str(), ros1_type_name.c_str()); 260 | ros1_pub.publish(ros1_msg); 261 | } 262 | 263 | public: 264 | // since convert functions call each other for sub messages they must be public 265 | // defined outside of the class 266 | static 267 | void 268 | convert_1_to_2( 269 | const ROS1_T & ros1_msg, 270 | ROS2_T & ros2_msg); 271 | static 272 | void 273 | convert_2_to_1( 274 | const ROS2_T & ros2_msg, 275 | ROS1_T & ros1_msg); 276 | 277 | 278 | const char * get_ros1_md5sum() const override 279 | { 280 | return ros::message_traits::MD5Sum::value(); 281 | } 282 | 283 | const char * get_ros1_data_type() const override 284 | { 285 | return ros::message_traits::DataType::value(); 286 | } 287 | 288 | const char * get_ros1_message_definition() const override 289 | { 290 | return ros::message_traits::Definition::value(); 291 | } 292 | 293 | bool convert_2_to_1_generic( 294 | const rclcpp::SerializedMessage & ros2_msg, 295 | std::vector & ros1_msg) const override 296 | { 297 | if (type_support_ == nullptr) { 298 | return false; 299 | } 300 | 301 | // Deserialize to a ROS2 message 302 | ROS2_T ros2_typed_msg; 303 | if (rmw_deserialize( 304 | &ros2_msg.get_rcl_serialized_message(), type_support_, 305 | &ros2_typed_msg) != RMW_RET_OK) 306 | { 307 | return false; 308 | } 309 | 310 | // Call convert_2_to_1 311 | ROS1_T ros1_typed_msg; 312 | convert_2_to_1(&ros2_typed_msg, &ros1_typed_msg); 313 | 314 | // Serialize the ROS1 message into a buffer 315 | uint32_t length = ros::serialization::serializationLength(ros1_typed_msg); 316 | ros1_msg.resize(length); 317 | ros::serialization::OStream out_stream(ros1_msg.data(), length); 318 | ros::serialization::serialize(out_stream, ros1_typed_msg); 319 | 320 | return true; 321 | } 322 | 323 | bool convert_1_to_2_generic( 324 | const std::vector & ros1_msg, 325 | rclcpp::SerializedMessage & ros2_msg) const override 326 | { 327 | if (type_support_ == nullptr) { 328 | return false; 329 | } 330 | 331 | // Deserialize to a ROS1 message 332 | ROS1_T ros1_typed_msg; 333 | // Both IStream and OStream inherits their functionality from Stream 334 | // So IStream needs a non-const data reference to data 335 | // However deserialization function probably shouldn't modify data they are serializing from 336 | uint8_t * ros1_msg_data = const_cast(ros1_msg.data()); 337 | ros::serialization::IStream in_stream(ros1_msg_data, ros1_msg.size()); 338 | ros::serialization::deserialize(in_stream, ros1_typed_msg); 339 | 340 | // Call convert_1_to_2 341 | ROS2_T ros2_typed_msg; 342 | convert_1_to_2(&ros1_typed_msg, &ros2_typed_msg); 343 | 344 | // Serialize ROS2 message 345 | if (rmw_serialize( 346 | &ros2_typed_msg, type_support_, 347 | &ros2_msg.get_rcl_serialized_message()) != RMW_RET_OK) 348 | { 349 | return false; 350 | } 351 | 352 | return true; 353 | } 354 | 355 | /** 356 | * @brief Writes (serializes) a ROS2 class directly to a ROS1 stream 357 | */ 358 | static void convert_2_to_1(const ROS2_T & msg, ros::serialization::OStream & out_stream); 359 | 360 | /** 361 | * @brief Reads (deserializes) a ROS2 class directly from a ROS1 stream 362 | */ 363 | static void convert_1_to_2(ros::serialization::IStream & in_stream, ROS2_T & msg); 364 | 365 | /** 366 | * @brief Determines the length of a ROS2 class if it was serialized to a ROS1 stream 367 | */ 368 | static uint32_t length_2_as_1_stream(const ROS2_T & msg); 369 | 370 | /** 371 | * @brief Internal helper functions conversion for ROS2 message types to/from ROS1 streams 372 | * 373 | * This function is not meant to be used externally. However, since this the internal helper 374 | * functions call each other for sub messages they must be public. 375 | */ 376 | static void internal_stream_translate_helper( 377 | ros::serialization::OStream & stream, 378 | const ROS2_T & msg); 379 | static void internal_stream_translate_helper( 380 | ros::serialization::IStream & stream, 381 | ROS2_T & msg); 382 | static void internal_stream_translate_helper( 383 | ros::serialization::LStream & stream, 384 | const ROS2_T & msg); 385 | 386 | std::string ros1_type_name_; 387 | std::string ros2_type_name_; 388 | 389 | std::shared_ptr ts_lib_; 390 | const rosidl_message_type_support_t * type_support_ = nullptr; 391 | }; 392 | 393 | 394 | template 395 | class ServiceFactory : public ServiceFactoryInterface 396 | { 397 | public: 398 | using ROS1Request = typename ROS1_T::Request; 399 | using ROS2Request = typename ROS2_T::Request; 400 | using ROS1Response = typename ROS1_T::Response; 401 | using ROS2Response = typename ROS2_T::Response; 402 | 403 | void forward_2_to_1( 404 | ros::ServiceClient client, rclcpp::Logger logger, const std::shared_ptr, 405 | const std::shared_ptr request, std::shared_ptr response) 406 | { 407 | ROS1_T srv; 408 | translate_2_to_1(*request, srv.request); 409 | if (client.call(srv)) { 410 | translate_1_to_2(srv.response, *response); 411 | } else { 412 | throw std::runtime_error("Failed to get response from ROS 1 service " + client.getService()); 413 | } 414 | } 415 | 416 | bool forward_1_to_2( 417 | rclcpp::ClientBase::SharedPtr cli, rclcpp::Logger logger, 418 | const ROS1Request & request1, ROS1Response & response1, 419 | int service_execution_timeout) 420 | { 421 | auto client = std::dynamic_pointer_cast>(cli); 422 | if (!client) { 423 | RCLCPP_ERROR(logger, "Failed to get ROS 2 client %s", cli->get_service_name()); 424 | return false; 425 | } 426 | auto request2 = std::make_shared(); 427 | translate_1_to_2(request1, *request2); 428 | while (!client->wait_for_service(std::chrono::seconds(1))) { 429 | if (!rclcpp::ok()) { 430 | RCLCPP_ERROR( 431 | logger, "Interrupted while waiting for ROS 2 service %s", cli->get_service_name()); 432 | return false; 433 | } 434 | RCLCPP_WARN(logger, "Waiting for ROS 2 service %s...", cli->get_service_name()); 435 | } 436 | auto timeout = std::chrono::seconds(service_execution_timeout); 437 | auto future = client->async_send_request(request2); 438 | auto status = future.wait_for(timeout); 439 | if (status == std::future_status::ready) { 440 | auto response2 = future.get(); 441 | translate_2_to_1(*response2, response1); 442 | } else { 443 | RCLCPP_ERROR( 444 | logger, "Failed to get response from ROS 2 service %s within %d seconds", 445 | cli->get_service_name(), service_execution_timeout); 446 | return false; 447 | } 448 | return true; 449 | } 450 | 451 | ServiceBridge1to2 service_bridge_1_to_2( 452 | ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name, 453 | int service_execution_timeout) 454 | { 455 | ServiceBridge1to2 bridge; 456 | bridge.client = ros2_node->create_client(name); 457 | auto m = &ServiceFactory::forward_1_to_2; 458 | auto f = std::bind( 459 | m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, 460 | std::placeholders::_2, service_execution_timeout); 461 | bridge.server = ros1_node.advertiseService(name, f); 462 | return bridge; 463 | } 464 | 465 | ServiceBridge2to1 service_bridge_2_to_1( 466 | ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name) 467 | { 468 | ServiceBridge2to1 bridge; 469 | bridge.client = ros1_node.serviceClient(name); 470 | auto m = &ServiceFactory::forward_2_to_1; 471 | std::function< 472 | void( 473 | const std::shared_ptr, 474 | const std::shared_ptr, 475 | std::shared_ptr)> f; 476 | f = std::bind( 477 | m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, 478 | std::placeholders::_2, std::placeholders::_3); 479 | bridge.server = ros2_node->create_service(name, f); 480 | return bridge; 481 | } 482 | 483 | private: 484 | void translate_1_to_2(const ROS1Request &, ROS2Request &); 485 | void translate_1_to_2(const ROS1Response &, ROS2Response &); 486 | void translate_2_to_1(const ROS2Request &, ROS1Request &); 487 | void translate_2_to_1(const ROS2Response &, ROS1Response &); 488 | }; 489 | 490 | } // namespace ros1_bridge 491 | 492 | #endif // ROS1_BRIDGE__FACTORY_HPP_ 493 | -------------------------------------------------------------------------------- /include/ros1_bridge/factory_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ROS1_BRIDGE__FACTORY_INTERFACE_HPP_ 16 | #define ROS1_BRIDGE__FACTORY_INTERFACE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | // include ROS 1 22 | #include "ros/node_handle.h" 23 | #include "ros/publisher.h" 24 | #include "ros/subscriber.h" 25 | 26 | // include ROS 2 27 | #include "rclcpp/node.hpp" 28 | #include "rclcpp/publisher.hpp" 29 | #include "rclcpp/subscription.hpp" 30 | 31 | namespace ros1_bridge 32 | { 33 | 34 | struct ServiceBridge1to2 35 | { 36 | ros::ServiceServer server; 37 | rclcpp::ClientBase::SharedPtr client; 38 | }; 39 | 40 | struct ServiceBridge2to1 41 | { 42 | rclcpp::ServiceBase::SharedPtr server; 43 | ros::ServiceClient client; 44 | }; 45 | 46 | class FactoryInterface 47 | { 48 | public: 49 | virtual 50 | ros::Publisher 51 | create_ros1_publisher( 52 | ros::NodeHandle node, 53 | const std::string & topic_name, 54 | size_t queue_size, 55 | bool latch = false) = 0; 56 | 57 | virtual 58 | rclcpp::PublisherBase::SharedPtr 59 | create_ros2_publisher( 60 | rclcpp::Node::SharedPtr node, 61 | const std::string & topic_name, 62 | size_t queue_size) = 0; 63 | 64 | virtual 65 | rclcpp::PublisherBase::SharedPtr 66 | create_ros2_publisher( 67 | rclcpp::Node::SharedPtr node, 68 | const std::string & topic_name, 69 | const rmw_qos_profile_t & qos_profile) = 0; 70 | 71 | virtual 72 | rclcpp::PublisherBase::SharedPtr 73 | create_ros2_publisher( 74 | rclcpp::Node::SharedPtr node, 75 | const std::string & topic_name, 76 | const rclcpp::QoS & qos) = 0; 77 | 78 | virtual 79 | ros::Subscriber 80 | create_ros1_subscriber( 81 | ros::NodeHandle node, 82 | const std::string & topic_name, 83 | size_t queue_size, 84 | rclcpp::PublisherBase::SharedPtr ros2_pub, 85 | rclcpp::Logger logger) = 0; 86 | 87 | virtual 88 | rclcpp::SubscriptionBase::SharedPtr 89 | create_ros2_subscriber( 90 | rclcpp::Node::SharedPtr node, 91 | const std::string & topic_name, 92 | size_t queue_size, 93 | ros::Publisher ros1_pub, 94 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) = 0; 95 | 96 | virtual 97 | rclcpp::SubscriptionBase::SharedPtr 98 | create_ros2_subscriber( 99 | rclcpp::Node::SharedPtr node, 100 | const std::string & topic_name, 101 | const rmw_qos_profile_t & qos_profile, 102 | ros::Publisher ros1_pub, 103 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) = 0; 104 | 105 | virtual 106 | rclcpp::SubscriptionBase::SharedPtr 107 | create_ros2_subscriber( 108 | rclcpp::Node::SharedPtr node, 109 | const std::string & topic_name, 110 | const rclcpp::QoS & qos, 111 | ros::Publisher ros1_pub, 112 | rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) = 0; 113 | 114 | virtual 115 | void 116 | convert_1_to_2(const void * ros1_msg, void * ros2_msg) const = 0; 117 | 118 | virtual 119 | void 120 | convert_2_to_1(const void * ros2_msg, void * ros1_msg) const = 0; 121 | 122 | virtual 123 | bool convert_2_to_1_generic( 124 | const rclcpp::SerializedMessage & ros2_msg, 125 | std::vector & ros1_msg) const = 0; 126 | 127 | virtual 128 | bool convert_1_to_2_generic( 129 | const std::vector & ros1_msg, 130 | rclcpp::SerializedMessage & ros2_msg) const = 0; 131 | 132 | virtual const char * get_ros1_md5sum() const = 0; 133 | virtual const char * get_ros1_data_type() const = 0; 134 | virtual const char * get_ros1_message_definition() const = 0; 135 | }; 136 | 137 | class ServiceFactoryInterface 138 | { 139 | public: 140 | virtual ServiceBridge1to2 service_bridge_1_to_2( 141 | ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &, int) = 0; 142 | 143 | virtual ServiceBridge2to1 service_bridge_2_to_1( 144 | ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &) = 0; 145 | }; 146 | 147 | } // namespace ros1_bridge 148 | 149 | #endif // ROS1_BRIDGE__FACTORY_INTERFACE_HPP_ 150 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros1_bridge 5 | 0.10.3 6 | A simple bridge between ROS 1 and ROS 2 7 | 8 | Brandon Ong 9 | Dharini Dutia 10 | Geoffrey Biggs 11 | 12 | Apache License 2.0 13 | 14 | Dirk Thomas 15 | Jacob Perron 16 | Shane Loretz 17 | 18 | ament_cmake 19 | ament_index_python 20 | python3 21 | python3-catkin-pkg-modules 22 | rosidl_cmake 23 | rosidl_parser 24 | 25 | builtin_interfaces 26 | libboost-dev 27 | pkg-config 28 | python3-yaml 29 | rclcpp 30 | rcpputils 31 | rcutils 32 | rmw_implementation_cmake 33 | std_msgs 34 | xmlrpcpp 35 | 36 | pkg-config 37 | 38 | builtin_interfaces 39 | python3-yaml 40 | rclcpp 41 | rcpputils 42 | rcutils 43 | std_msgs 44 | 45 | ament_cmake_gtest 46 | ament_lint_auto 47 | ament_lint_common 48 | demo_nodes_cpp 49 | diagnostic_msgs 50 | geometry_msgs 51 | launch 52 | launch_testing 53 | launch_testing_ament_cmake 54 | launch_testing_ros 55 | ros2run 56 | sensor_msgs 57 | 58 | rosidl_interface_packages 59 | 60 | 61 | ament_cmake 62 | 63 | 64 | -------------------------------------------------------------------------------- /resource/get_factory.cpp.em: -------------------------------------------------------------------------------- 1 | // generated from ros1_bridge/resource/get_factory.cpp.em 2 | 3 | @############################################### 4 | @# 5 | @# Factory for creating publisher / subscribers 6 | @# based on message names 7 | @# 8 | @# EmPy template for generating get_factory.cpp 9 | @# 10 | @############################################### 11 | @# Start of Template 12 | @# 13 | @# Context: 14 | @# - ros2_package_names (list of str) 15 | @# ROS 2 package names 16 | @############################################### 17 | @ 18 | @{ 19 | from ros1_bridge import camel_case_to_lower_case_underscore 20 | }@ 21 | #include "ros1_bridge/factory.hpp" 22 | #include "ros1_bridge/builtin_interfaces_factories.hpp" 23 | 24 | @[for ros2_package_name in sorted(ros2_package_names)]@ 25 | #include "@(ros2_package_name)_factories.hpp" 26 | @[end for]@ 27 | 28 | namespace ros1_bridge 29 | { 30 | 31 | std::shared_ptr 32 | get_factory(const std::string & ros1_type_name, const std::string & ros2_type_name) 33 | { 34 | @[if not ros2_package_names]@ 35 | (void)ros1_type_name; 36 | (void)ros2_type_name; 37 | @[else]@ 38 | std::shared_ptr factory; 39 | @[end if]@ 40 | factory = get_factory_builtin_interfaces(ros1_type_name, ros2_type_name); 41 | if (factory) { 42 | return factory; 43 | } 44 | @[for ros2_package_name in sorted(ros2_package_names)]@ 45 | factory = get_factory_@(ros2_package_name)(ros1_type_name, ros2_type_name); 46 | if (factory) { 47 | return factory; 48 | } 49 | @[end for]@ 50 | throw std::runtime_error("No template specialization for the pair"); 51 | } 52 | 53 | std::unique_ptr get_service_factory(const std::string & ros_id, const std::string & package_name, const std::string & service_name) 54 | { 55 | @[if not ros2_package_names]@ 56 | (void)ros_id; 57 | (void)package_name; 58 | (void)service_name; 59 | @[else]@ 60 | std::unique_ptr factory; 61 | @[end if]@ 62 | @[for ros2_package_name in sorted(ros2_package_names)]@ 63 | factory = get_service_factory_@(ros2_package_name)(ros_id, package_name, service_name); 64 | if (factory) { 65 | return factory; 66 | } 67 | @[end for]@ 68 | // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data()); 69 | return factory; 70 | } 71 | 72 | } // namespace ros1_bridge 73 | -------------------------------------------------------------------------------- /resource/get_mappings.cpp.em: -------------------------------------------------------------------------------- 1 | // generated from ros1_bridge/resource/get_mappings.cpp.em 2 | 3 | @############################################### 4 | @# 5 | @# Methods for determing mappings between 6 | @# ROS 1 and ROS 2 interfaces 7 | @# 8 | @# EmPy template for generating get_mappings.cpp 9 | @# 10 | @############################################### 11 | @# Start of Template 12 | @# 13 | @# Context: 14 | @# - mappings (list of ros1_bridge.Mapping) 15 | @# Mapping between messages as well as their fields 16 | @# - services (list of dictionaries) 17 | @# Mapping between services as well as their fields 18 | @############################################### 19 | @ 20 | #include 21 | #include 22 | 23 | namespace ros1_bridge 24 | { 25 | 26 | bool 27 | get_1to2_mapping(const std::string & ros1_type_name, std::string & ros2_type_name) 28 | { 29 | @[if not mappings]@ 30 | (void)ros1_type_name; 31 | (void)ros2_type_name; 32 | @[end if]@ 33 | 34 | @[for m in mappings]@ 35 | if (ros1_type_name == "@(m.ros1_msg.package_name)/@(m.ros1_msg.message_name)") 36 | { 37 | ros2_type_name = "@(m.ros2_msg.package_name)/msg/@(m.ros2_msg.message_name)"; 38 | return true; 39 | } 40 | @[end for]@ 41 | 42 | return false; 43 | } 44 | 45 | bool 46 | get_2to1_mapping(const std::string & ros2_type_name, std::string & ros1_type_name) 47 | { 48 | @[if not mappings]@ 49 | (void)ros1_type_name; 50 | (void)ros2_type_name; 51 | @[end if]@ 52 | 53 | @[for m in mappings]@ 54 | if (ros2_type_name == "@(m.ros2_msg.package_name)/msg/@(m.ros2_msg.message_name)") 55 | { 56 | ros1_type_name = "@(m.ros1_msg.package_name)/@(m.ros1_msg.message_name)"; 57 | return true; 58 | } 59 | @[end for]@ 60 | 61 | return false; 62 | } 63 | 64 | std::multimap 65 | get_all_message_mappings_2to1() 66 | { 67 | static std::multimap mappings = { 68 | @[for m in mappings]@ 69 | { 70 | "@(m.ros2_msg.package_name)/msg/@(m.ros2_msg.message_name)", // ROS 2 71 | "@(m.ros1_msg.package_name)/@(m.ros1_msg.message_name)" // ROS 1 72 | }, 73 | @[end for]@ 74 | }; 75 | return mappings; 76 | } 77 | 78 | std::multimap 79 | get_all_service_mappings_2to1() 80 | { 81 | static std::multimap mappings = { 82 | @[for s in services]@ 83 | { 84 | "@(s['ros2_package'])/srv/@(s['ros2_name'])", // ROS 2 85 | "@(s['ros1_package'])/@(s['ros1_name'])" // ROS 1 86 | }, 87 | @[end for]@ 88 | }; 89 | return mappings; 90 | } 91 | 92 | } // namespace ros1_bridge 93 | -------------------------------------------------------------------------------- /resource/pkg_factories.cpp.em: -------------------------------------------------------------------------------- 1 | // generated from ros1_bridge/resource/pkg_factories.cpp.em 2 | 3 | @############################################### 4 | @# 5 | @# Factory template specializations based on 6 | @# message types of a single ROS 2 package 7 | @# 8 | @# EmPy template for generating _factories.cpp 9 | @# 10 | @############################################### 11 | @# Start of Template 12 | @# 13 | @# Context: 14 | @# - ros2_package_name (str) 15 | @# The ROS 2 package name of this file 16 | @# - mappings (list of ros1_bridge.Mapping) 17 | @# Mapping between messages as well as their fields 18 | @############################################### 19 | @ 20 | #include "@(ros2_package_name)_factories.hpp" 21 | 22 | namespace ros1_bridge 23 | { 24 | 25 | std::shared_ptr 26 | get_factory_@(ros2_package_name)(const std::string & ros1_type_name, const std::string & ros2_type_name) 27 | { 28 | @[if not ros2_msg_types]@ 29 | (void)ros1_type_name; 30 | (void)ros2_type_name; 31 | @[else]@ 32 | std::shared_ptr factory; 33 | @[end if]@ 34 | @[for m in ros2_msg_types]@ 35 | factory = get_factory_@(ros2_package_name)__msg__@(m.message_name)(ros1_type_name, ros2_type_name); 36 | if (factory) { 37 | return factory; 38 | } 39 | @[end for]@ 40 | return std::shared_ptr(); 41 | } 42 | 43 | std::unique_ptr 44 | get_service_factory_@(ros2_package_name)(const std::string & ros_id, const std::string & package_name, const std::string & service_name) 45 | { 46 | @[if not ros2_srv_types]@ 47 | (void)ros_id; 48 | (void)package_name; 49 | (void)service_name; 50 | @[else]@ 51 | std::unique_ptr factory; 52 | @[end if]@ 53 | @[for s in ros2_srv_types]@ 54 | factory = get_service_factory_@(ros2_package_name)__srv__@(s.message_name)(ros_id, package_name, service_name); 55 | if (factory) { 56 | return factory; 57 | } 58 | @[end for]@ 59 | return nullptr; 60 | } 61 | } // namespace ros1_bridge 62 | -------------------------------------------------------------------------------- /resource/pkg_factories.hpp.em: -------------------------------------------------------------------------------- 1 | // generated from ros1_bridge/resource/pkg_factories.hpp.em 2 | 3 | @############################################### 4 | @# 5 | @# Factory template specializations based on 6 | @# message types of a single ROS 2 package 7 | @# 8 | @# EmPy template for generating _factories.hpp 9 | @# 10 | @############################################### 11 | @# Start of Template 12 | @# 13 | @# Context: 14 | @# - ros2_package_name (str) 15 | @# The ROS 2 package name of this file 16 | @# - mappings (list of ros1_bridge.Mapping) 17 | @# Mapping between messages as well as their fields 18 | @# - ros1_msgs (list of ros1_bridge.Message) 19 | @# ROS 1 messages 20 | @# - ros2_msgs (list of ros1_bridge.Message) 21 | @# ROS 2 messages 22 | @############################################### 23 | @ 24 | @{ 25 | from ros1_bridge import camel_case_to_lower_case_underscore 26 | }@ 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | // include ROS 1 messages 33 | @[for ros1_msg in mapped_ros1_msgs]@ 34 | #include <@(ros1_msg.package_name)/@(ros1_msg.message_name).h> 35 | @[end for]@ 36 | 37 | // include ROS 2 messages 38 | @[for ros2_msg in mapped_ros2_msgs]@ 39 | #include <@(ros2_msg.package_name)/msg/@(camel_case_to_lower_case_underscore(ros2_msg.message_name)).hpp> 40 | @[end for]@ 41 | 42 | namespace ros1_bridge 43 | { 44 | 45 | std::shared_ptr 46 | get_factory_@(ros2_package_name)(const std::string & ros1_type_name, const std::string & ros2_type_name); 47 | @[for m in ros2_msg_types]@ 48 | 49 | std::shared_ptr 50 | get_factory_@(ros2_package_name)__msg__@(m.message_name)(const std::string & ros1_type_name, const std::string & ros2_type_name); 51 | @[end for]@ 52 | 53 | std::unique_ptr 54 | get_service_factory_@(ros2_package_name)(const std::string & ros_id, const std::string & package_name, const std::string & service_name); 55 | @[for s in ros2_srv_types]@ 56 | 57 | std::unique_ptr 58 | get_service_factory_@(ros2_package_name)__srv__@(s.message_name)(const std::string & ros_id, const std::string & package_name, const std::string & service_name); 59 | @[end for]@ 60 | 61 | // conversion functions for available interfaces 62 | @[for m in mappings]@ 63 | 64 | template<> 65 | void 66 | Factory< 67 | @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name), 68 | @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) 69 | >::convert_1_to_2( 70 | const @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name) & ros1_msg, 71 | @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) & ros2_msg); 72 | 73 | template<> 74 | void 75 | Factory< 76 | @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name), 77 | @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) 78 | >::convert_2_to_1( 79 | const @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) & ros2_msg, 80 | @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name) & ros1_msg); 81 | 82 | @[end for]@ 83 | } // namespace ros1_bridge 84 | -------------------------------------------------------------------------------- /src/bridge.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "ros1_bridge/bridge.hpp" 18 | 19 | 20 | namespace ros1_bridge 21 | { 22 | 23 | Bridge1to2Handles 24 | create_bridge_from_1_to_2( 25 | ros::NodeHandle ros1_node, 26 | rclcpp::Node::SharedPtr ros2_node, 27 | const std::string & ros1_type_name, 28 | const std::string & ros1_topic_name, 29 | size_t subscriber_queue_size, 30 | const std::string & ros2_type_name, 31 | const std::string & ros2_topic_name, 32 | size_t publisher_queue_size) 33 | { 34 | return create_bridge_from_1_to_2( 35 | ros1_node, 36 | ros2_node, 37 | ros1_type_name, 38 | ros1_topic_name, 39 | subscriber_queue_size, 40 | ros2_type_name, 41 | ros2_topic_name, 42 | rclcpp::QoS(rclcpp::KeepLast(publisher_queue_size))); 43 | } 44 | 45 | Bridge1to2Handles 46 | create_bridge_from_1_to_2( 47 | ros::NodeHandle ros1_node, 48 | rclcpp::Node::SharedPtr ros2_node, 49 | const std::string & ros1_type_name, 50 | const std::string & ros1_topic_name, 51 | size_t subscriber_queue_size, 52 | const std::string & ros2_type_name, 53 | const std::string & ros2_topic_name, 54 | const rclcpp::QoS & publisher_qos) 55 | { 56 | auto factory = get_factory(ros1_type_name, ros2_type_name); 57 | auto ros2_pub = factory->create_ros2_publisher( 58 | ros2_node, ros2_topic_name, publisher_qos); 59 | 60 | auto ros1_sub = factory->create_ros1_subscriber( 61 | ros1_node, ros1_topic_name, subscriber_queue_size, ros2_pub, ros2_node->get_logger()); 62 | 63 | Bridge1to2Handles handles; 64 | handles.ros1_subscriber = ros1_sub; 65 | handles.ros2_publisher = ros2_pub; 66 | return handles; 67 | } 68 | 69 | Bridge2to1Handles 70 | create_bridge_from_2_to_1( 71 | rclcpp::Node::SharedPtr ros2_node, 72 | ros::NodeHandle ros1_node, 73 | const std::string & ros2_type_name, 74 | const std::string & ros2_topic_name, 75 | size_t subscriber_queue_size, 76 | const std::string & ros1_type_name, 77 | const std::string & ros1_topic_name, 78 | size_t publisher_queue_size, 79 | rclcpp::PublisherBase::SharedPtr ros2_pub) 80 | { 81 | auto subscriber_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(subscriber_queue_size)); 82 | return create_bridge_from_2_to_1( 83 | ros2_node, 84 | ros1_node, 85 | ros2_type_name, 86 | ros2_topic_name, 87 | subscriber_qos, 88 | ros1_type_name, 89 | ros1_topic_name, 90 | publisher_queue_size, 91 | ros2_pub); 92 | } 93 | 94 | Bridge2to1Handles 95 | create_bridge_from_2_to_1( 96 | rclcpp::Node::SharedPtr ros2_node, 97 | ros::NodeHandle ros1_node, 98 | const std::string & ros2_type_name, 99 | const std::string & ros2_topic_name, 100 | const rclcpp::QoS & subscriber_qos, 101 | const std::string & ros1_type_name, 102 | const std::string & ros1_topic_name, 103 | size_t publisher_queue_size, 104 | rclcpp::PublisherBase::SharedPtr ros2_pub) 105 | { 106 | auto factory = get_factory(ros1_type_name, ros2_type_name); 107 | auto ros1_pub = factory->create_ros1_publisher( 108 | ros1_node, ros1_topic_name, publisher_queue_size); 109 | 110 | auto ros2_sub = factory->create_ros2_subscriber( 111 | ros2_node, ros2_topic_name, subscriber_qos, ros1_pub, ros2_pub); 112 | 113 | Bridge2to1Handles handles; 114 | handles.ros2_subscriber = ros2_sub; 115 | handles.ros1_publisher = ros1_pub; 116 | return handles; 117 | } 118 | 119 | BridgeHandles 120 | create_bidirectional_bridge( 121 | ros::NodeHandle ros1_node, 122 | rclcpp::Node::SharedPtr ros2_node, 123 | const std::string & ros1_type_name, 124 | const std::string & ros2_type_name, 125 | const std::string & topic_name, 126 | size_t queue_size) 127 | { 128 | RCLCPP_INFO( 129 | ros2_node->get_logger(), "create bidirectional bridge for topic %s", 130 | topic_name.c_str()); 131 | BridgeHandles handles; 132 | handles.bridge1to2 = create_bridge_from_1_to_2( 133 | ros1_node, ros2_node, 134 | ros1_type_name, topic_name, queue_size, ros2_type_name, topic_name, queue_size); 135 | handles.bridge2to1 = create_bridge_from_2_to_1( 136 | ros2_node, ros1_node, 137 | ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size, 138 | handles.bridge1to2.ros2_publisher); 139 | return handles; 140 | } 141 | 142 | BridgeHandles 143 | create_bidirectional_bridge( 144 | ros::NodeHandle ros1_node, 145 | rclcpp::Node::SharedPtr ros2_node, 146 | const std::string & ros1_type_name, 147 | const std::string & ros2_type_name, 148 | const std::string & topic_name, 149 | size_t queue_size, 150 | const rclcpp::QoS & publisher_qos) 151 | { 152 | RCLCPP_INFO( 153 | ros2_node->get_logger(), "create bidirectional bridge for topic %s", 154 | topic_name.c_str()); 155 | BridgeHandles handles; 156 | handles.bridge1to2 = create_bridge_from_1_to_2( 157 | ros1_node, ros2_node, 158 | ros1_type_name, topic_name, queue_size, ros2_type_name, topic_name, publisher_qos); 159 | handles.bridge2to1 = create_bridge_from_2_to_1( 160 | ros2_node, ros1_node, 161 | ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size, 162 | handles.bridge1to2.ros2_publisher); 163 | return handles; 164 | } 165 | 166 | } // namespace ros1_bridge 167 | -------------------------------------------------------------------------------- /src/builtin_interfaces_factories.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | // include builtin interfaces 21 | #include "ros1_bridge/builtin_interfaces_factories.hpp" 22 | #include "ros1_bridge/convert_builtin_interfaces.hpp" 23 | 24 | namespace ros1_bridge 25 | { 26 | 27 | std::shared_ptr 28 | get_factory_builtin_interfaces( 29 | const std::string & ros1_type_name, 30 | const std::string & ros2_type_name) 31 | { 32 | // mapping from string to specialized template 33 | if ( 34 | (ros1_type_name == "std_msgs/Duration" || ros1_type_name == "") && 35 | ros2_type_name == "builtin_interfaces/msg/Duration") 36 | { 37 | return std::make_shared< 38 | Factory< 39 | std_msgs::Duration, 40 | builtin_interfaces::msg::Duration 41 | > 42 | >("std_msgs/Duration", ros2_type_name); 43 | } 44 | if ( 45 | (ros1_type_name == "std_msgs/Time" || ros1_type_name == "") && 46 | ros2_type_name == "builtin_interfaces/msg/Time") 47 | { 48 | return std::make_shared< 49 | Factory< 50 | std_msgs::Time, 51 | builtin_interfaces::msg::Time 52 | > 53 | >("std_msgs/Time", ros2_type_name); 54 | } 55 | return std::shared_ptr(); 56 | } 57 | 58 | // conversion functions for available interfaces 59 | template<> 60 | void 61 | Factory< 62 | std_msgs::Duration, 63 | builtin_interfaces::msg::Duration 64 | >::convert_1_to_2( 65 | const std_msgs::Duration & ros1_msg, 66 | builtin_interfaces::msg::Duration & ros2_msg) 67 | { 68 | ros1_bridge::convert_1_to_2(ros1_msg.data, ros2_msg); 69 | } 70 | 71 | template<> 72 | void 73 | Factory< 74 | std_msgs::Duration, 75 | builtin_interfaces::msg::Duration 76 | >::convert_2_to_1( 77 | const builtin_interfaces::msg::Duration & ros2_msg, 78 | std_msgs::Duration & ros1_msg) 79 | { 80 | ros1_bridge::convert_2_to_1(ros2_msg, ros1_msg.data); 81 | } 82 | 83 | template<> 84 | void 85 | Factory< 86 | std_msgs::Duration, 87 | builtin_interfaces::msg::Duration 88 | >::internal_stream_translate_helper( 89 | ros::serialization::OStream & stream, 90 | const builtin_interfaces::msg::Duration & ros2_msg) 91 | { 92 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); 93 | } 94 | 95 | template<> 96 | void 97 | Factory< 98 | std_msgs::Duration, 99 | builtin_interfaces::msg::Duration 100 | >::internal_stream_translate_helper( 101 | ros::serialization::IStream & stream, 102 | builtin_interfaces::msg::Duration & ros2_msg) 103 | { 104 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); 105 | } 106 | 107 | template<> 108 | void 109 | Factory< 110 | std_msgs::Duration, 111 | builtin_interfaces::msg::Duration 112 | >::internal_stream_translate_helper( 113 | ros::serialization::LStream & stream, 114 | const builtin_interfaces::msg::Duration & ros2_msg) 115 | { 116 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); 117 | } 118 | 119 | template<> 120 | void 121 | Factory< 122 | std_msgs::Duration, 123 | builtin_interfaces::msg::Duration 124 | >::convert_2_to_1( 125 | const builtin_interfaces::msg::Duration & ros2_msg, 126 | ros::serialization::OStream & out_stream) 127 | { 128 | internal_stream_translate_helper(out_stream, ros2_msg); 129 | } 130 | 131 | template<> 132 | void 133 | Factory< 134 | std_msgs::Duration, 135 | builtin_interfaces::msg::Duration 136 | >::convert_1_to_2( 137 | ros::serialization::IStream & in_stream, 138 | builtin_interfaces::msg::Duration & ros2_msg) 139 | { 140 | internal_stream_translate_helper(in_stream, ros2_msg); 141 | } 142 | 143 | template<> 144 | uint32_t 145 | Factory< 146 | std_msgs::Duration, 147 | builtin_interfaces::msg::Duration 148 | >::length_2_as_1_stream(const builtin_interfaces::msg::Duration & ros2_msg) 149 | { 150 | ros::serialization::LStream len_stream; 151 | internal_stream_translate_helper(len_stream, ros2_msg); 152 | return len_stream.getLength(); 153 | } 154 | 155 | template<> 156 | void 157 | Factory< 158 | std_msgs::Time, 159 | builtin_interfaces::msg::Time 160 | >::convert_1_to_2( 161 | const std_msgs::Time & ros1_msg, 162 | builtin_interfaces::msg::Time & ros2_msg) 163 | { 164 | ros1_bridge::convert_1_to_2(ros1_msg.data, ros2_msg); 165 | } 166 | 167 | template<> 168 | void 169 | Factory< 170 | std_msgs::Time, 171 | builtin_interfaces::msg::Time 172 | >::convert_2_to_1( 173 | const builtin_interfaces::msg::Time & ros2_msg, 174 | std_msgs::Time & ros1_msg) 175 | { 176 | ros1_bridge::convert_2_to_1(ros2_msg, ros1_msg.data); 177 | } 178 | 179 | template<> 180 | void 181 | Factory< 182 | std_msgs::Time, 183 | builtin_interfaces::msg::Time 184 | >::internal_stream_translate_helper( 185 | ros::serialization::OStream & stream, 186 | const builtin_interfaces::msg::Time & ros2_msg) 187 | { 188 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); 189 | } 190 | 191 | template<> 192 | void 193 | Factory< 194 | std_msgs::Time, 195 | builtin_interfaces::msg::Time 196 | >::internal_stream_translate_helper( 197 | ros::serialization::IStream & stream, 198 | builtin_interfaces::msg::Time & ros2_msg) 199 | { 200 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); 201 | } 202 | 203 | template<> 204 | void 205 | Factory< 206 | std_msgs::Time, 207 | builtin_interfaces::msg::Time 208 | >::internal_stream_translate_helper( 209 | ros::serialization::LStream & stream, 210 | const builtin_interfaces::msg::Time & ros2_msg) 211 | { 212 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); 213 | } 214 | 215 | template<> 216 | void 217 | Factory< 218 | std_msgs::Time, 219 | builtin_interfaces::msg::Time 220 | >::convert_2_to_1( 221 | const builtin_interfaces::msg::Time & ros2_msg, 222 | ros::serialization::OStream & out_stream) 223 | { 224 | internal_stream_translate_helper(out_stream, ros2_msg); 225 | } 226 | 227 | template<> 228 | void 229 | Factory< 230 | std_msgs::Time, 231 | builtin_interfaces::msg::Time 232 | >::convert_1_to_2( 233 | ros::serialization::IStream & in_stream, 234 | builtin_interfaces::msg::Time & ros2_msg) 235 | { 236 | internal_stream_translate_helper(in_stream, ros2_msg); 237 | } 238 | 239 | template<> 240 | uint32_t 241 | Factory< 242 | std_msgs::Time, 243 | builtin_interfaces::msg::Time 244 | >::length_2_as_1_stream(const builtin_interfaces::msg::Time & ros2_msg) 245 | { 246 | ros::serialization::LStream len_stream; 247 | internal_stream_translate_helper(len_stream, ros2_msg); 248 | return len_stream.getLength(); 249 | } 250 | 251 | template<> 252 | void 253 | Factory< 254 | std_msgs::Header, 255 | std_msgs::msg::Header 256 | >::internal_stream_translate_helper( 257 | ros::serialization::OStream & stream, 258 | const std_msgs::msg::Header & ros2_msg) 259 | { 260 | // ROS2 Header is missing seq, provide a fake one so stream matches 261 | uint32_t seq = 0; 262 | stream.next(seq); 263 | 264 | // write non-array field 265 | // write builtin field 266 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg.stamp); 267 | 268 | // write non-array field 269 | // write primitive field 270 | stream.next(ros2_msg.frame_id); 271 | } 272 | 273 | template<> 274 | void 275 | Factory< 276 | std_msgs::Header, 277 | std_msgs::msg::Header 278 | >::internal_stream_translate_helper( 279 | ros::serialization::IStream & stream, 280 | std_msgs::msg::Header & ros2_msg) 281 | { 282 | // ROS2 Header is missing seq, provide a fake one so stream matches 283 | uint32_t seq = 0; 284 | stream.next(seq); 285 | 286 | // write non-array field 287 | // write builtin field 288 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg.stamp); 289 | 290 | // write non-array field 291 | // write primitive field 292 | stream.next(ros2_msg.frame_id); 293 | } 294 | 295 | template<> 296 | void 297 | Factory< 298 | std_msgs::Header, 299 | std_msgs::msg::Header 300 | >::internal_stream_translate_helper( 301 | ros::serialization::LStream & stream, 302 | const std_msgs::msg::Header & ros2_msg) 303 | { 304 | // ROS2 Header is missing seq, provide a fake one so stream matches 305 | uint32_t seq = 0; 306 | stream.next(seq); 307 | 308 | // write non-array field 309 | // write builtin field 310 | ros1_bridge::internal_stream_translate_helper(stream, ros2_msg.stamp); 311 | 312 | // write non-array field 313 | // write primitive field 314 | stream.next(ros2_msg.frame_id); 315 | } 316 | 317 | template<> 318 | void 319 | Factory< 320 | std_msgs::Header, 321 | std_msgs::msg::Header 322 | >::convert_2_to_1( 323 | const std_msgs::msg::Header & ros2_msg, 324 | ros::serialization::OStream & out_stream) 325 | { 326 | internal_stream_translate_helper(out_stream, ros2_msg); 327 | } 328 | 329 | 330 | template<> 331 | void 332 | Factory< 333 | std_msgs::Header, 334 | std_msgs::msg::Header 335 | >::convert_1_to_2( 336 | ros::serialization::IStream & in_stream, 337 | std_msgs::msg::Header & ros2_msg) 338 | { 339 | internal_stream_translate_helper(in_stream, ros2_msg); 340 | } 341 | 342 | template<> 343 | uint32_t 344 | Factory< 345 | std_msgs::Header, 346 | std_msgs::msg::Header 347 | >::length_2_as_1_stream(const std_msgs::msg::Header & ros2_msg) 348 | { 349 | ros::serialization::LStream len_stream; 350 | internal_stream_translate_helper(len_stream, ros2_msg); 351 | return len_stream.getLength(); 352 | } 353 | 354 | } // namespace ros1_bridge 355 | -------------------------------------------------------------------------------- /src/convert_builtin_interfaces.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "ros1_bridge/convert_builtin_interfaces.hpp" 16 | 17 | #include "ros/serialization.h" 18 | 19 | namespace ros1_bridge 20 | { 21 | 22 | template<> 23 | void 24 | convert_1_to_2( 25 | const ros::Duration & ros1_type, 26 | builtin_interfaces::msg::Duration & ros2_msg) 27 | { 28 | ros2_msg.sec = ros1_type.sec; 29 | ros2_msg.nanosec = ros1_type.nsec; 30 | } 31 | 32 | template<> 33 | void 34 | convert_2_to_1( 35 | const builtin_interfaces::msg::Duration & ros2_msg, 36 | ros::Duration & ros1_type) 37 | { 38 | ros1_type.sec = ros2_msg.sec; 39 | ros1_type.nsec = ros2_msg.nanosec; 40 | } 41 | 42 | template<> 43 | void 44 | internal_stream_translate_helper( 45 | ros::serialization::OStream & stream, 46 | const builtin_interfaces::msg::Duration & ros2_msg) 47 | { 48 | stream.next(ros2_msg.sec); 49 | stream.next(ros2_msg.nanosec); 50 | } 51 | 52 | template<> 53 | void 54 | internal_stream_translate_helper( 55 | ros::serialization::IStream & stream, 56 | builtin_interfaces::msg::Duration & ros2_msg) 57 | { 58 | stream.next(ros2_msg.sec); 59 | stream.next(ros2_msg.nanosec); 60 | } 61 | 62 | template<> 63 | void 64 | internal_stream_translate_helper( 65 | ros::serialization::LStream & stream, 66 | const builtin_interfaces::msg::Duration & ros2_msg) 67 | { 68 | stream.next(ros2_msg.sec); 69 | stream.next(ros2_msg.nanosec); 70 | } 71 | 72 | template<> 73 | void 74 | convert_1_to_2( 75 | const ros::Time & ros1_type, 76 | builtin_interfaces::msg::Time & ros2_msg) 77 | { 78 | ros2_msg.sec = ros1_type.sec; 79 | ros2_msg.nanosec = ros1_type.nsec; 80 | } 81 | 82 | template<> 83 | void 84 | convert_2_to_1( 85 | const builtin_interfaces::msg::Time & ros2_msg, 86 | ros::Time & ros1_type) 87 | { 88 | ros1_type.sec = ros2_msg.sec; 89 | ros1_type.nsec = ros2_msg.nanosec; 90 | } 91 | 92 | template<> 93 | void 94 | internal_stream_translate_helper( 95 | ros::serialization::OStream & stream, 96 | const builtin_interfaces::msg::Time & ros2_msg) 97 | { 98 | stream.next(ros2_msg.sec); 99 | stream.next(ros2_msg.nanosec); 100 | } 101 | 102 | template<> 103 | void 104 | internal_stream_translate_helper( 105 | ros::serialization::IStream & stream, 106 | builtin_interfaces::msg::Time & ros2_msg) 107 | { 108 | stream.next(ros2_msg.sec); 109 | stream.next(ros2_msg.nanosec); 110 | } 111 | 112 | template<> 113 | void 114 | internal_stream_translate_helper( 115 | ros::serialization::LStream & stream, 116 | const builtin_interfaces::msg::Time & ros2_msg) 117 | { 118 | stream.next(ros2_msg.sec); 119 | stream.next(ros2_msg.nanosec); 120 | } 121 | 122 | } // namespace ros1_bridge 123 | -------------------------------------------------------------------------------- /src/parameter_bridge.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | // include ROS 1 21 | #ifdef __clang__ 22 | # pragma clang diagnostic push 23 | # pragma clang diagnostic ignored "-Wunused-parameter" 24 | #endif 25 | #include "ros/ros.h" 26 | #ifdef __clang__ 27 | # pragma clang diagnostic pop 28 | #endif 29 | 30 | // include ROS 2 31 | #include "rclcpp/rclcpp.hpp" 32 | 33 | #include "ros1_bridge/bridge.hpp" 34 | 35 | rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) 36 | { 37 | auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); 38 | 39 | printf("Qos("); 40 | 41 | if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) { 42 | if (qos_params.hasMember("history")) { 43 | auto history = static_cast(qos_params["history"]); 44 | printf("history: "); 45 | if (history == "keep_all") { 46 | ros2_publisher_qos.keep_all(); 47 | printf("keep_all, "); 48 | } else if (history == "keep_last") { 49 | if (qos_params.hasMember("depth")) { 50 | auto depth = static_cast(qos_params["depth"]); 51 | ros2_publisher_qos.keep_last(depth); 52 | printf("keep_last(%i), ", depth); 53 | } else { 54 | fprintf( 55 | stderr, 56 | "history: keep_last requires that also a depth is set\n"); 57 | } 58 | } else { 59 | fprintf( 60 | stderr, 61 | "invalid value for 'history': '%s', allowed values are 'keep_all'," 62 | "'keep_last' (also requires 'depth' to be set)\n", 63 | history.c_str()); 64 | } 65 | } 66 | 67 | if (qos_params.hasMember("reliability")) { 68 | auto reliability = static_cast(qos_params["reliability"]); 69 | printf("reliability: "); 70 | if (reliability == "best_effort") { 71 | ros2_publisher_qos.best_effort(); 72 | printf("best_effort, "); 73 | } else if (reliability == "reliable") { 74 | ros2_publisher_qos.reliable(); 75 | printf("reliable, "); 76 | } else { 77 | fprintf( 78 | stderr, 79 | "invalid value for 'reliability': '%s', allowed values are 'best_effort', 'reliable'\n", 80 | reliability.c_str()); 81 | } 82 | } 83 | 84 | if (qos_params.hasMember("durability")) { 85 | auto durability = static_cast(qos_params["durability"]); 86 | printf("durability: "); 87 | if (durability == "transient_local") { 88 | ros2_publisher_qos.transient_local(); 89 | printf("transient_local, "); 90 | } else if (durability == "volatile") { 91 | ros2_publisher_qos.durability_volatile(); 92 | printf("volatile, "); 93 | } else { 94 | fprintf( 95 | stderr, 96 | "invalid value for 'durability': '%s', allowed values are 'best_effort', 'volatile'\n", 97 | durability.c_str()); 98 | } 99 | } 100 | 101 | if (qos_params.hasMember("deadline")) { 102 | try { 103 | rclcpp::Duration dur = rclcpp::Duration( 104 | static_cast(qos_params["deadline"]["secs"]), 105 | static_cast(qos_params["deadline"]["nsecs"])); 106 | ros2_publisher_qos.deadline(dur); 107 | printf("deadline: Duration(nsecs: %ld), ", dur.nanoseconds()); 108 | } catch (std::runtime_error & e) { 109 | fprintf( 110 | stderr, 111 | "failed to parse deadline: '%s'\n", 112 | e.what()); 113 | } catch (XmlRpc::XmlRpcException & e) { 114 | fprintf( 115 | stderr, 116 | "failed to parse deadline: '%s'\n", 117 | e.getMessage().c_str()); 118 | } 119 | } 120 | 121 | if (qos_params.hasMember("lifespan")) { 122 | try { 123 | rclcpp::Duration dur = rclcpp::Duration( 124 | static_cast(qos_params["lifespan"]["secs"]), 125 | static_cast(qos_params["lifespan"]["nsecs"])); 126 | ros2_publisher_qos.lifespan(dur); 127 | printf("lifespan: Duration(nsecs: %ld), ", dur.nanoseconds()); 128 | } catch (std::runtime_error & e) { 129 | fprintf( 130 | stderr, 131 | "failed to parse lifespan: '%s'\n", 132 | e.what()); 133 | } catch (XmlRpc::XmlRpcException & e) { 134 | fprintf( 135 | stderr, 136 | "failed to parse lifespan: '%s'\n", 137 | e.getMessage().c_str()); 138 | } 139 | } 140 | 141 | if (qos_params.hasMember("liveliness")) { 142 | if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt) { 143 | try { 144 | auto liveliness = static_cast(qos_params["liveliness"]); 145 | ros2_publisher_qos.liveliness(static_cast(liveliness)); 146 | printf("liveliness: %i, ", static_cast(liveliness)); 147 | } catch (std::runtime_error & e) { 148 | fprintf( 149 | stderr, 150 | "failed to parse liveliness: '%s'\n", 151 | e.what()); 152 | } catch (XmlRpc::XmlRpcException & e) { 153 | fprintf( 154 | stderr, 155 | "failed to parse liveliness: '%s'\n", 156 | e.getMessage().c_str()); 157 | } 158 | } else if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeString) { 159 | try { 160 | rmw_qos_liveliness_policy_t liveliness = 161 | rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; 162 | auto liveliness_str = static_cast(qos_params["liveliness"]); 163 | if (liveliness_str == "LIVELINESS_SYSTEM_DEFAULT" || 164 | liveliness_str == "liveliness_system_default") 165 | { 166 | liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; 167 | } else if (liveliness_str == "LIVELINESS_AUTOMATIC" || // NOLINT 168 | liveliness_str == "liveliness_automatic") 169 | { 170 | liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; 171 | } else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" || // NOLINT 172 | liveliness_str == "liveliness_manual_by_topic") 173 | { 174 | liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; 175 | } else { 176 | fprintf( 177 | stderr, 178 | "invalid value for 'liveliness': '%s', allowed values are " 179 | "LIVELINESS_{SYSTEM_DEFAULT, AUTOMATIC, MANUAL_BY_TOPIC}, upper or lower case\n", 180 | liveliness_str.c_str()); 181 | } 182 | 183 | ros2_publisher_qos.liveliness(liveliness); 184 | printf("liveliness: %s, ", liveliness_str.c_str()); 185 | } catch (std::runtime_error & e) { 186 | fprintf( 187 | stderr, 188 | "failed to parse liveliness: '%s'\n", 189 | e.what()); 190 | } catch (XmlRpc::XmlRpcException & e) { 191 | fprintf( 192 | stderr, 193 | "failed to parse liveliness: '%s'\n", 194 | e.getMessage().c_str()); 195 | } 196 | } else { 197 | fprintf( 198 | stderr, 199 | "failed to parse liveliness, parameter was not a string or int \n"); 200 | } 201 | } 202 | 203 | if (qos_params.hasMember("liveliness_lease_duration")) { 204 | try { 205 | rclcpp::Duration dur = rclcpp::Duration( 206 | static_cast(qos_params["liveliness_lease_duration"]["secs"]), 207 | static_cast(qos_params["liveliness_lease_duration"]["nsecs"])); 208 | ros2_publisher_qos.liveliness_lease_duration(dur); 209 | printf("liveliness_lease_duration: Duration(nsecs: %ld), ", dur.nanoseconds()); 210 | } catch (std::runtime_error & e) { 211 | fprintf( 212 | stderr, 213 | "failed to parse liveliness_lease_duration: '%s'\n", 214 | e.what()); 215 | } catch (XmlRpc::XmlRpcException & e) { 216 | fprintf( 217 | stderr, 218 | "failed to parse liveliness_lease_duration: '%s'\n", 219 | e.getMessage().c_str()); 220 | } 221 | } 222 | } else { 223 | fprintf( 224 | stderr, 225 | "QoS parameters could not be read\n"); 226 | } 227 | 228 | printf(")"); 229 | return ros2_publisher_qos; 230 | } 231 | 232 | int main(int argc, char * argv[]) 233 | { 234 | // ROS 1 node 235 | ros::init(argc, argv, "ros_bridge"); 236 | ros::NodeHandle ros1_node; 237 | 238 | // ROS 2 node 239 | rclcpp::init(argc, argv); 240 | auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); 241 | 242 | std::list all_handles; 243 | std::list service_bridges_1_to_2; 244 | std::list service_bridges_2_to_1; 245 | 246 | // bridge all topics listed in a ROS 1 parameter 247 | // the topics parameter needs to be an array 248 | // and each item needs to be a dictionary with the following keys; 249 | // topic: the name of the topic to bridge (e.g. '/topic_name') 250 | // type: the type of the topic to bridge (e.g. 'pkgname/msg/MsgName') 251 | // queue_size: the queue size to use (default: 100) 252 | const char * topics_parameter_name = "topics"; 253 | // the services parameters need to be arrays 254 | // and each item needs to be a dictionary with the following keys; 255 | // service: the name of the service to bridge (e.g. '/service_name') 256 | // type: the type of the service to bridge (e.g. 'pkgname/srv/SrvName') 257 | const char * services_1_to_2_parameter_name = "services_1_to_2"; 258 | const char * services_2_to_1_parameter_name = "services_2_to_1"; 259 | const char * service_execution_timeout_parameter_name = 260 | "ros1_bridge/parameter_bridge/service_execution_timeout"; 261 | if (argc > 1) { 262 | topics_parameter_name = argv[1]; 263 | } 264 | if (argc > 2) { 265 | services_1_to_2_parameter_name = argv[2]; 266 | } 267 | if (argc > 3) { 268 | services_2_to_1_parameter_name = argv[3]; 269 | } 270 | 271 | // Topics 272 | XmlRpc::XmlRpcValue topics; 273 | if ( 274 | ros1_node.getParam(topics_parameter_name, topics) && 275 | topics.getType() == XmlRpc::XmlRpcValue::TypeArray) 276 | { 277 | for (size_t i = 0; i < static_cast(topics.size()); ++i) { 278 | std::string topic_name = static_cast(topics[i]["topic"]); 279 | std::string type_name = static_cast(topics[i]["type"]); 280 | size_t queue_size = static_cast(topics[i]["queue_size"]); 281 | if (!queue_size) { 282 | queue_size = 100; 283 | } 284 | printf( 285 | "Trying to create bidirectional bridge for topic '%s' " 286 | "with ROS 2 type '%s'\n", 287 | topic_name.c_str(), type_name.c_str()); 288 | 289 | try { 290 | if (topics[i].hasMember("qos")) { 291 | printf("Setting up QoS for '%s': ", topic_name.c_str()); 292 | auto qos_settings = qos_from_params(topics[i]["qos"]); 293 | printf("\n"); 294 | ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( 295 | ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings); 296 | all_handles.push_back(handles); 297 | } else { 298 | ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( 299 | ros1_node, ros2_node, "", type_name, topic_name, queue_size); 300 | all_handles.push_back(handles); 301 | } 302 | } catch (std::runtime_error & e) { 303 | fprintf( 304 | stderr, 305 | "failed to create bidirectional bridge for topic '%s' " 306 | "with ROS 2 type '%s': %s\n", 307 | topic_name.c_str(), type_name.c_str(), e.what()); 308 | } 309 | } 310 | } else { 311 | fprintf( 312 | stderr, 313 | "The parameter '%s' either doesn't exist or isn't an array\n", topics_parameter_name); 314 | } 315 | 316 | // ROS 1 Services in ROS 2 317 | XmlRpc::XmlRpcValue services_1_to_2; 318 | if ( 319 | ros1_node.getParam(services_1_to_2_parameter_name, services_1_to_2) && 320 | services_1_to_2.getType() == XmlRpc::XmlRpcValue::TypeArray) 321 | { 322 | int service_execution_timeout{5}; 323 | ros1_node.getParamCached( 324 | service_execution_timeout_parameter_name, service_execution_timeout); 325 | for (size_t i = 0; i < static_cast(services_1_to_2.size()); ++i) { 326 | std::string service_name = static_cast(services_1_to_2[i]["service"]); 327 | std::string type_name = static_cast(services_1_to_2[i]["type"]); 328 | { 329 | // for backward compatibility 330 | std::string package_name = static_cast(services_1_to_2[i]["package"]); 331 | if (!package_name.empty()) { 332 | fprintf( 333 | stderr, 334 | "The service '%s' uses the key 'package' which is deprecated for " 335 | "services. Instead prepend the 'type' value with '/'.\n", 336 | service_name.c_str()); 337 | type_name = package_name + "/" + type_name; 338 | } 339 | } 340 | printf( 341 | "Trying to create bridge for ROS 2 service '%s' with type '%s'\n", 342 | service_name.c_str(), type_name.c_str()); 343 | 344 | const size_t index = type_name.find("/"); 345 | if (index == std::string::npos) { 346 | fprintf( 347 | stderr, 348 | "the service '%s' has a type '%s' without a slash.\n", 349 | service_name.c_str(), type_name.c_str()); 350 | continue; 351 | } 352 | auto factory = ros1_bridge::get_service_factory( 353 | "ros2", type_name.substr(0, index), type_name.substr(index + 1)); 354 | if (factory) { 355 | try { 356 | service_bridges_1_to_2.push_back( 357 | factory->service_bridge_1_to_2( 358 | ros1_node, ros2_node, service_name, service_execution_timeout)); 359 | printf("Created 1 to 2 bridge for service %s\n", service_name.c_str()); 360 | } catch (std::runtime_error & e) { 361 | fprintf( 362 | stderr, 363 | "failed to create bridge ROS 1 service '%s' with type '%s': %s\n", 364 | service_name.c_str(), type_name.c_str(), e.what()); 365 | } 366 | } else { 367 | fprintf( 368 | stderr, 369 | "failed to create bridge ROS 1 service '%s' no conversion for type '%s'\n", 370 | service_name.c_str(), type_name.c_str()); 371 | } 372 | } 373 | 374 | } else { 375 | fprintf( 376 | stderr, 377 | "The parameter '%s' either doesn't exist or isn't an array\n", 378 | services_1_to_2_parameter_name); 379 | } 380 | 381 | // ROS 2 Services in ROS 1 382 | XmlRpc::XmlRpcValue services_2_to_1; 383 | if ( 384 | ros1_node.getParam(services_2_to_1_parameter_name, services_2_to_1) && 385 | services_2_to_1.getType() == XmlRpc::XmlRpcValue::TypeArray) 386 | { 387 | for (size_t i = 0; i < static_cast(services_2_to_1.size()); ++i) { 388 | std::string service_name = static_cast(services_2_to_1[i]["service"]); 389 | std::string type_name = static_cast(services_2_to_1[i]["type"]); 390 | { 391 | // for backward compatibility 392 | std::string package_name = static_cast(services_2_to_1[i]["package"]); 393 | if (!package_name.empty()) { 394 | fprintf( 395 | stderr, 396 | "The service '%s' uses the key 'package' which is deprecated for " 397 | "services. Instead prepend the 'type' value with '/'.\n", 398 | service_name.c_str()); 399 | type_name = package_name + "/" + type_name; 400 | } 401 | } 402 | printf( 403 | "Trying to create bridge for ROS 1 service '%s' with type '%s'\n", 404 | service_name.c_str(), type_name.c_str()); 405 | 406 | const size_t index = type_name.find("/"); 407 | if (index == std::string::npos) { 408 | fprintf( 409 | stderr, 410 | "the service '%s' has a type '%s' without a slash.\n", 411 | service_name.c_str(), type_name.c_str()); 412 | continue; 413 | } 414 | 415 | auto factory = ros1_bridge::get_service_factory( 416 | "ros1", type_name.substr(0, index), type_name.substr(index + 1)); 417 | if (factory) { 418 | try { 419 | service_bridges_2_to_1.push_back( 420 | factory->service_bridge_2_to_1(ros1_node, ros2_node, service_name)); 421 | printf("Created 2 to 1 bridge for service %s\n", service_name.c_str()); 422 | } catch (std::runtime_error & e) { 423 | fprintf( 424 | stderr, 425 | "failed to create bridge ROS 2 service '%s' with type '%s': %s\n", 426 | service_name.c_str(), type_name.c_str(), e.what()); 427 | } 428 | } else { 429 | fprintf( 430 | stderr, 431 | "failed to create bridge ROS 2 service '%s' no conversion for type '%s'\n", 432 | service_name.c_str(), type_name.c_str()); 433 | } 434 | } 435 | 436 | } else { 437 | fprintf( 438 | stderr, 439 | "The parameter '%s' either doesn't exist or isn't an array\n", 440 | services_2_to_1_parameter_name); 441 | } 442 | 443 | // ROS 1 asynchronous spinner 444 | ros::AsyncSpinner async_spinner(1); 445 | async_spinner.start(); 446 | 447 | // ROS 2 spinning loop 448 | rclcpp::executors::SingleThreadedExecutor executor; 449 | while (ros1_node.ok() && rclcpp::ok()) { 450 | executor.spin_node_once(ros2_node, std::chrono::milliseconds(1000)); 451 | } 452 | 453 | return 0; 454 | } 455 | -------------------------------------------------------------------------------- /src/simple_bridge.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | // include ROS 1 20 | #include "ros/message.h" 21 | #ifdef __clang__ 22 | # pragma clang diagnostic push 23 | # pragma clang diagnostic ignored "-Wunused-parameter" 24 | #endif 25 | #include "ros/ros.h" 26 | #include "std_msgs/String.h" 27 | #ifdef __clang__ 28 | # pragma clang diagnostic pop 29 | #endif 30 | 31 | // include ROS 2 32 | #include "rclcpp/rclcpp.hpp" 33 | #include "std_msgs/msg/string.hpp" 34 | 35 | 36 | ros::Publisher ros1_pub; 37 | 38 | void ros2ChatterCallback(const std_msgs::msg::String::SharedPtr ros2_msg) 39 | { 40 | printf(" I heard from ROS 2: [%s]\n", ros2_msg->data.c_str()); 41 | 42 | std_msgs::String ros1_msg; 43 | ros1_msg.data = ros2_msg->data; 44 | printf(" Passing along to ROS 1: [%s]\n", ros1_msg.data.c_str()); 45 | ros1_pub.publish(ros1_msg); 46 | } 47 | 48 | 49 | rclcpp::Publisher::SharedPtr ros2_pub; 50 | 51 | void ros1ChatterCallback(const ros::MessageEvent & ros1_msg_event) 52 | { 53 | const boost::shared_ptr & connection_header = 54 | ros1_msg_event.getConnectionHeaderPtr(); 55 | std::string key = "callerid"; 56 | if (connection_header->find(key) != connection_header->end()) { 57 | if (connection_header->at(key) == "/ros_bridge") { 58 | printf(" I heard from ROS 1 from myself\n"); 59 | return; 60 | } 61 | printf("I heard from ROS 1 from: [%s]\n", connection_header->at(key).c_str()); 62 | } 63 | 64 | const boost::shared_ptr & ros1_msg = ros1_msg_event.getConstMessage(); 65 | printf("I heard from ROS 1: [%s]\n", ros1_msg->data.c_str()); 66 | 67 | std_msgs::msg::String ros2_msg; 68 | ros2_msg.data = ros1_msg->data; 69 | printf("Passing along to ROS 2: [%s]\n", ros2_msg.data.c_str()); 70 | ros2_pub->publish(ros2_msg); 71 | } 72 | 73 | 74 | int main(int argc, char * argv[]) 75 | { 76 | // ROS 1 node and publisher 77 | ros::init(argc, argv, "ros_bridge"); 78 | ros::NodeHandle ros1_node; 79 | ros1_pub = ros1_node.advertise("chatter", 10); 80 | 81 | // ROS 2 node and publisher 82 | rclcpp::init(argc, argv); 83 | auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); 84 | ros2_pub = ros2_node->create_publisher( 85 | "chatter", 10); 86 | 87 | // ROS 1 subscriber 88 | ros::Subscriber ros1_sub = ros1_node.subscribe( 89 | "chatter", 10, ros1ChatterCallback); 90 | 91 | // ROS 2 subscriber 92 | rclcpp::SubscriptionOptions options; 93 | options.ignore_local_publications = true; 94 | auto ros2_sub = ros2_node->create_subscription( 95 | "chatter", rclcpp::SensorDataQoS(), ros2ChatterCallback, options); 96 | 97 | // ROS 1 asynchronous spinner 98 | ros::AsyncSpinner async_spinner(1); 99 | async_spinner.start(); 100 | 101 | // ROS 2 spinning loop 102 | rclcpp::executors::SingleThreadedExecutor executor; 103 | while (ros1_node.ok() && rclcpp::ok()) { 104 | executor.spin_node_once(ros2_node, std::chrono::milliseconds(1000)); 105 | } 106 | 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /src/simple_bridge_1_to_2.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | // include ROS 1 20 | #ifdef __clang__ 21 | # pragma clang diagnostic push 22 | # pragma clang diagnostic ignored "-Wunused-parameter" 23 | #endif 24 | #include "ros/ros.h" 25 | #include "std_msgs/String.h" 26 | #ifdef __clang__ 27 | # pragma clang diagnostic pop 28 | #endif 29 | 30 | // include ROS 2 31 | #include "rclcpp/rclcpp.hpp" 32 | #include "std_msgs/msg/string.hpp" 33 | 34 | 35 | rclcpp::Publisher::SharedPtr pub; 36 | 37 | void chatterCallback(const std_msgs::String::ConstPtr & ros1_msg) 38 | { 39 | std::cout << "I heard: [" << ros1_msg->data << "]" << std::endl; 40 | 41 | auto ros2_msg = std::make_unique(); 42 | ros2_msg->data = ros1_msg->data; 43 | std::cout << "Passing along: [" << ros2_msg->data << "]" << std::endl; 44 | pub->publish(std::move(ros2_msg)); 45 | } 46 | 47 | int main(int argc, char * argv[]) 48 | { 49 | // ROS 2 node and publisher 50 | rclcpp::init(argc, argv); 51 | auto node = rclcpp::Node::make_shared("talker"); 52 | pub = node->create_publisher("chatter", 10); 53 | 54 | // ROS 1 node and subscriber 55 | ros::init(argc, argv, "listener"); 56 | ros::NodeHandle n; 57 | ros::Subscriber sub = n.subscribe("chatter", 10, chatterCallback); 58 | 59 | ros::spin(); 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /src/simple_bridge_2_to_1.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | // include ROS 1 18 | #ifdef __clang__ 19 | # pragma clang diagnostic push 20 | # pragma clang diagnostic ignored "-Wunused-parameter" 21 | #endif 22 | #include "ros/ros.h" 23 | #include "std_msgs/String.h" 24 | #ifdef __clang__ 25 | # pragma clang diagnostic pop 26 | #endif 27 | 28 | // include ROS 2 29 | #include "rclcpp/rclcpp.hpp" 30 | #include "std_msgs/msg/string.hpp" 31 | 32 | 33 | ros::Publisher pub; 34 | 35 | void chatterCallback(const std_msgs::msg::String::SharedPtr ros2_msg) 36 | { 37 | std::cout << "I heard: [" << ros2_msg->data << "]" << std::endl; 38 | 39 | std_msgs::String ros1_msg; 40 | ros1_msg.data = ros2_msg->data; 41 | std::cout << "Passing along: [" << ros1_msg.data << "]" << std::endl; 42 | pub.publish(ros1_msg); 43 | } 44 | 45 | int main(int argc, char * argv[]) 46 | { 47 | // ROS 1 node and publisher 48 | ros::init(argc, argv, "talker"); 49 | ros::NodeHandle n; 50 | pub = n.advertise("chatter", 10); 51 | 52 | // ROS 2 node and subscriber 53 | rclcpp::init(argc, argv); 54 | auto node = rclcpp::Node::make_shared("listener"); 55 | auto sub = node->create_subscription( 56 | "chatter", rclcpp::SensorDataQoS(), chatterCallback); 57 | 58 | rclcpp::spin(node); 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /src/static_bridge.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | // include ROS 1 18 | #ifdef __clang__ 19 | # pragma clang diagnostic push 20 | # pragma clang diagnostic ignored "-Wunused-parameter" 21 | #endif 22 | #include "ros/ros.h" 23 | #ifdef __clang__ 24 | # pragma clang diagnostic pop 25 | #endif 26 | 27 | // include ROS 2 28 | #include "rclcpp/rclcpp.hpp" 29 | 30 | #include "ros1_bridge/bridge.hpp" 31 | 32 | 33 | int main(int argc, char * argv[]) 34 | { 35 | // ROS 1 node 36 | ros::init(argc, argv, "ros_bridge"); 37 | ros::NodeHandle ros1_node; 38 | 39 | // ROS 2 node 40 | rclcpp::init(argc, argv); 41 | auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); 42 | 43 | // bridge one example topic 44 | std::string topic_name = "chatter"; 45 | std::string ros1_type_name = "std_msgs/String"; 46 | std::string ros2_type_name = "std_msgs/msg/String"; 47 | size_t queue_size = 10; 48 | 49 | auto handles = ros1_bridge::create_bidirectional_bridge( 50 | ros1_node, ros2_node, ros1_type_name, ros2_type_name, topic_name, queue_size); 51 | 52 | // ROS 1 asynchronous spinner 53 | ros::AsyncSpinner async_spinner(1); 54 | async_spinner.start(); 55 | 56 | // ROS 2 spinning loop 57 | rclcpp::executors::SingleThreadedExecutor executor; 58 | while (ros1_node.ok() && rclcpp::ok()) { 59 | executor.spin_node_once(ros2_node, std::chrono::milliseconds(1000)); 60 | } 61 | 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /test/test_convert_generic.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // GTest 16 | #include 17 | 18 | // ROS1 Messsages 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | // C++ Standard Library 28 | #include 29 | #include 30 | #include 31 | 32 | // ros1_bridge 33 | #include "ros1_bridge/factory.hpp" 34 | 35 | // RCLCPP 36 | #include "rclcpp/serialized_message.hpp" 37 | 38 | // ROS2 Messsages 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | template 47 | struct GenericTestBase 48 | { 49 | using ROS1_T = ROS1_T_; 50 | using ROS2_T = ROS2_T_; 51 | using FACTORY_T = ros1_bridge::Factory; 52 | 53 | FACTORY_T factory; 54 | ROS1_T ros1_msg; 55 | ROS2_T ros2_msg; 56 | GenericTestBase(const std::string & ros1_type_name, const std::string & ros2_type_name) 57 | : factory(ros1_type_name, ros2_type_name) {} 58 | 59 | // Generate serialized buffer from ROS1 message 60 | static std::vector generateRos1SerializedMessage(const ROS1_T & ros1_msg) 61 | { 62 | // Serialize ROS1 message 63 | const uint32_t length = ros::serialization::serializationLength(ros1_msg); 64 | std::vector buffer(length); 65 | ros::serialization::OStream out_stream(buffer.data(), length); 66 | ros::serialization::serialize(out_stream, ros1_msg); 67 | return buffer; 68 | } 69 | 70 | // Generate SerializedMessage from a ROS2 message 71 | rclcpp::SerializedMessage generateRos2SerializedMessage(const ROS2_T & ros2_msg) 72 | { 73 | // Directly serialize ROS2 message 74 | rclcpp::SerializedMessage serialized_msg; 75 | auto ret = rmw_serialize( 76 | &ros2_msg, factory.type_support_, 77 | &serialized_msg.get_rcl_serialized_message()); 78 | EXPECT_EQ(RMW_RET_OK, ret); 79 | return serialized_msg; 80 | } 81 | }; 82 | 83 | struct Vector3Test : public GenericTestBase 84 | { 85 | Vector3Test() 86 | : GenericTestBase("geometry_msgs/Vector3", "geometry_msgs/msg/Vector3") 87 | { 88 | ros1_msg.x = ros2_msg.x = 1.1; 89 | ros1_msg.y = ros2_msg.y = 2.2; 90 | ros1_msg.z = ros2_msg.z = 3.3; 91 | } 92 | }; 93 | 94 | 95 | struct StringTestEmpty : public GenericTestBase 97 | { 98 | StringTestEmpty() 99 | : GenericTestBase("std_msgs/String", "std_msgs/msg/String") {} 100 | }; 101 | 102 | 103 | struct StringTestHello : public StringTestEmpty 104 | { 105 | StringTestHello() 106 | { 107 | ros1_msg.data = ros2_msg.data = "hello"; 108 | } 109 | }; 110 | 111 | struct TimeTest : public GenericTestBase 113 | { 114 | TimeTest() 115 | : GenericTestBase("std_msgs/Time", "builtin_interfaces/msg/Time") 116 | { 117 | ros1_msg.data.sec = ros2_msg.sec = 1000 * 2000; 118 | ros1_msg.data.nsec = ros2_msg.nanosec = 1000 * 1000 * 1000; 119 | } 120 | }; 121 | 122 | struct HeaderTestEmpty : public GenericTestBase 124 | { 125 | HeaderTestEmpty() 126 | : GenericTestBase("std_msgs/Header", "std_msgs/msg/Header") 127 | { 128 | ros1_msg.stamp.sec = ros2_msg.stamp.sec = 100 * 100; 129 | ros1_msg.stamp.nsec = ros2_msg.stamp.nanosec = 100 * 200 * 300; 130 | ros1_msg.seq = 0; 131 | // Leave header.seq as zero, ros2_msg header does not have seq number so generic 132 | // serialization function will always write a zero to output stream 133 | } 134 | }; 135 | 136 | struct HeaderTestBaseLink : public HeaderTestEmpty 137 | { 138 | HeaderTestBaseLink() 139 | { 140 | ros1_msg.frame_id = ros2_msg.frame_id = "base_link"; 141 | } 142 | }; 143 | 144 | 145 | struct PoseTest : public GenericTestBase 147 | { 148 | PoseTest() 149 | : GenericTestBase("geometry_msgs/Pose", "geometry_msgs/msg/Pose") 150 | { 151 | ros1_msg.position.x = ros2_msg.position.x = 1.0; 152 | ros1_msg.position.y = ros2_msg.position.y = 2.0; 153 | ros1_msg.position.z = ros2_msg.position.z = 3.0; 154 | ros1_msg.orientation.x = ros2_msg.orientation.x = 4.0; 155 | ros1_msg.orientation.y = ros2_msg.orientation.y = 5.0; 156 | ros1_msg.orientation.z = ros2_msg.orientation.z = 6.0; 157 | ros1_msg.orientation.w = ros2_msg.orientation.w = 7.0; 158 | } 159 | }; 160 | 161 | struct PoseArrayTestEmpty : public GenericTestBase 163 | { 164 | PoseArrayTestEmpty() 165 | : GenericTestBase("geometry_msgs/PoseArray", "geometry_msgs/msg/PoseArray") {} 166 | }; 167 | 168 | struct PoseArrayTest : public PoseArrayTestEmpty 169 | { 170 | PoseArrayTest() 171 | { 172 | ros1_msg.header.frame_id = ros2_msg.header.frame_id = "base_link"; 173 | ros1_msg.header.stamp.sec = ros2_msg.header.stamp.sec = 0x12345678; 174 | ros1_msg.header.stamp.nsec = ros2_msg.header.stamp.nanosec = 0x76543210; 175 | const size_t size = 10; 176 | ros1_msg.poses.resize(size); 177 | ros2_msg.poses.resize(size); 178 | for (size_t ii = 0; ii < size; ++ii) { 179 | auto & pose1 = ros1_msg.poses.at(ii); 180 | auto & pose2 = ros2_msg.poses.at(ii); 181 | pose1.orientation.x = pose2.orientation.x = 0.1 * ii; 182 | // By default ROS2 message orientation is 1.0, so need to set this 183 | pose1.orientation.w = pose2.orientation.w = 0.9 * ii; 184 | } 185 | } 186 | }; 187 | 188 | 189 | template 190 | class ConvertGenericTest : public testing::Test 191 | { 192 | public: 193 | using TEST_T = TEST_T_; 194 | using ROS1_T = typename TEST_T::ROS1_T; 195 | using ROS2_T = typename TEST_T::ROS2_T; 196 | using FACTORY_T = typename TEST_T::FACTORY_T; 197 | 198 | TEST_T test; 199 | 200 | void TestBody() 201 | { 202 | // nothing 203 | } 204 | }; 205 | 206 | 207 | using ConvertGenericTypes = ::testing::Types< 208 | Vector3Test, // 0 209 | StringTestEmpty, // 1 210 | StringTestHello, // 2 211 | TimeTest, // 3 212 | HeaderTestEmpty, // 4 213 | HeaderTestBaseLink, // 5 214 | PoseTest, // 6 215 | PoseArrayTestEmpty, // 7 216 | PoseArrayTest // 8 217 | >; 218 | TYPED_TEST_SUITE(ConvertGenericTest, ConvertGenericTypes); 219 | 220 | 221 | // cppcheck-suppress syntaxError 222 | TYPED_TEST(ConvertGenericTest, test_factory_md5) 223 | { 224 | TestFixture fixture; 225 | using ROS1_T = typename TestFixture::ROS1_T; 226 | EXPECT_EQ( 227 | std::string(fixture.test.factory.get_ros1_md5sum()), 228 | std::string(ros::message_traits::MD5Sum::value())); 229 | } 230 | 231 | 232 | // cppcheck-suppress syntaxError 233 | TYPED_TEST(ConvertGenericTest, test_factory_data_type) 234 | { 235 | TestFixture fixture; 236 | using ROS1_T = typename TestFixture::ROS1_T; 237 | EXPECT_EQ( 238 | std::string(fixture.test.factory.get_ros1_data_type()), 239 | std::string(ros::message_traits::DataType::value())); 240 | } 241 | 242 | 243 | TYPED_TEST(ConvertGenericTest, test_factory_msg_def) 244 | { 245 | TestFixture fixture; 246 | using ROS1_T = typename TestFixture::ROS1_T; 247 | EXPECT_EQ( 248 | std::string(fixture.test.factory.get_ros1_message_definition()), 249 | std::string(ros::message_traits::Definition::value())); 250 | } 251 | 252 | // cppcheck-suppress syntaxError 253 | TYPED_TEST(ConvertGenericTest, test_convert_2_to_1) 254 | { 255 | // Directly serialize ROS2 message 256 | rclcpp::SerializedMessage serialized_msg = 257 | this->test.generateRos2SerializedMessage(this->test.ros2_msg); 258 | 259 | // Convert ROS2 SerializedMessage into ROS1 buffer 260 | // This is the function-under-test 261 | std::vector buffer2; 262 | bool success = this->test.factory.convert_2_to_1_generic(serialized_msg, buffer2); 263 | ASSERT_TRUE(success); 264 | 265 | // Write ROS1 message (which has same fields values) into a different stream 266 | std::vector buffer1 = 267 | TestFixture::TEST_T::generateRos1SerializedMessage(this->test.ros1_msg); 268 | 269 | // Buffer1 and Buffer2 should match in size and contents 270 | // ROS1 serialization this should always be true because there is no padding or empty space 271 | // left in any output buffers 272 | ASSERT_EQ(buffer1.size(), buffer2.size()); 273 | 274 | // The Gtest output from comparing buffers directly is a little hard to 275 | // understand when there is a few mismatching value 276 | // Instead use custom loop to make each mismatched byte easier to understand 277 | // ASSERT_EQ(buffer1, buffer2); 278 | unsigned mismatch_count = 0; 279 | const unsigned mismatch_count_limit = 10; 280 | for (size_t idx = 0; idx < buffer1.size(); ++idx) { 281 | int val1 = buffer1.at(idx); 282 | int val2 = buffer2.at(idx); 283 | EXPECT_EQ(val1, val2) << " idx=" << idx << " of " << buffer1.size(); 284 | if (val1 != val2) { 285 | ++mismatch_count; 286 | } 287 | ASSERT_LE( 288 | mismatch_count, 289 | mismatch_count_limit) << " stopping comparison after " << mismatch_count_limit << 290 | " mismatches"; 291 | } 292 | ASSERT_EQ(mismatch_count, 0u) << " the output buffers should be exactly the same"; 293 | } 294 | 295 | 296 | // cppcheck-suppress syntaxError 297 | TYPED_TEST(ConvertGenericTest, test_convert_1_to_2_to_1) 298 | { 299 | // Serialize ROS1 message into a ShapeShifter 300 | std::vector buffer1 = 301 | TestFixture::TEST_T::generateRos1SerializedMessage(this->test.ros1_msg); 302 | 303 | // Convert ROS1 shape-shifter into ROS2 SerializedMessage 304 | // This is the function-under-test 305 | rclcpp::SerializedMessage serialized_msg; 306 | bool success = this->test.factory.convert_1_to_2_generic(buffer1, serialized_msg); 307 | ASSERT_TRUE(success); 308 | 309 | // The serialized data from matching ROS2 message seems to contain padding between 310 | // values. This padding might have random values so the serialized data 311 | // might different even though the message fields all have the same value. 312 | // However ROS1 messages are "packed" so there is no space for garbage data. 313 | // Since convert_1_to_2_generic is tested indepedently elsewere, use it to 314 | // to test convert_1_to_2_generic 315 | std::vector buffer2; 316 | success = this->test.factory.convert_2_to_1_generic(serialized_msg, buffer2); 317 | ASSERT_TRUE(success); 318 | 319 | // Buffer1 and Buffer2 should match in size and contents 320 | // ROS1 serialization this should always be true because there is no padding or empty space 321 | // left in any output buffers 322 | ASSERT_EQ(buffer1.size(), buffer2.size()); 323 | 324 | unsigned mismatch_count = 0; 325 | const unsigned mismatch_count_limit = 10; 326 | for (size_t idx = 0; idx < buffer1.size(); ++idx) { 327 | int val1 = buffer1.at(idx); 328 | int val2 = buffer2.at(idx); 329 | EXPECT_EQ(val1, val2) << " idx=" << idx << " of " << buffer1.size(); 330 | if (val1 != val2) { 331 | ++mismatch_count; 332 | } 333 | ASSERT_LE( 334 | mismatch_count, 335 | mismatch_count_limit) << " stopping comparison after " << mismatch_count_limit << 336 | " mismatches"; 337 | } 338 | ASSERT_EQ(mismatch_count, 0u) << " the output buffers should be exactly the same"; 339 | } 340 | 341 | int main(int argc, char ** argv) 342 | { 343 | testing::InitGoogleTest(&argc, argv); 344 | return RUN_ALL_TESTS(); 345 | } 346 | -------------------------------------------------------------------------------- /test/test_ros1_client.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "diagnostic_msgs/SelfTest.h" 19 | #include "ros/ros.h" 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | ros::init(argc, argv, "ros1_bridge_test_client"); 24 | ros::NodeHandle n; 25 | ros::ServiceClient client = n.serviceClient( 26 | "ros1_bridge_test"); 27 | client.waitForExistence(); 28 | diagnostic_msgs::SelfTest request; 29 | if (client.call(request)) { 30 | if (request.response.id != "ros2") { 31 | throw std::runtime_error("Expected a response from ROS2"); 32 | } 33 | } else { 34 | throw std::runtime_error("Failed to call service ros1_bridge_test"); 35 | } 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /test/test_ros1_server.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "diagnostic_msgs/SelfTest.h" 16 | #include "ros/ros.h" 17 | 18 | bool add(diagnostic_msgs::SelfTest::Request & /* req */, diagnostic_msgs::SelfTest::Response & res) 19 | { 20 | res.id = "ros1"; 21 | return true; 22 | } 23 | 24 | int main(int argc, char ** argv) 25 | { 26 | ros::init(argc, argv, "ros1_bridge_test_server"); 27 | ros::NodeHandle n; 28 | ros::ServiceServer service = n.advertiseService("ros1_bridge_test", add); 29 | ros::spin(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /test/test_ros2_client.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "diagnostic_msgs/srv/self_test.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | using namespace std::chrono_literals; 22 | 23 | int main(int argc, char ** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | auto node = rclcpp::Node::make_shared("ros1_bridge_test_client"); 27 | auto client = node->create_client("ros1_bridge_test"); 28 | auto request = std::make_shared(); 29 | 30 | if (!client->wait_for_service(20s)) { 31 | throw std::runtime_error("Service is not available"); 32 | } 33 | 34 | auto future = client->async_send_request(request); 35 | if ( 36 | rclcpp::spin_until_future_complete(node, future, 2s) == 37 | rclcpp::FutureReturnCode::SUCCESS) 38 | { 39 | auto response = future.get(); 40 | if (response->id != "ros1") { 41 | throw std::runtime_error("Expected a response from ROS1"); 42 | } 43 | } else { 44 | throw std::runtime_error("Failed to call service ros1_bridge_test"); 45 | } 46 | 47 | rclcpp::shutdown(); 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /test/test_ros2_server.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "diagnostic_msgs/srv/self_test.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | void handler( 21 | const std::shared_ptr/* request_header */, 22 | const std::shared_ptr/* request */, 23 | std::shared_ptr response) 24 | { 25 | response->id = "ros2"; 26 | } 27 | 28 | int main(int argc, char ** argv) 29 | { 30 | rclcpp::init(argc, argv); 31 | auto node = rclcpp::Node::make_shared("ros1_bridge_test_server"); 32 | auto server = node->create_service("ros1_bridge_test", handler); 33 | rclcpp::spin(node); 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /test/test_ros2_to_ros1_serialization.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // GTest 16 | #include 17 | 18 | // ROS1 Messsages 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // C++ Standard Library 29 | #include 30 | #include 31 | #include 32 | 33 | // ros1_bridge 34 | #include "ros1_bridge/bridge.hpp" 35 | #include "ros1_bridge/factory.hpp" 36 | 37 | // RCLCPP 38 | #include "rclcpp/serialized_message.hpp" 39 | 40 | // ROS2 Messsages 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | 49 | template 50 | struct GenericTestBase 51 | { 52 | using ROS1_T = ROS1_T_; 53 | using ROS2_T = ROS2_T_; 54 | using FACTORY_T = ros1_bridge::Factory; 55 | 56 | FACTORY_T factory; 57 | ROS1_T ros1_msg; 58 | ROS2_T ros2_msg; 59 | GenericTestBase(const std::string & ros1_type_name, const std::string & ros2_type_name) 60 | : factory(ros1_type_name, ros2_type_name) {} 61 | 62 | // Generate serialized buffer from ROS1 message 63 | static std::vector generateRos1SerializedMessage(const ROS1_T & ros1_msg) 64 | { 65 | // Serialize ROS1 message 66 | const uint32_t length = ros::serialization::serializationLength(ros1_msg); 67 | std::vector buffer(length); 68 | ros::serialization::OStream out_stream(buffer.data(), length); 69 | ros::serialization::serialize(out_stream, ros1_msg); 70 | return buffer; 71 | } 72 | 73 | // Generate SerializedMessage from a ROS2 message 74 | rclcpp::SerializedMessage generateRos2SerializedMessage(const ROS2_T & ros2_msg) 75 | { 76 | // Directly serialize ROS2 message 77 | rclcpp::SerializedMessage serialized_msg; 78 | auto ret = rmw_serialize( 79 | &ros2_msg, factory.type_support_, 80 | &serialized_msg.get_rcl_serialized_message()); 81 | EXPECT_EQ(RMW_RET_OK, ret); 82 | return serialized_msg; 83 | } 84 | 85 | static bool compare(const ROS2_T &, const ROS2_T &) 86 | { 87 | return false; 88 | } 89 | }; 90 | 91 | template 92 | bool compareXYZ(const T & a, const T & b) 93 | { 94 | return (a.x == b.x) && (a.y == b.y) && (a.z == b.z); 95 | } 96 | 97 | template 98 | bool compareXYZW(const T & a, const T & b) 99 | { 100 | return (a.x == b.x) && (a.y == b.y) && (a.z == b.z) && (a.w == b.w); 101 | } 102 | 103 | bool compareTime( 104 | const builtin_interfaces::msg::Time & a, 105 | const builtin_interfaces::msg::Time & b) 106 | { 107 | return (a.sec == b.sec) && (a.nanosec == b.nanosec); 108 | } 109 | 110 | bool compareDuration( 111 | const builtin_interfaces::msg::Duration & a, 112 | const builtin_interfaces::msg::Duration & b) 113 | { 114 | return (a.sec == b.sec) && (a.nanosec == b.nanosec); 115 | } 116 | 117 | bool compareHeader( 118 | const std_msgs::msg::Header & a, 119 | const std_msgs::msg::Header & b) 120 | { 121 | return compareTime(a.stamp, b.stamp) && (a.frame_id == b.frame_id); 122 | } 123 | 124 | bool comparePose( 125 | const geometry_msgs::msg::Pose & a, 126 | const geometry_msgs::msg::Pose & b) 127 | { 128 | return compareXYZ(a.position, b.position) && compareXYZW(a.orientation, b.orientation); 129 | } 130 | 131 | bool comparePoseArray( 132 | const geometry_msgs::msg::PoseArray & a, 133 | const geometry_msgs::msg::PoseArray & b) 134 | { 135 | if (!compareHeader(a.header, b.header)) { 136 | return false; 137 | } 138 | if (a.poses.size() != b.poses.size()) { 139 | std::cerr << "a.poses.size() != b.poses.size()" << a.poses.size() << " != " << b.poses.size() << 140 | std::endl; 141 | return false; 142 | } 143 | for (size_t idx = 0; idx < a.poses.size(); ++idx) { 144 | if (!comparePose(a.poses.at(idx), b.poses.at(idx))) { 145 | return false; 146 | } 147 | } 148 | return true; 149 | } 150 | 151 | struct Vector3Test : public GenericTestBase 152 | { 153 | Vector3Test() 154 | : GenericTestBase("geometry_msgs/Vector3", "geometry_msgs/msg/Vector3") 155 | { 156 | ros1_msg.x = ros2_msg.x = 1.1; 157 | ros1_msg.y = ros2_msg.y = 2.2; 158 | ros1_msg.z = ros2_msg.z = 3.3; 159 | } 160 | 161 | static bool compare( 162 | const geometry_msgs::msg::Vector3 & a, 163 | const geometry_msgs::msg::Vector3 & b) 164 | { 165 | return compareXYZ(a, b); 166 | } 167 | }; 168 | 169 | 170 | struct StringTestEmpty : public GenericTestBase 172 | { 173 | StringTestEmpty() 174 | : GenericTestBase("std_msgs/String", "std_msgs/msg/String") {} 175 | 176 | static bool compare( 177 | const std_msgs::msg::String & a, 178 | const std_msgs::msg::String & b) 179 | { 180 | return a.data == b.data; 181 | } 182 | }; 183 | 184 | 185 | struct StringTestHello : public StringTestEmpty 186 | { 187 | StringTestHello() 188 | { 189 | ros1_msg.data = ros2_msg.data = "hello"; 190 | } 191 | }; 192 | 193 | struct TimeTest : public GenericTestBase< 194 | std_msgs::Time, 195 | builtin_interfaces::msg::Time> 196 | { 197 | TimeTest() 198 | : GenericTestBase("std_msgs/Time", "builtin_interfaces/msg/Time") 199 | { 200 | ros1_msg.data.sec = ros2_msg.sec = 1000 * 2000; 201 | ros1_msg.data.nsec = ros2_msg.nanosec = 1000 * 1000 * 1000; 202 | } 203 | 204 | static bool compare( 205 | const builtin_interfaces::msg::Time & a, 206 | const builtin_interfaces::msg::Time & b) 207 | { 208 | return compareTime(a, b); 209 | } 210 | }; 211 | 212 | struct DurationTest : public GenericTestBase< 213 | std_msgs::Duration, 214 | builtin_interfaces::msg::Duration> 215 | { 216 | DurationTest() 217 | : GenericTestBase("std_msgs/Duration", "builtin_interfaces/msg/Duration") 218 | { 219 | ros1_msg.data.sec = ros2_msg.sec = 1001 * 2001; 220 | ros1_msg.data.nsec = ros2_msg.nanosec = 1002 * 1003 * 1004; 221 | } 222 | 223 | static bool compare( 224 | const builtin_interfaces::msg::Duration & a, 225 | const builtin_interfaces::msg::Duration & b) 226 | { 227 | return compareDuration(a, b); 228 | } 229 | }; 230 | 231 | struct HeaderTestEmpty : public GenericTestBase< 232 | std_msgs::Header, 233 | std_msgs::msg::Header> 234 | { 235 | HeaderTestEmpty() 236 | : GenericTestBase("std_msgs/Header", "std_msgs/msg/Header") 237 | { 238 | ros1_msg.stamp.sec = ros2_msg.stamp.sec = 100 * 100; 239 | ros1_msg.stamp.nsec = ros2_msg.stamp.nanosec = 100 * 200 * 300; 240 | ros1_msg.seq = 0; 241 | // Leave header.seq as zero, ros2_msg header does not have seq number so generic 242 | // serialization function will always write a zero to output stream 243 | } 244 | 245 | static bool compare( 246 | const std_msgs::msg::Header & a, 247 | const std_msgs::msg::Header & b) 248 | { 249 | return compareHeader(a, b); 250 | } 251 | }; 252 | 253 | struct HeaderTestBaseLink : public HeaderTestEmpty 254 | { 255 | HeaderTestBaseLink() 256 | { 257 | ros1_msg.frame_id = ros2_msg.frame_id = "base_link"; 258 | } 259 | 260 | static bool compare( 261 | const std_msgs::msg::Header & a, 262 | const std_msgs::msg::Header & b) 263 | { 264 | return compareHeader(a, b); 265 | } 266 | }; 267 | 268 | struct PoseTest : public GenericTestBase< 269 | geometry_msgs::Pose, 270 | geometry_msgs::msg::Pose> 271 | { 272 | PoseTest() 273 | : GenericTestBase("geometry_msgs/Pose", "geometry_msgs/msg/Pose") 274 | { 275 | ros1_msg.position.x = ros2_msg.position.x = 1.0; 276 | ros1_msg.position.y = ros2_msg.position.y = 2.0; 277 | ros1_msg.position.z = ros2_msg.position.z = 3.0; 278 | ros1_msg.orientation.x = ros2_msg.orientation.x = 4.0; 279 | ros1_msg.orientation.y = ros2_msg.orientation.y = 5.0; 280 | ros1_msg.orientation.z = ros2_msg.orientation.z = 6.0; 281 | ros1_msg.orientation.w = ros2_msg.orientation.w = 7.0; 282 | } 283 | 284 | static bool compare( 285 | const geometry_msgs::msg::Pose & a, 286 | const geometry_msgs::msg::Pose & b) 287 | { 288 | return comparePose(a, b); 289 | } 290 | }; 291 | 292 | struct PoseArrayTestEmpty : public GenericTestBase< 293 | geometry_msgs::PoseArray, 294 | geometry_msgs::msg::PoseArray> 295 | { 296 | PoseArrayTestEmpty() 297 | : GenericTestBase("geometry_msgs/PoseArray", "geometry_msgs/msg/PoseArray") {} 298 | 299 | static bool compare( 300 | const geometry_msgs::msg::PoseArray & a, 301 | const geometry_msgs::msg::PoseArray & b) 302 | { 303 | return comparePoseArray(a, b); 304 | } 305 | }; 306 | 307 | struct PoseArrayTest : public PoseArrayTestEmpty 308 | { 309 | PoseArrayTest() 310 | { 311 | ros1_msg.header.frame_id = ros2_msg.header.frame_id = "base_link"; 312 | ros1_msg.header.stamp.sec = ros2_msg.header.stamp.sec = 0x12345678; 313 | ros1_msg.header.stamp.nsec = ros2_msg.header.stamp.nanosec = 0x76543210; 314 | const size_t size = 10; 315 | ros1_msg.poses.resize(size); 316 | ros2_msg.poses.resize(size); 317 | for (size_t ii = 0; ii < size; ++ii) { 318 | auto & pose1 = ros1_msg.poses.at(ii); 319 | auto & pose2 = ros2_msg.poses.at(ii); 320 | pose1.orientation.x = pose2.orientation.x = 0.1 * ii; 321 | // By default ROS2 message orientation is 1.0, so need to set this 322 | pose1.orientation.w = pose2.orientation.w = 0.9 * ii; 323 | } 324 | } 325 | }; 326 | 327 | 328 | template 329 | class Ros2ToRos1SerializationTest : public testing::Test 330 | { 331 | public: 332 | using TEST_T = TEST_T_; 333 | using ROS1_T = typename TEST_T::ROS1_T; 334 | using ROS2_T = typename TEST_T::ROS2_T; 335 | using FACTORY_T = typename TEST_T::FACTORY_T; 336 | 337 | TEST_T test; 338 | 339 | void TestBody() {} 340 | }; 341 | 342 | 343 | using Ros2ToRos1SerializationTypes = ::testing::Types< 344 | Vector3Test, // 0 345 | StringTestEmpty, // 1 346 | StringTestHello, // 2 347 | TimeTest, // 3 348 | DurationTest, // 4 349 | HeaderTestEmpty, // 5 350 | HeaderTestBaseLink, // 6 351 | PoseTest, // 7 352 | PoseArrayTestEmpty, // 8 353 | PoseArrayTest // 9 354 | >; 355 | TYPED_TEST_SUITE(Ros2ToRos1SerializationTest, Ros2ToRos1SerializationTypes); 356 | 357 | 358 | // cppcheck-suppress syntaxError 359 | TYPED_TEST(Ros2ToRos1SerializationTest, test_length) 360 | { 361 | using FACTORY_T = typename TestFixture::FACTORY_T; 362 | using ROS1_T = typename TestFixture::ROS1_T; 363 | using ROS2_T = typename TestFixture::ROS2_T; 364 | const ROS1_T & ros1_msg = this->test.ros1_msg; 365 | const ROS2_T & ros2_msg = this->test.ros2_msg; 366 | uint32_t ros1_len = ros::serialization::serializationLength(ros1_msg); 367 | uint32_t ros2_len = FACTORY_T::length_2_as_1_stream(ros2_msg); 368 | EXPECT_EQ(ros1_len, ros2_len); 369 | } 370 | 371 | 372 | // cppcheck-suppress syntaxError 373 | TYPED_TEST(Ros2ToRos1SerializationTest, test_get_factory) 374 | { 375 | // Make sure get_factory still works with Header and other types 376 | const std::string & ros1_type_name = this->test.factory.ros1_type_name_; 377 | const std::string & ros2_type_name = this->test.factory.ros2_type_name_; 378 | std::shared_ptr factory; 379 | EXPECT_NO_THROW(factory = ros1_bridge::get_factory(ros1_type_name, ros2_type_name)); 380 | ASSERT_TRUE(static_cast(factory)); 381 | } 382 | 383 | 384 | // cppcheck-suppress syntaxError 385 | TYPED_TEST(Ros2ToRos1SerializationTest, test_deserialization) 386 | { 387 | using FACTORY_T = typename TestFixture::FACTORY_T; 388 | using ROS1_T = typename TestFixture::ROS1_T; 389 | using ROS2_T = typename TestFixture::ROS2_T; 390 | const ROS1_T & ros1_msg = this->test.ros1_msg; 391 | const ROS2_T & ros2_msg = this->test.ros2_msg; 392 | const uint32_t ros1_len = ros::serialization::serializationLength(ros1_msg); 393 | std::vector buffer(ros1_len); 394 | ros::serialization::OStream out_stream(buffer.data(), ros1_len); 395 | FACTORY_T::convert_2_to_1(ros2_msg, out_stream); 396 | ros::serialization::IStream in_stream(buffer.data(), ros1_len); 397 | ROS2_T ros2_msg2; 398 | FACTORY_T::convert_1_to_2(in_stream, ros2_msg2); 399 | EXPECT_TRUE(TestFixture::TEST_T::compare(ros2_msg, ros2_msg2)); 400 | } 401 | 402 | 403 | // cppcheck-suppress syntaxError 404 | TYPED_TEST(Ros2ToRos1SerializationTest, test_serialization) 405 | { 406 | using FACTORY_T = typename TestFixture::FACTORY_T; 407 | using ROS1_T = typename TestFixture::ROS1_T; 408 | using ROS2_T = typename TestFixture::ROS2_T; 409 | const ROS1_T & ros1_msg = this->test.ros1_msg; 410 | const ROS2_T & ros2_msg = this->test.ros2_msg; 411 | const uint32_t ros1_len = ros::serialization::serializationLength(ros1_msg); 412 | const uint32_t ros2_len = FACTORY_T::length_2_as_1_stream(ros2_msg); 413 | ASSERT_EQ(ros1_len, ros2_len); 414 | const uint32_t len = ros1_len; 415 | 416 | // Serialize both streams and check byte-by-byte the value match 417 | const uint8_t guard_value = 0x42; 418 | 419 | std::vector buffer1(len + 1); 420 | { 421 | buffer1.at(len) = guard_value; 422 | ros::serialization::OStream stream(buffer1.data(), len); 423 | EXPECT_EQ(stream.getLength(), len); 424 | ros::serialization::serialize(stream, ros1_msg); 425 | // after serializating all the "capacity" should be used up 426 | EXPECT_EQ(stream.getLength(), 0ul); 427 | // Hacky check to make sure serialization didn't run over the end of the buffer 428 | EXPECT_EQ(buffer1.at(len), guard_value); 429 | } 430 | 431 | std::vector buffer2(len + 1); 432 | { 433 | buffer2.at(len) = guard_value; 434 | ros::serialization::OStream stream(buffer2.data(), len); 435 | EXPECT_EQ(stream.getLength(), len); 436 | FACTORY_T::convert_2_to_1(ros2_msg, stream); 437 | EXPECT_EQ(stream.getLength(), 0ul); 438 | EXPECT_EQ(buffer2.at(len), guard_value); 439 | } 440 | 441 | unsigned mismatch_count = 0; 442 | const unsigned max_mismatch_count = 0; 443 | for (size_t idx = 0; idx < len; ++idx) { 444 | unsigned val1 = buffer1.at(idx); 445 | unsigned val2 = buffer2.at(idx); 446 | EXPECT_EQ(val1, val2) << " idx=" << idx << " of " << len; 447 | if (val1 != val2) { 448 | ++mismatch_count; 449 | ASSERT_LE(mismatch_count, max_mismatch_count); 450 | } 451 | } 452 | ASSERT_EQ(mismatch_count, 0u); 453 | } 454 | 455 | 456 | int main(int argc, char ** argv) 457 | { 458 | testing::InitGoogleTest(&argc, argv); 459 | return RUN_ALL_TESTS(); 460 | } 461 | -------------------------------------------------------------------------------- /test/test_services_across_dynamic_bridge.py.in: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import unittest 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import ExecuteProcess 19 | from launch.actions import OpaqueFunction 20 | 21 | import launch_testing 22 | import launch_testing.actions 23 | 24 | 25 | TEST_BRIDGE_ROS1_ENV = '@TEST_BRIDGE_ROS1_ENV@' 26 | TEST_BRIDGE_ROSCORE = '@TEST_BRIDGE_ROSCORE@' 27 | TEST_BRIDGE_ROS1_CLIENT = '@TEST_BRIDGE_ROS1_CLIENT@' 28 | TEST_BRIDGE_ROS1_SERVER = '@TEST_BRIDGE_ROS1_SERVER@' 29 | TEST_BRIDGE_DYNAMIC_BRIDGE = '@TEST_BRIDGE_DYNAMIC_BRIDGE@' 30 | TEST_BRIDGE_ROS2_CLIENT = '@TEST_BRIDGE_ROS2_CLIENT@' 31 | TEST_BRIDGE_ROS2_SERVER = '@TEST_BRIDGE_ROS2_SERVER@' 32 | 33 | 34 | @launch_testing.parametrize('test_name,server_cmd,client_cmd', [ 35 | ('ros1_server_ros2_client_across_dynamic_bridge', 36 | [TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_ROS1_SERVER], 37 | [TEST_BRIDGE_ROS2_CLIENT]), 38 | ('ros2_server_ros1_client_across_dynamic_bridge', 39 | [TEST_BRIDGE_ROS2_SERVER], 40 | [TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_ROS1_CLIENT]), 41 | ]) 42 | def generate_test_description(test_name, server_cmd, client_cmd): 43 | launch_description = LaunchDescription() 44 | 45 | # ROS 1 core 46 | launch_description.add_action(ExecuteProcess( 47 | cmd=[TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_ROSCORE], 48 | name=test_name + '__roscore', 49 | )) 50 | 51 | # dynamic bridge 52 | rosbridge_process = ExecuteProcess( 53 | cmd=[TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_DYNAMIC_BRIDGE], 54 | name=test_name + '__dynamic_bridge', 55 | ) 56 | launch_description.add_action(rosbridge_process) 57 | 58 | server_process = ExecuteProcess( 59 | cmd=server_cmd, name=test_name + '__server', 60 | ) 61 | launch_description.add_action(server_process) 62 | 63 | client_process = ExecuteProcess( 64 | cmd=client_cmd, name=test_name + '__client', 65 | ) 66 | launch_description.add_action(client_process) 67 | 68 | launch_description.add_action( 69 | launch_testing.actions.ReadyToTest() 70 | ) 71 | return launch_description, locals() 72 | 73 | 74 | class TestServicesAcrossDynamicBridge(unittest.TestCase): 75 | def test_client_process_terminates_after_a_finite_amount_of_time(self, 76 | client_process, 77 | proc_info): 78 | """Test that the client executables terminates after a finite amount of time.""" 79 | proc_info.assertWaitForShutdown(process=client_process, timeout=30) 80 | 81 | 82 | @launch_testing.post_shutdown_test() 83 | class TestServicesAcrossDynamicBridgeAfterShutdown(unittest.TestCase): 84 | 85 | def test_processes_finished_gracefully(self, proc_info, rosbridge_process, 86 | server_process, client_process): 87 | """Test that both executables finished gracefully.""" 88 | launch_testing.asserts.assertExitCodes(proc_info, process=rosbridge_process) 89 | launch_testing.asserts.assertExitCodes(proc_info, process=server_process) 90 | launch_testing.asserts.assertExitCodes( 91 | proc_info, 92 | [launch_testing.asserts.EXIT_OK], 93 | client_process 94 | ) 95 | -------------------------------------------------------------------------------- /test/test_topics_across_dynamic_bridge.py.in: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | import re 17 | 18 | import unittest 19 | 20 | from launch import LaunchDescription 21 | from launch.actions import ExecuteProcess 22 | from launch.actions import OpaqueFunction 23 | 24 | import launch_testing 25 | import launch_testing.actions 26 | import launch_testing_ros 27 | 28 | from ros2run.api import get_executable_path 29 | 30 | 31 | TEST_BRIDGE_ROS1_ENV = '@TEST_BRIDGE_ROS1_ENV@' 32 | TEST_BRIDGE_ROSCORE = '@TEST_BRIDGE_ROSCORE@' 33 | TEST_BRIDGE_ROS1_TALKER = ['rosrun', 'roscpp_tutorials', 'talker'] 34 | TEST_BRIDGE_ROS1_LISTENER = ['rosrun', 'rospy_tutorials', 'listener'] 35 | TEST_BRIDGE_DYNAMIC_BRIDGE = '@TEST_BRIDGE_DYNAMIC_BRIDGE@' 36 | TEST_BRIDGE_ROS2_TALKER = get_executable_path( 37 | package_name='demo_nodes_cpp', executable_name='talker') 38 | TEST_BRIDGE_ROS2_LISTENER = get_executable_path( 39 | package_name='demo_nodes_cpp', executable_name='listener') 40 | TEST_BRIDGE_RMW = '@TEST_BRIDGE_RMW@' 41 | 42 | 43 | @launch_testing.parametrize('test_name,talker_cmd,listener_cmd,logs_stream', [ 44 | ('ros1_talker_ros2_listener_across_dynamic_bridge', 45 | [TEST_BRIDGE_ROS1_ENV] + TEST_BRIDGE_ROS1_TALKER, 46 | [TEST_BRIDGE_ROS2_LISTENER], 'stderr'), 47 | ('ros2_talker_ros1_listener_across_dynamic_bridge', 48 | [TEST_BRIDGE_ROS2_TALKER], 49 | [TEST_BRIDGE_ROS1_ENV] + TEST_BRIDGE_ROS1_LISTENER, 'stdout'), 50 | ]) 51 | def generate_test_description(test_name, talker_cmd, listener_cmd, logs_stream): 52 | launch_description = LaunchDescription() 53 | 54 | # ROS 1 core 55 | launch_description.add_action(ExecuteProcess( 56 | cmd=[TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_ROSCORE], 57 | name=test_name + '__roscore', 58 | )) 59 | 60 | # dynamic bridge 61 | rosbridge_process = ExecuteProcess( 62 | cmd=[TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_DYNAMIC_BRIDGE], 63 | name=test_name + '__dynamic_bridge', 64 | ) 65 | launch_description.add_action(rosbridge_process) 66 | 67 | talker_process = ExecuteProcess( 68 | cmd=talker_cmd, name=test_name + '__talker', 69 | ) 70 | launch_description.add_action(talker_process) 71 | 72 | env = dict(os.environ) 73 | env['PYTHONUNBUFFERED'] = '1' 74 | listener_process = ExecuteProcess( 75 | cmd=listener_cmd, name=test_name + '__listener', env=env 76 | ) 77 | launch_description.add_action(listener_process) 78 | 79 | launch_description.add_action( 80 | launch_testing.actions.ReadyToTest() 81 | ) 82 | return launch_description, locals() 83 | 84 | 85 | class TestTopicsAcrossDynamicBridge(unittest.TestCase): 86 | 87 | def test_listener_output(self, proc_output, listener_process, logs_stream): 88 | output_filter = launch_testing_ros.tools.basic_output_filter( 89 | filtered_rmw_implementation=TEST_BRIDGE_RMW 90 | ) 91 | proc_output.assertWaitFor( 92 | expected_output=[re.compile('I heard.+')], 93 | process=listener_process, 94 | output_filter=output_filter, 95 | timeout=10, 96 | stream=logs_stream 97 | ) 98 | 99 | 100 | @launch_testing.post_shutdown_test() 101 | class TestTopicsAcrossDynamicBridgeAfterShutdown(unittest.TestCase): 102 | 103 | def test_processes_finished_gracefully(self, proc_info, rosbridge_process, 104 | talker_process, listener_process): 105 | """Test that both executables finished gracefully.""" 106 | launch_testing.asserts.assertExitCodes(proc_info, process=rosbridge_process) 107 | launch_testing.asserts.assertExitCodes(proc_info, process=talker_process) 108 | launch_testing.asserts.assertExitCodes(proc_info, process=listener_process) 109 | --------------------------------------------------------------------------------