From 0fcd0a55c1476601ef050fc3a12a91a2e30bf217 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 18 Apr 2024 23:22:48 +0000 Subject: [PATCH] Imported upstream version '1.1.1' of 'upstream' --- .github/workflows/docker-ros.yml | 28 ++ .github/workflows/industrial_ci.yml | 25 ++ CHANGELOG.rst | 17 ++ CMakeLists.txt | 286 ++++++++++++++++++ LICENSE | 21 ++ README.md | 185 +++++++++++ .../MessageTfFrameTransformer.h | 154 ++++++++++ .../MessageTfFrameTransformer.ros2.hpp | 172 +++++++++++ .../message_types.h | 13 + .../message_types.macro | 31 ++ .../message_types.ros2.hpp | 13 + .../message_types.ros2.macro | 31 ++ launch/message_tf_frame_transformer.launch | 18 ++ ...ssage_tf_frame_transformer.launch.ros2.xml | 23 ++ nodelet_plugins.xml | 7 + package.xml | 37 +++ src/MessageTfFrameTransformer.cpp | 123 ++++++++ src/MessageTfFrameTransformer.node.cpp | 44 +++ src/MessageTfFrameTransformer.ros2.cpp | 170 +++++++++++ 19 files changed, 1398 insertions(+) create mode 100644 .github/workflows/docker-ros.yml create mode 100644 .github/workflows/industrial_ci.yml create mode 100644 CHANGELOG.rst create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 README.md create mode 100644 include/message_tf_frame_transformer/MessageTfFrameTransformer.h create mode 100644 include/message_tf_frame_transformer/MessageTfFrameTransformer.ros2.hpp create mode 100644 include/message_tf_frame_transformer/message_types.h create mode 100644 include/message_tf_frame_transformer/message_types.macro create mode 100644 include/message_tf_frame_transformer/message_types.ros2.hpp create mode 100644 include/message_tf_frame_transformer/message_types.ros2.macro create mode 100644 launch/message_tf_frame_transformer.launch create mode 100644 launch/message_tf_frame_transformer.launch.ros2.xml create mode 100644 nodelet_plugins.xml create mode 100644 package.xml create mode 100644 src/MessageTfFrameTransformer.cpp create mode 100644 src/MessageTfFrameTransformer.node.cpp create mode 100644 src/MessageTfFrameTransformer.ros2.cpp diff --git a/.github/workflows/docker-ros.yml b/.github/workflows/docker-ros.yml new file mode 100644 index 0000000..4db714b --- /dev/null +++ b/.github/workflows/docker-ros.yml @@ -0,0 +1,28 @@ +name: docker-ros + +on: push + +jobs: + + ros: + runs-on: ubuntu-latest + steps: + - uses: ika-rwth-aachen/docker-ros@v1.0.0 + with: + image-tag: ros + base-image: rwthika/ros:latest + command: rosrun message_tf_frame_transformer message_tf_frame_transformer + platform: amd64,arm64 + target: run + + ros2: + runs-on: ubuntu-latest + steps: + - uses: ika-rwth-aachen/docker-ros@v1.0.0 + with: + image-tag: ros2 + base-image: rwthika/ros2:latest + command: ros2 run message_tf_frame_transformer message_tf_frame_transformer + platform: amd64,arm64 + target: run + enable-push-as-latest: 'true' diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml new file mode 100644 index 0000000..cb3afbc --- /dev/null +++ b/.github/workflows/industrial_ci.yml @@ -0,0 +1,25 @@ +name: industrial_ci + +on: push + +jobs: + industrial_ci: + name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) + runs-on: ubuntu-latest + strategy: + matrix: + ROS_DISTRO: + - noetic + - humble + - iron + - rolling + ROS_REPO: + - testing + - main + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + with: + config: ${{ toJSON(matrix) }} + env: + NOT_TEST_BUILD: true diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..fb9fbfa --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,17 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package message_tf_frame_transformer +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.1 (2024-03-19) +------------------ +* Merge pull request #3 from clalancette/clalancette/fix-compile-error + Fix a compile error with modern rclcpp. +* Contributors: Lennart Reiher + +1.1.0 (2023-06-10) +------------------ +* integrate docker-ros +* add support for ros2 iron +* add remark for pointcloud2 + closes #1 +* Contributors: Lennart Reiher diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..41baf52 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,286 @@ +cmake_minimum_required(VERSION 3.12.0 FATAL_ERROR) +project(message_tf_frame_transformer) + +find_package(ros_environment REQUIRED QUIET) +set(ROS_VERSION $ENV{ROS_VERSION}) + +## Compile as C++17 +add_compile_options(-std=c++17) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# === ROS2 (AMENT) ============================================================= +if(${ROS_VERSION} EQUAL 2) + + find_package(ament_cmake REQUIRED) + + find_package(geometry_msgs REQUIRED) + find_package(rclcpp REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(tf2 REQUIRED) + find_package(tf2_geometry_msgs REQUIRED) + find_package(tf2_ros REQUIRED) + find_package(tf2_sensor_msgs REQUIRED) + + add_executable(${PROJECT_NAME} src/MessageTfFrameTransformer.ros2.cpp) + + target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + + ament_target_dependencies(${PROJECT_NAME} + geometry_msgs + rclcpp + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + ) + + install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} + ) + + install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + FILES_MATCHING PATTERN "*ros2*" + ) + + # if(BUILD_TESTING) + # find_package(ament_lint_auto REQUIRED) + # # the following line skips the linter which checks for copyrights + # # comment the line when a copyright and license is added to all source files + # set(ament_cmake_copyright_FOUND TRUE) + # # the following line skips cpplint (only works in a git repo) + # # comment the line when this package is in a git repo and when + # # a copyright and license is added to all source files + # set(ament_cmake_cpplint_FOUND TRUE) + # ament_lint_auto_find_test_dependencies() + # endif() + + ament_package() + +# === ROS1 (CATKIN) ============================================================ +elseif(${ROS_VERSION} EQUAL 1) + + ## Find catkin macros and libraries + ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) + ## is used, also find other catkin packages + find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nodelet + roscpp + tf2 + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + topic_tools + ) + + ## System dependencies are found with CMake's conventions + # find_package(Boost REQUIRED) + + ## Uncomment this if the package has a setup.py. This macro ensures + ## modules and global scripts declared therein get installed + ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html + # catkin_python_setup() + + ################################################ + ## Declare ROS messages, services and actions ## + ################################################ + + ## To declare and build messages, services or actions from within this + ## package, follow these steps: + ## * Let MSG_DEP_SET be the set of packages whose message types you use in + ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). + ## * In the file package.xml: + ## * add a build_depend tag for "message_generation" + ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET + ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in + ## but can be declared for certainty nonetheless: + ## * add a exec_depend tag for "message_runtime" + ## * In this file (CMakeLists.txt): + ## * add "message_generation" and every package in MSG_DEP_SET to + ## find_package(catkin REQUIRED COMPONENTS ...) + ## * add "message_runtime" and every package in MSG_DEP_SET to + ## catkin_package(CATKIN_DEPENDS ...) + ## * uncomment the add_*_files sections below as needed + ## and list every .msg/.srv/.action file to be processed + ## * uncomment the generate_messages entry below + ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + + ## Generate messages in the 'msg' folder + # add_message_files( + # FILES + # Message1.msg + # Message2.msg + # ) + + ## Generate services in the 'srv' folder + # add_service_files( + # FILES + # Service1.srv + # Service2.srv + # ) + + ## Generate actions in the 'action' folder + # add_action_files( + # FILES + # Action1.action + # Action2.action + # ) + + ## Generate added messages and services with any dependencies listed here + # generate_messages( + # DEPENDENCIES + # std_msgs + # ) + + ################################################ + ## Declare ROS dynamic reconfigure parameters ## + ################################################ + + ## To declare and build dynamic reconfigure parameters within this + ## package, follow these steps: + ## * In the file package.xml: + ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" + ## * In this file (CMakeLists.txt): + ## * add "dynamic_reconfigure" to + ## find_package(catkin REQUIRED COMPONENTS ...) + ## * uncomment the "generate_dynamic_reconfigure_options" section below + ## and list every .cfg file to be processed + + ## Generate dynamic reconfigure parameters in the 'cfg' folder + # generate_dynamic_reconfigure_options( + # cfg/params.cfg + # ) + + ################################### + ## catkin specific configuration ## + ################################### + ## The catkin_package macro generates cmake config files for your package + ## Declare things to be passed to dependent projects + ## INCLUDE_DIRS: uncomment this if your package contains header files + ## LIBRARIES: libraries you create in this project that dependent projects also need + ## CATKIN_DEPENDS: catkin_packages dependent projects also need + ## DEPENDS: system dependencies of this project that dependent projects also need + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + geometry_msgs + nodelet + roscpp + tf2 + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + topic_tools + # DEPENDS + ) + + ########### + ## Build ## + ########### + + ## Specify additional locations of header files + ## Your package locations should be listed before other locations + include_directories( + include + ${catkin_INCLUDE_DIRS} + ) + + ## Declare a C++ library + add_library(${PROJECT_NAME} + src/MessageTfFrameTransformer.cpp + ) + + ## Add cmake target dependencies of the library + ## as an example, code may need to be generated before libraries + ## either from message generation or dynamic reconfigure + # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + + ## Declare a C++ executable + ## With catkin_make all packages are built within a single CMake context + ## The recommended prefix ensures that target names across packages don't collide + add_executable(${PROJECT_NAME}_node src/MessageTfFrameTransformer.node.cpp) + + ## Rename C++ executable without node postfix + set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME ${PROJECT_NAME} PREFIX "") + + ## Add cmake target dependencies of the executable + ## same as for the library above + add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + + ## Specify libraries to link a library or executable target against + target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ) + + ############# + ## Install ## + ############# + + # all install targets should use catkin DESTINATION variables + # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + + ## Mark executable scripts (Python etc.) for installation + ## in contrast to setup.py, you can choose the destination + # install(PROGRAMS + # scripts/my_python_script + # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + # ) + + ## Mark executables and/or libraries for installation + install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + # FILES_MATCHING PATTERN "*.h" + # PATTERN ".svn" EXCLUDE + ) + + ## Mark other files for installation (e.g. launch and bag files, etc.) + install(FILES + nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + install(DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "*ros2*" EXCLUDE + ) + + ############# + ## Testing ## + ############# + + ## Add gtest based cpp test target and link libraries + # catkin_add_gtest(${PROJECT_NAME}-test test/test_hx_testmanager.cpp) + #if(CATKIN_ENABLE_TESTING) + # find_package(rostest REQUIRED) + # add_rostest_gtest(test_ika_dogm test/ika_dogm.test test/UnitTest.cpp) + # catkin_add_gtest(test_ika_dogm test/UnitTest.cpp) + # target_link_libraries(test_ika_dogm ${catkin_LIBRARIES} ${PROJECT_NAME}_dogm_creation) + #endif() + # if(TARGET ${PROJECT_NAME}-test) + # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) + # endif() + + ## Add folders to be run by python nosetests + # catkin_add_nosetests(test) + +endif() diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..dfae5ae --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Institute for Automotive Engineering of RWTH Aachen University + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..5f5acac --- /dev/null +++ b/README.md @@ -0,0 +1,185 @@ +# message_tf_frame_transformer + +

+ + + + + + + +

+ +The *message_tf_frame_transformer* package provides a ROS / ROS 2 node(let) to transform ROS messages of arbitrary type to a different coordinate frame. This can be helpful if you cannot or do not want to modify the source code of other ROS nodes that require your data to be valid in a specific coordinate frame. Simply launch the *message_tf_frame_transformer* node and transform arbitrary ROS message to a target coordinate frame. + +- [Installation](#installation) + - [docker-ros](#docker-ros) +- [Usage](#usage) +- [Supported Message Types](#supported-message-types) +- [Nodes/Nodelets](#nodesnodelets) +- [Acknowledgements](#acknowledgements) + + +## Installation + +The *message_tf_frame_transformer* package is released as an official ROS / ROS 2 package and can easily be installed via a package manager. + +```bash +sudo apt install ros-$ROS_DISTRO-message-tf-frame-transformer +``` + +If you would like to install *message_tf_frame_transformer* from source, simply clone this repository into your ROS workspace. All dependencies that are listed in the ROS [`package.xml`](./package.xml) can be installed using [*rosdep*](http://wiki.ros.org/rosdep). + +```bash +# message_tf_frame_transformer$ +rosdep install -r --ignore-src --from-paths . + +# ROS 2 +# workspace$ +colcon build --packages-up-to message_tf_frame_transformer --cmake-args -DCMAKE_BUILD_TYPE=Release + +# ROS +# workspace$ +catkin build -DCMAKE_BUILD_TYPE=Release message_tf_frame_transformer +``` + +### docker-ros + +*message_tf_frame_transformer* is also available as a Docker image, containerized through [*docker-ros*](https://github.com/ika-rwth-aachen/docker-ros). + +```bash +# ROS +docker run --rm ghcr.io/ika-rwth-aachen/message_tf_frame_transformer:ros + +# ROS 2 +docker run --rm ghcr.io/ika-rwth-aachen/message_tf_frame_transformer:ros2 +``` + + +## Usage + +In order to transform messages on topic `$INPUT_TOPIC` to frame `$TARGET_FRAME_ID` and publish them to topic `$OUTPUT_TOPIC`, the *message_tf_frame_transformer* node can be started with the following topic remappings and parameter setting. Only the `target_frame_id` parameter is required. The `source_frame_id` parameter is only required for non-stamped messages without an [`std_msgs/Header`](https://docs.ros.org/en/api/std_msgs/html/msg/Header.html). The topics default to `~/input` and `~/transformed` in the node's private namespace. + +```bash +# ROS 2 +ros2 run message_tf_frame_transformer message_tf_frame_transformer --ros-args \ + -r \~/input:=$INPUT_TOPIC \ + -r \~/transformed:=$OUTPUT_TOPIC \ + -p source_frame_id:=$SOURCE_FRAME_ID \ + -p target_frame_id:=$TARGET_FRAME_ID + +# ROS +rosrun message_tf_frame_transformer message_tf_frame_transformer \ + ~input:=$INPUT_TOPIC \ + ~transformed:=$OUTPUT_TOPIC \ + _source_frame_id:=$SOURCE_FRAME_ID \ + _target_frame_id:=$TARGET_FRAME_ID +``` + +The provided launch file enables you to directly launch a [`tf2_ros/static_transform_publisher`](http://wiki.ros.org/tf2_ros) alongside the *message_tf_frame_transformer* node. This way you can transform a topic to a new coordinate frame with a single command. + +```bash +# ROS 2 + ros2 launch message_tf_frame_transformer message_tf_frame_transformer.launch.ros2.xml \ + input_topic:=$INPUT_TOPIC \ + output_topic:=$OUTPUT_TOPIC \ + source_frame_id:=$SOURCE_FRAME_ID \ + target_frame_id:=$TARGET_FRAME_ID \ + x:=$X \ + y:=$Y \ + z:=$Z \ + roll:=$ROLL \ + pitch:=$PITCH \ + yaw:=$YAW + +# ROS +roslaunch message_tf_frame_transformer message_tf_frame_transformer.launch \ + input_topic:=$INPUT_TOPIC \ + output_topic:=$OUTPUT_TOPIC \ + source_frame_id:=$SOURCE_FRAME_ID \ + target_frame_id:=$TARGET_FRAME_ID \ + xyzrpy:="$X $Y $Z $ROLL $PITCH $YAW" +``` + + +## Supported Message Types + +The *message_tf_frame_transformer* package is able to support any ROS message type that integrates with [`tf2::doTransform`](http://wiki.ros.org/tf2/Tutorials/Transforming%20your%20own%20datatypes). Currently, the following message types are explicitly supported. + +| ROS | ROS 2 | Remarks | +| --- | --- | --- | +| [`geometry_msgs/Point`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Point.html) | [`geometry_msgs/msg/Point`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/Point.html) | | +| [`geometry_msgs/PointStamped`](https://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html) | [`geometry_msgs/msg/PointStamped`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/PointStamped.html) | | +| [`geometry_msgs/Pose`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Pose.html) | [`geometry_msgs/msg/Pose`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/Pose.html) | | +| [`geometry_msgs/PoseStamped`](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html) | [`geometry_msgs/msg/PoseStamped`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/PoseStamped.html) | | +| [`geometry_msgs/PoseWithCovariance`](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseWithCovariance.html) | [`geometry_msgs/msg/PoseWithCovariance`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/PoseWithCovariance.html) | | +| [`geometry_msgs/PoseWithCovarianceStamped`](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html) | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/PoseWithCovarianceStamped.html) | | +| [`geometry_msgs/Quaternion`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Quaternion.html) | [`geometry_msgs/msg/Quaternion`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/Quaternion.html) | | +| [`geometry_msgs/QuaternionStamped`](https://docs.ros.org/en/api/geometry_msgs/html/msg/QuaternionStamped.html) | [`geometry_msgs/msg/QuaternionStamped`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/QuaternionStamped.html) | | +| [`geometry_msgs/Transform`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Transform.html) | [`geometry_msgs/msg/Transform`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/Transform.html) | | +| [`geometry_msgs/TransformStamped`](https://docs.ros.org/en/api/geometry_msgs/html/msg/TransformStamped.html) | [`geometry_msgs/msg/TransformStamped`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/TransformStamped.html) | | +| [`geometry_msgs/Vector3`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Vector3.html) | [`geometry_msgs/msg/Vector3`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/Vector3.html) | | +| [`geometry_msgs/Vector3Stamped`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Vector3Stamped.html) | [`geometry_msgs/msg/Vector3Stamped`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/Vector3Stamped.html) | | +| [`geometry_msgs/Wrench`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Wrench.html) | [`geometry_msgs/msg/Wrench`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/Wrench.html) | | +| [`geometry_msgs/WrenchStamped`](https://docs.ros.org/en/api/geometry_msgs/html/msg/WrenchStamped.html) | [`geometry_msgs/msg/WrenchStamped`](https://docs.ros2.org/foxy/api/geometry_msgs/msg/WrenchStamped.html) | | +| [`sensor_msgs/PointCloud2`](http://docs.ros.org/en/api/sensor_msgs/html/msg/PointCloud2.html) | [`sensor_msgs/msg/PointCloud2`](https://docs.ros2.org/foxy/api/sensor_msgs/msg/PointCloud.html) | Only the first three point cloud channels (usually `xyz`) are transformed. | + +### Adding Support for a New Message Type + +Through application of preprocessor macros, adding support for a new ROS message type is as easy as adding only two lines of code. Note that the ROS message types have to integrate with [`tf2::doTransform`](http://wiki.ros.org/tf2/Tutorials/Transforming%20your%20own%20datatypes). Feel free to open a pull request to add support for more message types! + +1. [`message_types.h`](./include/message_tf_frame_transformer/message_types.h) (ROS) / [`message_types.ros2.hpp`](./include/message_tf_frame_transformer/message_types.ros2.hpp) (ROS 2) + - include required message headers +1. [`message_types.macro`](./include/message_tf_frame_transformer/message_types.h) (ROS) / [`message_types.ros2.macro`](./include/message_tf_frame_transformer/message_types.ros2.hpp) (ROS 2) + - define information about the new message type by calling the `MESSAGE_TYPE` macro + - `TYPE`: ROS message type (e.g. `geometry_msgs::msg::PointStamped`) + - `NAME`: *(ROS 2 only)* ROS message type name (e.g. `geometry_msgs/msg/PointStamped`) + + +## Nodes/Nodelets + +##### ROS 2 + +| Package | Node | Description | +| --- | --- | --- | +| `message_tf_frame_transformer` | `message_tf_frame_transformer` | transform arbitrary ROS messages to a different coordinate frame | + +##### ROS + +| Package | Node | Nodelet | Description | +| --- | --- | --- | --- | +| `message_tf_frame_transformer` | `message_tf_frame_transformer` | `MessageTfFrameTransformer` | transform arbitrary ROS messages to a different coordinate frame | + +### message_tf_frame_transformer/message_tf_frame_transformer + +#### Subscribed Topics + +| Topic | Type | Description | +| --- | --- | --- | +| `~/input` | see [Supported Message Types](#supported-message-types) | message to transform | + +#### Published Topics + +| Topic | Type | Description | +| --- | --- | --- | +| `~/transformed` | see [Supported Message Types](#supported-message-types) | transformed message | + +#### Services + +\- + +#### Actions + +\- + +#### Parameters + +| Parameter | Type | Description | +| --- | --- | --- | +| `~/target_frame_id` | `string` | target frame ID | +| `~/source_frame_id` | `string` | source frame ID (optional; if message has no [`std_msgs/Header`](https://docs.ros.org/en/api/std_msgs/html/msg/Header.html)) | + + +## Acknowledgements + +This research is accomplished within the project [6GEM](https://6gem.de/) (FKZ 16KISK036K). We acknowledge the financial support for the project by the Federal Ministry of Education and Research of Germany (BMBF). diff --git a/include/message_tf_frame_transformer/MessageTfFrameTransformer.h b/include/message_tf_frame_transformer/MessageTfFrameTransformer.h new file mode 100644 index 0000000..cce1f91 --- /dev/null +++ b/include/message_tf_frame_transformer/MessageTfFrameTransformer.h @@ -0,0 +1,154 @@ +/* +============================================================================== +MIT License + +Copyright 2023 Institute for Automotive Engineering of RWTH Aachen University. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================== +*/ + + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + + +namespace message_tf_frame_transformer { + + +// definitions for distinguishing between ROS messages with/without header +// based on SFINAE (https://fekir.info/post/detect-member-variables/) +using HasRosHeaderNo = std::false_type; +using HasRosHeaderYes = std::true_type; + +template +struct HasRosHeader : HasRosHeaderNo {}; + +template +struct HasRosHeader : HasRosHeaderYes {}; + + +class MessageTfFrameTransformer : public nodelet::Nodelet { + + protected: + + virtual void onInit() override; + + void loadParameters(); + + void setup(); + + void detectMessageType(const topic_tools::ShapeShifter::ConstPtr& generic_msg); + + template + void transform(const typename T::ConstPtr& msg); + + template + void transform(const typename T::ConstPtr& msg, const HasRosHeaderYes&); + + template + void transform(const typename T::ConstPtr& msg, const HasRosHeaderNo&); + + protected: + + static const std::string kInputTopic; + + static const std::string kOutputTopic; + + static const std::string kSourceFrameIdParam; + + static const std::string kTargetFrameIdParam; + + std::string source_frame_id_; + + std::string target_frame_id_; + + ros::NodeHandle node_handle_; + + ros::NodeHandle private_node_handle_; + + tf2_ros::Buffer tf_buffer_; + + std::shared_ptr tf_listener_; + + ros::Subscriber subscriber_; + + ros::Publisher publisher_; +}; + + +template +void MessageTfFrameTransformer::transform(const typename T::ConstPtr& msg) { + this->transform(msg, HasRosHeader()); +} + +template +void MessageTfFrameTransformer::transform(const typename T::ConstPtr& msg, const HasRosHeaderYes&) { + + // transform + T tf_msg; + try { + tf_buffer_.transform(*msg, tf_msg, target_frame_id_); + } catch (tf2::LookupException &e) { + NODELET_ERROR("Failed to lookup transform from '%s' to '%s': %s", msg->header.frame_id.c_str(), target_frame_id_.c_str(), e.what()); + return; + } + + // publish + NODELET_DEBUG("Publishing data transformed from '%s' to '%s'", msg->header.frame_id.c_str(), target_frame_id_.c_str()); + publisher_.publish(tf_msg); +} + +template +void MessageTfFrameTransformer::transform(const typename T::ConstPtr& msg, const HasRosHeaderNo&) { + + if (source_frame_id_.empty()) { + NODELET_ERROR("Transforming messages without an 'std_msgs/Header' requires the '%s' parameter to be set", kSourceFrameIdParam.c_str()); + return; + } + + // lookup transform + geometry_msgs::TransformStamped tf; + try { + tf = tf_buffer_.lookupTransform(target_frame_id_, source_frame_id_, ros::Time(0)); + } catch (tf2::LookupException &e) { + NODELET_ERROR("Failed to lookup transform from '%s' to '%s': %s", source_frame_id_.c_str(), target_frame_id_.c_str(), e.what()); + return; + } + + // transform + T tf_msg; + tf2::doTransform(*msg, tf_msg, tf); + + // publish + NODELET_DEBUG("Publishing data transformed from '%s' to '%s'", source_frame_id_.c_str(), target_frame_id_.c_str()); + publisher_.publish(tf_msg); +} + + +} // namespace message_tf_frame_transformer diff --git a/include/message_tf_frame_transformer/MessageTfFrameTransformer.ros2.hpp b/include/message_tf_frame_transformer/MessageTfFrameTransformer.ros2.hpp new file mode 100644 index 0000000..9119171 --- /dev/null +++ b/include/message_tf_frame_transformer/MessageTfFrameTransformer.ros2.hpp @@ -0,0 +1,172 @@ +/* +============================================================================== +MIT License + +Copyright 2023 Institute for Automotive Engineering of RWTH Aachen University. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================== +*/ + + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + + +namespace message_tf_frame_transformer { + + +// definitions for distinguishing between ROS messages with/without header +// based on SFINAE (https://fekir.info/post/detect-member-variables/) +using HasRosHeaderNo = std::false_type; +using HasRosHeaderYes = std::true_type; + +template +struct HasRosHeader : HasRosHeaderNo {}; + +template +struct HasRosHeader : HasRosHeaderYes {}; + + +class MessageTfFrameTransformer : public rclcpp::Node { + + public: + + MessageTfFrameTransformer(); + + protected: + + void loadParameters(); + + void setup(); + + void detectMessageType(); + + void transformGeneric(const std::shared_ptr& serialized_msg); + + template + void transform(const T& msg); + + template + void transform(const T& msg, const HasRosHeaderYes&); + + template + void transform(const T& msg, const HasRosHeaderNo&); + + protected: + + static const std::string kInputTopic; + + static const std::string kOutputTopic; + + static const std::string kSourceFrameIdParam; + + static const std::string kTargetFrameIdParam; + + std::string source_frame_id_; + + std::string target_frame_id_; + + std::unique_ptr tf_buffer_; + + std::shared_ptr tf_listener_; + + rclcpp::TimerBase::SharedPtr detect_message_type_timer_; + + rclcpp::GenericSubscription::SharedPtr subscriber_; + + rclcpp::PublisherBase::SharedPtr publisher_; + + std::string msg_type_; +}; + + +template +void MessageTfFrameTransformer::transform(const T& msg) { + this->transform(msg, HasRosHeader()); +} + +template +void MessageTfFrameTransformer::transform(const T& msg, const HasRosHeaderYes&) { + + // transform + T tf_msg; + try { + tf_buffer_->transform(msg, tf_msg, target_frame_id_); + } catch (tf2::LookupException &e) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to lookup transform from '%s' to '%s': %s", msg.header.frame_id.c_str(), target_frame_id_.c_str(), e.what() + ); + return; + } + + // publish + RCLCPP_DEBUG( + this->get_logger(), + "Publishing data transformed from '%s' to '%s'", msg.header.frame_id.c_str(), target_frame_id_.c_str() + ); + std::static_pointer_cast>(publisher_)->publish(tf_msg); +} + +template +void MessageTfFrameTransformer::transform(const T& msg, const HasRosHeaderNo&) { + + if (source_frame_id_.empty()) { + RCLCPP_ERROR( + this->get_logger(), + "Transforming messages without an 'std_msgs/Header' requires the '%s' parameter to be set", kSourceFrameIdParam.c_str() + ); + return; + } + + // lookup transform + geometry_msgs::msg::TransformStamped tf; + try { + tf = tf_buffer_->lookupTransform(target_frame_id_, source_frame_id_, tf2::TimePointZero); + } catch (tf2::LookupException &e) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to lookup transform from '%s' to '%s': %s", source_frame_id_.c_str(), target_frame_id_.c_str(), e.what() + ); + return; + } + + // transform + T tf_msg; + tf2::doTransform(msg, tf_msg, tf); + + // publish + RCLCPP_DEBUG( + this->get_logger(), + "Publishing data transformed from '%s' to '%s'", source_frame_id_.c_str(), target_frame_id_.c_str() + ); + std::static_pointer_cast>(publisher_)->publish(tf_msg); +} + + +} // namespace message_tf_frame_transformer diff --git a/include/message_tf_frame_transformer/message_types.h b/include/message_tf_frame_transformer/message_types.h new file mode 100644 index 0000000..bc2547d --- /dev/null +++ b/include/message_tf_frame_transformer/message_types.h @@ -0,0 +1,13 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include diff --git a/include/message_tf_frame_transformer/message_types.macro b/include/message_tf_frame_transformer/message_types.macro new file mode 100644 index 0000000..740b7ca --- /dev/null +++ b/include/message_tf_frame_transformer/message_types.macro @@ -0,0 +1,31 @@ +/* ============================================================================= +The lines below represent calls to a macro named MESSAGE_TYPE. +Wherever needed, this macro may be defined and called for all supported +message types by including this file. + +Example: + +#define MESSAGE_TYPE(TYPE) \ + std::cout << "The ROS message type " << #TYPE << " is supported. "; \ + std::cout << std::endl; +#include "message_tf_frame_transformer/message_types.macro" +#undef MESSAGE_TYPE + +============================================================================= */ + +MESSAGE_TYPE(geometry_msgs::Point) +MESSAGE_TYPE(geometry_msgs::PointStamped) +MESSAGE_TYPE(geometry_msgs::Pose) +MESSAGE_TYPE(geometry_msgs::PoseStamped) +MESSAGE_TYPE(geometry_msgs::PoseWithCovariance) +MESSAGE_TYPE(geometry_msgs::PoseWithCovarianceStamped) +MESSAGE_TYPE(geometry_msgs::Quaternion) +MESSAGE_TYPE(geometry_msgs::QuaternionStamped) +MESSAGE_TYPE(geometry_msgs::Transform) +MESSAGE_TYPE(geometry_msgs::TransformStamped) +MESSAGE_TYPE(geometry_msgs::Vector3) +MESSAGE_TYPE(geometry_msgs::Vector3Stamped) +MESSAGE_TYPE(geometry_msgs::Wrench) +MESSAGE_TYPE(geometry_msgs::WrenchStamped) + +MESSAGE_TYPE(sensor_msgs::PointCloud2) diff --git a/include/message_tf_frame_transformer/message_types.ros2.hpp b/include/message_tf_frame_transformer/message_types.ros2.hpp new file mode 100644 index 0000000..d477786 --- /dev/null +++ b/include/message_tf_frame_transformer/message_types.ros2.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include diff --git a/include/message_tf_frame_transformer/message_types.ros2.macro b/include/message_tf_frame_transformer/message_types.ros2.macro new file mode 100644 index 0000000..26ec9d1 --- /dev/null +++ b/include/message_tf_frame_transformer/message_types.ros2.macro @@ -0,0 +1,31 @@ +/* ============================================================================= +The lines below represent calls to a macro named MESSAGE_TYPE. +Wherever needed, this macro may be defined and called for all supported +message types by including this file. + +Example: + +#define MESSAGE_TYPE(TYPE, NAME) \ + std::cout << "The ROS message type " << #TYPE << " is supported. "; \ + std::cout << std::endl; +#include "message_tf_frame_transformer/message_types.ros2.macro" +#undef MESSAGE_TYPE + +============================================================================= */ + +MESSAGE_TYPE(geometry_msgs::msg::Point, geometry_msgs/msg/Point) +MESSAGE_TYPE(geometry_msgs::msg::PointStamped, geometry_msgs/msg/PointStamped) +MESSAGE_TYPE(geometry_msgs::msg::Pose, geometry_msgs/msg/Pose) +MESSAGE_TYPE(geometry_msgs::msg::PoseStamped, geometry_msgs/msg/PoseStamped) +MESSAGE_TYPE(geometry_msgs::msg::PoseWithCovariance, geometry_msgs/msg/PoseWithCovariance) +MESSAGE_TYPE(geometry_msgs::msg::PoseWithCovarianceStamped, geometry_msgs/msg/PoseWithCovarianceStamped) +MESSAGE_TYPE(geometry_msgs::msg::Quaternion, geometry_msgs/msg/Quaternion) +MESSAGE_TYPE(geometry_msgs::msg::QuaternionStamped, geometry_msgs/msg/QuaternionStamped) +MESSAGE_TYPE(geometry_msgs::msg::Transform, geometry_msgs/msg/Transform) +MESSAGE_TYPE(geometry_msgs::msg::TransformStamped, geometry_msgs/msg/TransformStamped) +MESSAGE_TYPE(geometry_msgs::msg::Vector3, geometry_msgs/msg/Vector3) +MESSAGE_TYPE(geometry_msgs::msg::Vector3Stamped, geometry_msgs/msg/Vector3Stamped) +MESSAGE_TYPE(geometry_msgs::msg::Wrench, geometry_msgs/msg/Wrench) +MESSAGE_TYPE(geometry_msgs::msg::WrenchStamped, geometry_msgs/msg/WrenchStamped) + +MESSAGE_TYPE(sensor_msgs::msg::PointCloud2, sensor_msgs/msg/PointCloud2) diff --git a/launch/message_tf_frame_transformer.launch b/launch/message_tf_frame_transformer.launch new file mode 100644 index 0000000..e739e90 --- /dev/null +++ b/launch/message_tf_frame_transformer.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/launch/message_tf_frame_transformer.launch.ros2.xml b/launch/message_tf_frame_transformer.launch.ros2.xml new file mode 100644 index 0000000..9f6416a --- /dev/null +++ b/launch/message_tf_frame_transformer.launch.ros2.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml new file mode 100644 index 0000000..3345178 --- /dev/null +++ b/nodelet_plugins.xml @@ -0,0 +1,7 @@ + + + + message_tf_frame_transformer nodelet + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..197dd16 --- /dev/null +++ b/package.xml @@ -0,0 +1,37 @@ + + + + message_tf_frame_transformer + 1.1.1 + Transforms messages of arbitrary type to a different frame using tf2::doTransform + + Lennart Reiher + Lennart Reiher + + MIT + + geometry_msgs + ros_environment + sensor_msgs + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + tf2 + + + ament_cmake + rclcpp + + + catkin + nodelet + roscpp + topic_tools + + + catkin + ament_cmake + + + + diff --git a/src/MessageTfFrameTransformer.cpp b/src/MessageTfFrameTransformer.cpp new file mode 100644 index 0000000..7f6f1fd --- /dev/null +++ b/src/MessageTfFrameTransformer.cpp @@ -0,0 +1,123 @@ +/* +============================================================================== +MIT License + +Copyright 2023 Institute for Automotive Engineering of RWTH Aachen University. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================== +*/ + + +#include +#include +#include +#include + + +PLUGINLIB_EXPORT_CLASS(message_tf_frame_transformer::MessageTfFrameTransformer, nodelet::Nodelet) + + +namespace message_tf_frame_transformer { + + +const std::string MessageTfFrameTransformer::kInputTopic = "input"; + +const std::string MessageTfFrameTransformer::kOutputTopic = "transformed"; + +const std::string MessageTfFrameTransformer::kSourceFrameIdParam = "source_frame_id"; + +const std::string MessageTfFrameTransformer::kTargetFrameIdParam = "target_frame_id"; + + +void MessageTfFrameTransformer::onInit() { + + node_handle_ = this->getMTNodeHandle(); + private_node_handle_ = this->getMTPrivateNodeHandle(); + + loadParameters(); + setup(); +} + + +void MessageTfFrameTransformer::loadParameters() { + + private_node_handle_.getParam(kSourceFrameIdParam, source_frame_id_); + + bool found = private_node_handle_.getParam(kTargetFrameIdParam, target_frame_id_); + if (!found) { + NODELET_FATAL("Parameter '%s' is required", kTargetFrameIdParam.c_str()); + exit(EXIT_FAILURE); + } + + NODELET_INFO("Transforming data on topic '%s' to frame '%s' published on topic '%s'", + ros::names::resolve("~" + kInputTopic).c_str(), target_frame_id_.c_str(), + ros::names::resolve("~" + kOutputTopic).c_str()); +} + + +void MessageTfFrameTransformer::setup() { + + // listen to tf + tf_listener_ = std::make_shared(tf_buffer_); + + // setup subscriber to detect message type + subscriber_ = private_node_handle_.subscribe(kInputTopic, 10, &MessageTfFrameTransformer::detectMessageType, this); +} + + +void MessageTfFrameTransformer::detectMessageType(const topic_tools::ShapeShifter::ConstPtr& generic_msg) { + + const std::string msg_type = generic_msg->getDataType(); + const std::string& msg_type_md5 = generic_msg->getMD5Sum(); + NODELET_DEBUG("Detected message type '%s'", msg_type.c_str()); + + // detect message type based on md5 hash + if (false) {} +#define MESSAGE_TYPE(TYPE) \ + else if (msg_type_md5 == ros::message_traits::MD5Sum::value()) { \ + \ + /* instantiate generic message as message of concrete type */ \ + TYPE::ConstPtr msg = generic_msg->instantiate(); \ + \ + /* setup publisher and pass message to transform callback */ \ + publisher_ = private_node_handle_.advertise(kOutputTopic, 10); \ + this->transform(msg); \ + \ + /* re-initialize concrete subscriber */ \ + subscriber_.shutdown(); \ + subscriber_ = private_node_handle_.subscribe( \ + kInputTopic, \ + 10, \ + &MessageTfFrameTransformer::transform, \ + this \ + ); \ + \ + } +#include "message_tf_frame_transformer/message_types.macro" +#undef MESSAGE_TYPE + else { + NODELET_ERROR("Transforming message type '%s' is not supported", + msg_type.c_str()); + return; + } +} + + +} // namespace message_tf_frame_transformer diff --git a/src/MessageTfFrameTransformer.node.cpp b/src/MessageTfFrameTransformer.node.cpp new file mode 100644 index 0000000..7de2735 --- /dev/null +++ b/src/MessageTfFrameTransformer.node.cpp @@ -0,0 +1,44 @@ +/* +============================================================================== +MIT License + +Copyright 2023 Institute for Automotive Engineering of RWTH Aachen University. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================== +*/ + + +#include +#include + +#include + + +int main(int argc, char** argv) { + + ros::init(argc, argv, "message_tf_frame_transformer"); + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + nodelet::V_string nargv; + std::string nodelet_name = ros::this_node::getName(); + nodelet.load(nodelet_name, "message_tf_frame_transformer/MessageTfFrameTransformer", remap, nargv); + ros::spin(); + return 0; +} diff --git a/src/MessageTfFrameTransformer.ros2.cpp b/src/MessageTfFrameTransformer.ros2.cpp new file mode 100644 index 0000000..894b0ce --- /dev/null +++ b/src/MessageTfFrameTransformer.ros2.cpp @@ -0,0 +1,170 @@ +/* +============================================================================== +MIT License + +Copyright 2023 Institute for Automotive Engineering of RWTH Aachen University. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================== +*/ + + +#include + +#include +#include + + +namespace message_tf_frame_transformer { + + +const std::string MessageTfFrameTransformer::kInputTopic = "~/input"; + +const std::string MessageTfFrameTransformer::kOutputTopic = "~/transformed"; + +const std::string MessageTfFrameTransformer::kSourceFrameIdParam = "source_frame_id"; + +const std::string MessageTfFrameTransformer::kTargetFrameIdParam = "target_frame_id"; + + +MessageTfFrameTransformer::MessageTfFrameTransformer() : Node("message_tf_frame_transformer") { + + loadParameters(); + setup(); +} + + +void MessageTfFrameTransformer::loadParameters() { + + rcl_interfaces::msg::ParameterDescriptor source_frame_id_param_desc; + source_frame_id_param_desc.description = "Source frame ID to transform from (optional; if message has no std_msgs/Header)"; + this->declare_parameter(kSourceFrameIdParam, rclcpp::ParameterType::PARAMETER_STRING, source_frame_id_param_desc); + + try { + source_frame_id_ = this->get_parameter(kSourceFrameIdParam).as_string(); + } catch (rclcpp::exceptions::ParameterUninitializedException&) {} + + rcl_interfaces::msg::ParameterDescriptor target_frame_id_param_desc; + target_frame_id_param_desc.description = "Target frame ID to transform to"; + this->declare_parameter(kTargetFrameIdParam, rclcpp::ParameterType::PARAMETER_STRING, target_frame_id_param_desc); + + try { + target_frame_id_ = this->get_parameter(kTargetFrameIdParam).as_string(); + } catch (rclcpp::exceptions::ParameterUninitializedException&) { + RCLCPP_FATAL(get_logger(), "Parameter '%s' is required", kTargetFrameIdParam.c_str()); + exit(EXIT_FAILURE); + } + + RCLCPP_INFO( + this->get_logger(), + "Transforming data on topic '%s' to frame '%s' published on topic '%s'", + this->get_node_topics_interface()->resolve_topic_name(kInputTopic).c_str(), + target_frame_id_.c_str(), + this->get_node_topics_interface()->resolve_topic_name(kOutputTopic).c_str() + ); +} + + +void MessageTfFrameTransformer::setup() { + + // listen to tf + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // setup timer to detect subscription type and then subscribe + detect_message_type_timer_ = + create_wall_timer(std::chrono::duration(0.001), + std::bind(&MessageTfFrameTransformer::detectMessageType, this)); +} + + +void MessageTfFrameTransformer::detectMessageType() { + + // check if topic to subscribe exists + std::string resolved_input_topic = this->get_node_topics_interface()->resolve_topic_name(kInputTopic); + const auto all_topics_and_types = get_topic_names_and_types(); + if (all_topics_and_types.count(resolved_input_topic)) { + + detect_message_type_timer_->cancel(); + msg_type_ = all_topics_and_types.at(resolved_input_topic)[0]; + RCLCPP_DEBUG(this->get_logger(), "Detected message type '%s'", msg_type_.c_str()); + + // setup publisher with correct message type + if (false) {} +#define MESSAGE_TYPE(TYPE, NAME) \ + else if (msg_type_ == #NAME) { \ + publisher_ = this->create_publisher(kOutputTopic, 10); \ + } +#include "message_tf_frame_transformer/message_types.ros2.macro" +#undef MESSAGE_TYPE + else { + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Transforming message type '%s' is not supported", + msg_type_.c_str()); + detect_message_type_timer_->reset(); + return; + } + + // setup generic subscriber with correct message type (will have missed at least one message) + subscriber_ = this->create_generic_subscription( + kInputTopic, + msg_type_, + 10, + std::bind(&MessageTfFrameTransformer::transformGeneric, this, std::placeholders::_1) + ); + + RCLCPP_WARN(this->get_logger(), + "Subscribed to message type '%s' on topic '%s'; first message may have been missed", + msg_type_.c_str(), + subscriber_->get_topic_name() + ); + } +} + + +void MessageTfFrameTransformer::transformGeneric(const std::shared_ptr& serialized_msg) { + + if (false) {} +#define MESSAGE_TYPE(TYPE, NAME) \ + else if (msg_type_ == #NAME) { \ + \ + /* instantiate generic message as message of concrete type */ \ + TYPE msg; \ + rclcpp::Serialization serializer; \ + serializer.deserialize_message(serialized_msg.get(), &msg); \ + \ + /* pass message to transform callback */ \ + this->transform(msg); \ + } +#include "message_tf_frame_transformer/message_types.ros2.macro" +#undef MESSAGE_TYPE +} + + +} // namespace message_tf_frame_transformer + + +int main(int argc, char *argv[]) { + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +}