├── docker
    └── blacklisted-packages.txt
├── .github
    └── workflows
    │   ├── industrial_ci.yml
    │   ├── bloom-release.yml
    │   └── docker-ros.yml
├── package.xml
├── LICENSE
├── launch
    └── message_tf_frame_transformer.launch.xml
├── CMakeLists.txt
├── CHANGELOG.rst
├── include
    └── message_tf_frame_transformer
    │   ├── message_types.hpp
    │   ├── message_types.macro
    │   └── MessageTfFrameTransformer.hpp
├── src
    └── message_tf_frame_transformer.cpp
└── README.md


/docker/blacklisted-packages.txt:
--------------------------------------------------------------------------------
1 | perception_msgs_rendering
2 | perception_msgs_rviz_plugins
3 | route_planning_msgs_rviz_plugins
4 | trajectory_planning_msgs_rviz_plugins


--------------------------------------------------------------------------------
/.github/workflows/industrial_ci.yml:
--------------------------------------------------------------------------------
 1 | name: industrial_ci
 2 | 
 3 | on:
 4 |   push:
 5 |     branches:
 6 |       - main
 7 |   pull_request:
 8 |     branches:
 9 |       - main
10 | 
11 | jobs:
12 |   industrial_ci:
13 |     name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }})
14 |     runs-on: ubuntu-latest
15 |     strategy:
16 |       matrix:
17 |         ROS_DISTRO:
18 |           - humble
19 |           - jazzy
20 |           - kilted
21 |           - rolling
22 |         ROS_REPO:
23 |           - main
24 |     steps:
25 |       - uses: actions/checkout@v3
26 |       - uses: ros-industrial/industrial_ci@master
27 |         with:
28 |           config: ${{ toJSON(matrix) }}
29 | 


--------------------------------------------------------------------------------
/.github/workflows/bloom-release.yml:
--------------------------------------------------------------------------------
 1 | name: bloom-release
 2 | 
 3 | on:
 4 |   push:
 5 |     tags:
 6 |       - 'v[0-9]+.[0-9]+.[0-9]+'
 7 | 
 8 | jobs:
 9 |   bloom-release:
10 |     runs-on: ubuntu-latest
11 |     steps:
12 |       - name: Release ROS 2
13 |         uses: at-wat/bloom-release-action@v0
14 |         with:
15 |           ros_distro: humble jazzy kilted rolling
16 |           github_token_bloom: ${{ secrets.GH_TOKEN_FOR_BLOOM_RELEASE }}
17 |           github_user: lreiher
18 |           git_user: Lennart Reiher
19 |           git_email: lennart.reiher@ika.rwth-aachen.de
20 |           release_repository_push_url: https://github.com/ros2-gbp/message_tf_frame_transformer-release.git
21 |           open_pr: false
22 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="3">
 3 |   <name>message_tf_frame_transformer</name>
 4 |   <version>1.1.3</version>
 5 |   <description>Transforms messages of arbitrary type to a different frame using tf2::doTransform</description>
 6 | 
 7 |   <maintainer email="lennart.reiher@rwth-aachen.de">Lennart Reiher</maintainer>
 8 |   <author email="lennart.reiher@rwth-aachen.de">Lennart Reiher</author>
 9 | 
10 |   <license>MIT</license>
11 | 
12 |   <buildtool_depend>ament_cmake</buildtool_depend>
13 | 
14 |   <depend>geometry_msgs</depend>
15 |   <depend>rclcpp</depend>
16 |   <depend>sensor_msgs</depend>
17 |   <depend>tf2_geometry_msgs</depend>
18 |   <depend>tf2_ros</depend>
19 |   <depend>tf2_sensor_msgs</depend>
20 |   <depend>tf2</depend>
21 | 
22 |   <export>
23 |     <build_type>ament_cmake</build_type>
24 |   </export>
25 | </package>
26 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2023 Institute for Automotive Engineering of RWTH Aachen University
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/launch/message_tf_frame_transformer.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <arg name="input_topic" default="~/input" />
 4 |   <arg name="output_topic" default="~/transformed" />
 5 |   <arg name="source_frame_id" />
 6 |   <arg name="target_frame_id" />
 7 |   <arg name="x" default="0.0" />
 8 |   <arg name="y" default="0.0" />
 9 |   <arg name="z" default="0.0" />
10 |   <arg name="roll" default="0.0" />
11 |   <arg name="pitch" default="0.0" />
12 |   <arg name="yaw" default="0.0" />
13 | 
14 |   <node pkg="tf2_ros" exec="static_transform_publisher" name="static_transform_publisher" args="--x $(var x) --y $(var y) --z $(var z) --roll $(var roll) --pitch $(var pitch) --yaw $(var yaw) --frame-id $(var source_frame_id) --child-frame-id $(var target_frame_id)" output="screen" />
15 | 
16 |   <node pkg="message_tf_frame_transformer" exec="message_tf_frame_transformer" name="message_tf_frame_transformer" output="screen">
17 |     <remap from="~/input" to="$(var input_topic)" />
18 |     <remap from="~/transformed" to="$(var output_topic)" />
19 |     <param name="source_frame_id" value="$(var source_frame_id)" />
20 |     <param name="target_frame_id" value="$(var target_frame_id)" />
21 |   </node>
22 | 
23 | </launch>
24 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.12.0 FATAL_ERROR)
 2 | project(message_tf_frame_transformer)
 3 | 
 4 | ## Compile as C++17
 5 | add_compile_options(-std=c++17)
 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 7 |   add_compile_options(-Wall -Wextra -Wpedantic)
 8 | endif()
 9 | 
10 | find_package(ament_cmake REQUIRED)
11 | find_package(geometry_msgs REQUIRED)
12 | find_package(rclcpp REQUIRED)
13 | find_package(sensor_msgs REQUIRED)
14 | find_package(tf2 REQUIRED)
15 | find_package(tf2_geometry_msgs REQUIRED)
16 | find_package(tf2_ros REQUIRED)
17 | find_package(tf2_sensor_msgs REQUIRED)
18 | 
19 | add_executable(${PROJECT_NAME} src/message_tf_frame_transformer.cpp)
20 | 
21 | target_include_directories(${PROJECT_NAME} PUBLIC
22 |   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
23 |   $<INSTALL_INTERFACE:include>)
24 | 
25 | ament_target_dependencies(${PROJECT_NAME}
26 |   geometry_msgs
27 |   rclcpp
28 |   sensor_msgs
29 |   tf2
30 |   tf2_geometry_msgs
31 |   tf2_ros
32 |   tf2_sensor_msgs
33 | )
34 | 
35 | install(TARGETS
36 |   ${PROJECT_NAME}
37 |   DESTINATION lib/${PROJECT_NAME}
38 | )
39 | 
40 | install(
41 |   DIRECTORY launch
42 |   DESTINATION share/${PROJECT_NAME}
43 | )
44 | 
45 | ament_package()
46 | 


--------------------------------------------------------------------------------
/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package message_tf_frame_transformer
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 1.1.3 (2025-08-18)
 6 | ------------------
 7 | * Merge pull request `#6 <https://github.com/ika-rwth-aachen/message_tf_frame_transformer/issues/6>`_ from ika-rwth-aachen/codex/find-common-message-packages-on-ros-index
 8 |   Remove perception_interfaces and planning_interfaces; add all geometry_msgs; improve CI
 9 | * Merge pull request `#5 <https://github.com/ika-rwth-aachen/message_tf_frame_transformer/issues/5>`_ from ika-rwth-aachen/codex/deprecate-ros-1-noetic-and-update-ros-2-support
10 | * Merge branch 'main' into codex/deprecate-ros-1-noetic-and-update-ros-2-support
11 | * Merge pull request `#4 <https://github.com/ika-rwth-aachen/message_tf_frame_transformer/issues/4>`_ from ika-rwth-aachen/add-ika-interfaces
12 |   Add interfaces "perception_interfaces" and "planning_interfaces"
13 | * Contributors: Lennart Reiher
14 | 
15 | 1.1.2 (2024-06-06)
16 | ------------------
17 | 
18 | 1.1.1 (2024-03-19)
19 | ------------------
20 | * Merge pull request #3 from clalancette/clalancette/fix-compile-error
21 |   Fix a compile error with modern rclcpp.
22 | * Contributors: Lennart Reiher
23 | 
24 | 1.1.0 (2023-06-10)
25 | ------------------
26 | * integrate docker-ros
27 | * add support for ros2 iron
28 | * add remark for pointcloud2
29 |   closes #1
30 | * Contributors: Lennart Reiher
31 | 


--------------------------------------------------------------------------------
/include/message_tf_frame_transformer/message_types.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 4 | // #include <geometry_msgs/msg/accel.hpp>
 5 | // #include <geometry_msgs/msg/accel_stamped.hpp>
 6 | // #include <geometry_msgs/msg/accel_with_covariance.hpp>
 7 | // #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 8 | // #include <geometry_msgs/msg/inertia.hpp>
 9 | // #include <geometry_msgs/msg/inertia_stamped.hpp>
10 | #include <geometry_msgs/msg/point.hpp>
11 | #include <geometry_msgs/msg/point32.hpp>
12 | #include <geometry_msgs/msg/point_stamped.hpp>
13 | #include <geometry_msgs/msg/polygon.hpp>
14 | // #include <geometry_msgs/msg/polygon_instance.hpp>
15 | // #include <geometry_msgs/msg/polygon_instance_stamped.hpp>
16 | #include <geometry_msgs/msg/polygon_stamped.hpp>
17 | #include <geometry_msgs/msg/pose.hpp>
18 | // #include <geometry_msgs/msg/pose2_d.hpp>
19 | // #include <geometry_msgs/msg/pose_array.hpp>
20 | #include <geometry_msgs/msg/pose_stamped.hpp>
21 | #include <geometry_msgs/msg/pose_with_covariance.hpp>
22 | #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
23 | #include <geometry_msgs/msg/quaternion.hpp>
24 | #include <geometry_msgs/msg/quaternion_stamped.hpp>
25 | #include <geometry_msgs/msg/transform.hpp>
26 | #include <geometry_msgs/msg/transform_stamped.hpp>
27 | // #include <geometry_msgs/msg/twist.hpp>
28 | // #include <geometry_msgs/msg/twist_stamped.hpp>
29 | // #include <geometry_msgs/msg/twist_with_covariance.hpp>
30 | // #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
31 | #include <geometry_msgs/msg/vector3.hpp>
32 | #include <geometry_msgs/msg/vector3_stamped.hpp>
33 | // #include <geometry_msgs/msg/velocity_stamped.hpp>
34 | #include <geometry_msgs/msg/wrench.hpp>
35 | #include <geometry_msgs/msg/wrench_stamped.hpp>
36 | 
37 | #include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
38 | #include <sensor_msgs/msg/point_cloud2.hpp>
39 | 


--------------------------------------------------------------------------------
/.github/workflows/docker-ros.yml:
--------------------------------------------------------------------------------
 1 | name: docker-ros
 2 | 
 3 | on:
 4 |   push:
 5 |     branches:
 6 |       - main
 7 |   pull_request:
 8 |     branches:
 9 |       - main
10 | 
11 | jobs:
12 | 
13 |   ros2-humble:
14 |     runs-on: ubuntu-latest
15 |     steps:
16 |       - uses: ika-rwth-aachen/docker-ros@main
17 |         with:
18 |           platform: amd64
19 |           target: run
20 |           image-tag: ros2-humble
21 |           base-image: rwthika/ros2:humble
22 |           command: ros2 launch message_tf_frame_transformer message_tf_frame_transformer.launch.xml
23 |           enable-recursive-vcs-import: 'false'
24 | 
25 |   ros2-jazzy:
26 |     runs-on: ubuntu-latest
27 |     steps:
28 |       - uses: ika-rwth-aachen/docker-ros@main
29 |         with:
30 |           platform: amd64
31 |           target: run
32 |           image-tag: ros2-jazzy
33 |           enable-push-as-latest: 'true'
34 |           base-image: rwthika/ros2:jazzy
35 |           rmw-implementation: rmw_zenoh_cpp
36 |           command: ros2 launch message_tf_frame_transformer message_tf_frame_transformer.launch.xml
37 |           enable-recursive-vcs-import: 'false'
38 | 
39 |   ros2-kilted:
40 |     runs-on: ubuntu-latest
41 |     steps:
42 |       - uses: ika-rwth-aachen/docker-ros@main
43 |         with:
44 |           platform: amd64
45 |           target: run
46 |           image-tag: ros2-kilted
47 |           base-image: rwthika/ros2:kilted
48 |           rmw-implementation: rmw_zenoh_cpp
49 |           command: ros2 launch message_tf_frame_transformer message_tf_frame_transformer.launch.xml
50 |           enable-recursive-vcs-import: 'false'
51 | 
52 |   ros2-rolling:
53 |     runs-on: ubuntu-latest
54 |     steps:
55 |       - uses: ika-rwth-aachen/docker-ros@main
56 |         with:
57 |           platform: amd64
58 |           target: run
59 |           image-tag: ros2-rolling
60 |           base-image: rwthika/ros2:rolling
61 |           rmw-implementation: rmw_zenoh_cpp
62 |           command: ros2 launch message_tf_frame_transformer message_tf_frame_transformer.launch.xml
63 |           enable-recursive-vcs-import: 'false'
64 | 


--------------------------------------------------------------------------------
/include/message_tf_frame_transformer/message_types.macro:
--------------------------------------------------------------------------------
 1 | /* =============================================================================
 2 | The lines below represent calls to a macro named MESSAGE_TYPE.
 3 | Wherever needed, this macro may be defined and called for all supported
 4 | message types by including this file.
 5 | 
 6 | Example:
 7 | 
 8 | #define MESSAGE_TYPE(TYPE, NAME) \
 9 |   std::cout << "The ROS message type " << #TYPE << " is supported. "; \
10 |   std::cout << std::endl;
11 | #include "message_tf_frame_transformer/message_types.macro"
12 | #undef MESSAGE_TYPE
13 | 
14 | ============================================================================= */
15 | 
16 | // MESSAGE_TYPE(geometry_msgs::msg::Accel, geometry_msgs/msg/Accel)
17 | // MESSAGE_TYPE(geometry_msgs::msg::AccelStamped, geometry_msgs/msg/AccelStamped)
18 | // MESSAGE_TYPE(geometry_msgs::msg::AccelWithCovariance, geometry_msgs/msg/AccelWithCovariance)
19 | // MESSAGE_TYPE(geometry_msgs::msg::AccelWithCovarianceStamped, geometry_msgs/msg/AccelWithCovarianceStamped)
20 | // MESSAGE_TYPE(geometry_msgs::msg::Inertia, geometry_msgs/msg/Inertia)
21 | // MESSAGE_TYPE(geometry_msgs::msg::InertiaStamped, geometry_msgs/msg/InertiaStamped)
22 | MESSAGE_TYPE(geometry_msgs::msg::Point, geometry_msgs/msg/Point)
23 | MESSAGE_TYPE(geometry_msgs::msg::Point32, geometry_msgs/msg/Point32)
24 | MESSAGE_TYPE(geometry_msgs::msg::PointStamped, geometry_msgs/msg/PointStamped)
25 | MESSAGE_TYPE(geometry_msgs::msg::Polygon, geometry_msgs/msg/Polygon)
26 | // MESSAGE_TYPE(geometry_msgs::msg::PolygonInstance, geometry_msgs/msg/PolygonInstance)
27 | // MESSAGE_TYPE(geometry_msgs::msg::PolygonInstanceStamped, geometry_msgs/msg/PolygonInstanceStamped)
28 | MESSAGE_TYPE(geometry_msgs::msg::PolygonStamped, geometry_msgs/msg/PolygonStamped)
29 | MESSAGE_TYPE(geometry_msgs::msg::Pose, geometry_msgs/msg/Pose)
30 | // MESSAGE_TYPE(geometry_msgs::msg::Pose2D, geometry_msgs/msg/Pose2D)
31 | // MESSAGE_TYPE(geometry_msgs::msg::PoseArray, geometry_msgs/msg/PoseArray)
32 | MESSAGE_TYPE(geometry_msgs::msg::PoseStamped, geometry_msgs/msg/PoseStamped)
33 | MESSAGE_TYPE(geometry_msgs::msg::PoseWithCovariance, geometry_msgs/msg/PoseWithCovariance)
34 | MESSAGE_TYPE(geometry_msgs::msg::PoseWithCovarianceStamped, geometry_msgs/msg/PoseWithCovarianceStamped)
35 | MESSAGE_TYPE(geometry_msgs::msg::Quaternion, geometry_msgs/msg/Quaternion)
36 | MESSAGE_TYPE(geometry_msgs::msg::QuaternionStamped, geometry_msgs/msg/QuaternionStamped)
37 | MESSAGE_TYPE(geometry_msgs::msg::Transform, geometry_msgs/msg/Transform)
38 | MESSAGE_TYPE(geometry_msgs::msg::TransformStamped, geometry_msgs/msg/TransformStamped)
39 | // MESSAGE_TYPE(geometry_msgs::msg::Twist, geometry_msgs/msg/Twist)
40 | // MESSAGE_TYPE(geometry_msgs::msg::TwistStamped, geometry_msgs/msg/TwistStamped)
41 | // MESSAGE_TYPE(geometry_msgs::msg::TwistWithCovariance, geometry_msgs/msg/TwistWithCovariance)
42 | // MESSAGE_TYPE(geometry_msgs::msg::TwistWithCovarianceStamped, geometry_msgs/msg/TwistWithCovarianceStamped)
43 | MESSAGE_TYPE(geometry_msgs::msg::Vector3, geometry_msgs/msg/Vector3)
44 | MESSAGE_TYPE(geometry_msgs::msg::Vector3Stamped, geometry_msgs/msg/Vector3Stamped)
45 | // MESSAGE_TYPE(geometry_msgs::msg::VelocityStamped, geometry_msgs/msg/VelocityStamped)
46 | MESSAGE_TYPE(geometry_msgs::msg::Wrench, geometry_msgs/msg/Wrench)
47 | MESSAGE_TYPE(geometry_msgs::msg::WrenchStamped, geometry_msgs/msg/WrenchStamped)
48 | 
49 | MESSAGE_TYPE(sensor_msgs::msg::PointCloud2, sensor_msgs/msg/PointCloud2)
50 | 


--------------------------------------------------------------------------------
/include/message_tf_frame_transformer/MessageTfFrameTransformer.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | ==============================================================================
  3 | MIT License
  4 | 
  5 | Copyright 2023 Institute for Automotive Engineering of RWTH Aachen University.
  6 | 
  7 | Permission is hereby granted, free of charge, to any person obtaining a copy
  8 | of this software and associated documentation files (the "Software"), to deal
  9 | in the Software without restriction, including without limitation the rights
 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 11 | copies of the Software, and to permit persons to whom the Software is
 12 | furnished to do so, subject to the following conditions:
 13 | 
 14 | The above copyright notice and this permission notice shall be included in all
 15 | copies or substantial portions of the Software.
 16 | 
 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 23 | SOFTWARE.
 24 | ==============================================================================
 25 | */
 26 | 
 27 | 
 28 | #pragma once
 29 | 
 30 | #include <memory>
 31 | #include <string>
 32 | #include <type_traits>
 33 | 
 34 | #include <rclcpp/rclcpp.hpp>
 35 | #include <rclcpp/serialization.hpp>
 36 | #include <tf2_ros/buffer.h>
 37 | #include <tf2_ros/transform_listener.h>
 38 | 
 39 | 
 40 | namespace message_tf_frame_transformer {
 41 | 
 42 | 
 43 | // definitions for distinguishing between ROS messages with/without header
 44 | // based on SFINAE (https://fekir.info/post/detect-member-variables/)
 45 | using HasRosHeaderNo = std::false_type;
 46 | using HasRosHeaderYes = std::true_type;
 47 | 
 48 | template <typename T, typename = std_msgs::msg::Header>
 49 | struct HasRosHeader : HasRosHeaderNo {};
 50 | 
 51 | template <typename T>
 52 | struct HasRosHeader<T, decltype(T::header)> : HasRosHeaderYes {};
 53 | 
 54 | 
 55 | class MessageTfFrameTransformer : public rclcpp::Node {
 56 | 
 57 |  public:
 58 | 
 59 |   MessageTfFrameTransformer();
 60 | 
 61 |  protected:
 62 | 
 63 |   void loadParameters();
 64 | 
 65 |   void setup();
 66 | 
 67 |   void detectMessageType();
 68 | 
 69 |   void transformGeneric(const std::shared_ptr<const rclcpp::SerializedMessage>& serialized_msg);
 70 | 
 71 |   template <typename T>
 72 |   void transform(const T& msg);
 73 | 
 74 |   template <typename T>
 75 |   void transform(const T& msg, const HasRosHeaderYes&);
 76 | 
 77 |   template <typename T>
 78 |   void transform(const T& msg, const HasRosHeaderNo&);
 79 | 
 80 |  protected:
 81 | 
 82 |   static const std::string kInputTopic;
 83 | 
 84 |   static const std::string kOutputTopic;
 85 | 
 86 |   static const std::string kSourceFrameIdParam;
 87 | 
 88 |   static const std::string kTargetFrameIdParam;
 89 | 
 90 |   std::string source_frame_id_;
 91 | 
 92 |   std::string target_frame_id_;
 93 | 
 94 |   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 95 | 
 96 |   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 97 | 
 98 |   rclcpp::TimerBase::SharedPtr detect_message_type_timer_;
 99 | 
100 |   rclcpp::GenericSubscription::SharedPtr subscriber_;
101 | 
102 |   rclcpp::PublisherBase::SharedPtr publisher_;
103 | 
104 |   std::string msg_type_;
105 | };
106 | 
107 | 
108 | template <typename T>
109 | void MessageTfFrameTransformer::transform(const T& msg) {
110 |   this->transform<T>(msg, HasRosHeader<T>());
111 | }
112 | 
113 | template <typename T>
114 | void MessageTfFrameTransformer::transform(const T& msg, const HasRosHeaderYes&) {
115 | 
116 |   // transform
117 |   T tf_msg;
118 |   try {
119 |     tf_buffer_->transform(msg, tf_msg, target_frame_id_);
120 |   } catch (tf2::LookupException &e) {
121 |     RCLCPP_ERROR(
122 |       this->get_logger(),
123 |       "Failed to lookup transform from '%s' to '%s': %s", msg.header.frame_id.c_str(), target_frame_id_.c_str(), e.what()
124 |     );
125 |     return;
126 |   }
127 | 
128 |   // publish
129 |   RCLCPP_DEBUG(
130 |     this->get_logger(),
131 |     "Publishing data transformed from '%s' to '%s'", msg.header.frame_id.c_str(), target_frame_id_.c_str()
132 |   );
133 |   std::static_pointer_cast<rclcpp::Publisher<T>>(publisher_)->publish(tf_msg);
134 | }
135 | 
136 | template <typename T>
137 | void MessageTfFrameTransformer::transform(const T& msg, const HasRosHeaderNo&) {
138 | 
139 |   if (source_frame_id_.empty()) {
140 |     RCLCPP_ERROR(
141 |       this->get_logger(),
142 |       "Transforming messages without an 'std_msgs/Header' requires the '%s' parameter to be set", kSourceFrameIdParam.c_str()
143 |     );
144 |     return;
145 |   }
146 | 
147 |   // lookup transform
148 |   geometry_msgs::msg::TransformStamped tf;
149 |   try {
150 |     tf = tf_buffer_->lookupTransform(target_frame_id_, source_frame_id_, tf2::TimePointZero);
151 |   } catch (tf2::LookupException &e) {
152 |     RCLCPP_ERROR(
153 |       this->get_logger(),
154 |       "Failed to lookup transform from '%s' to '%s': %s", source_frame_id_.c_str(), target_frame_id_.c_str(), e.what()
155 |     );
156 |     return;
157 |   }
158 | 
159 |   // transform
160 |   T tf_msg;
161 |   tf2::doTransform(msg, tf_msg, tf);
162 | 
163 |   // publish
164 |   RCLCPP_DEBUG(
165 |     this->get_logger(),
166 |     "Publishing data transformed from '%s' to '%s'", source_frame_id_.c_str(), target_frame_id_.c_str()
167 |   );
168 |   std::static_pointer_cast<rclcpp::Publisher<T>>(publisher_)->publish(tf_msg);
169 | }
170 | 
171 | 
172 | }  // namespace message_tf_frame_transformer
173 | 


--------------------------------------------------------------------------------
/src/message_tf_frame_transformer.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | ==============================================================================
  3 | MIT License
  4 | 
  5 | Copyright 2023 Institute for Automotive Engineering of RWTH Aachen University.
  6 | 
  7 | Permission is hereby granted, free of charge, to any person obtaining a copy
  8 | of this software and associated documentation files (the "Software"), to deal
  9 | in the Software without restriction, including without limitation the rights
 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 11 | copies of the Software, and to permit persons to whom the Software is
 12 | furnished to do so, subject to the following conditions:
 13 | 
 14 | The above copyright notice and this permission notice shall be included in all
 15 | copies or substantial portions of the Software.
 16 | 
 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 23 | SOFTWARE.
 24 | ==============================================================================
 25 | */
 26 | 
 27 | 
 28 | #include <regex>
 29 | 
 30 | #include <message_tf_frame_transformer/MessageTfFrameTransformer.hpp>
 31 | #include <message_tf_frame_transformer/message_types.hpp>
 32 | 
 33 | 
 34 | namespace message_tf_frame_transformer {
 35 | 
 36 | 
 37 | const std::string MessageTfFrameTransformer::kInputTopic = "~/input";
 38 | 
 39 | const std::string MessageTfFrameTransformer::kOutputTopic = "~/transformed";
 40 | 
 41 | const std::string MessageTfFrameTransformer::kSourceFrameIdParam = "source_frame_id";
 42 | 
 43 | const std::string MessageTfFrameTransformer::kTargetFrameIdParam = "target_frame_id";
 44 | 
 45 | 
 46 | MessageTfFrameTransformer::MessageTfFrameTransformer() : Node("message_tf_frame_transformer") {
 47 | 
 48 |   loadParameters();
 49 |   setup();
 50 | }
 51 | 
 52 | 
 53 | void MessageTfFrameTransformer::loadParameters() {
 54 | 
 55 |   rcl_interfaces::msg::ParameterDescriptor source_frame_id_param_desc;
 56 |   source_frame_id_param_desc.description = "Source frame ID to transform from (optional; if message has no std_msgs/Header)";
 57 |   this->declare_parameter(kSourceFrameIdParam, rclcpp::ParameterType::PARAMETER_STRING, source_frame_id_param_desc);
 58 | 
 59 |   try {
 60 |     source_frame_id_ = this->get_parameter(kSourceFrameIdParam).as_string();
 61 |   } catch (rclcpp::exceptions::ParameterUninitializedException&) {}
 62 | 
 63 |   rcl_interfaces::msg::ParameterDescriptor target_frame_id_param_desc;
 64 |   target_frame_id_param_desc.description = "Target frame ID to transform to";
 65 |   this->declare_parameter(kTargetFrameIdParam, rclcpp::ParameterType::PARAMETER_STRING, target_frame_id_param_desc);
 66 | 
 67 |   try {
 68 |     target_frame_id_ = this->get_parameter(kTargetFrameIdParam).as_string();
 69 |   } catch (rclcpp::exceptions::ParameterUninitializedException&) {
 70 |     RCLCPP_FATAL(get_logger(), "Parameter '%s' is required", kTargetFrameIdParam.c_str());
 71 |     exit(EXIT_FAILURE);
 72 |   }
 73 | 
 74 |   RCLCPP_INFO(
 75 |     this->get_logger(),
 76 |     "Transforming data on topic '%s' to frame '%s' published on topic '%s'",
 77 |     this->get_node_topics_interface()->resolve_topic_name(kInputTopic).c_str(),
 78 |     target_frame_id_.c_str(),
 79 |     this->get_node_topics_interface()->resolve_topic_name(kOutputTopic).c_str()
 80 |   );
 81 | }
 82 | 
 83 | 
 84 | void MessageTfFrameTransformer::setup() {
 85 | 
 86 |   // listen to tf
 87 |   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
 88 |   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 89 | 
 90 |   // setup timer to detect subscription type and then subscribe
 91 |   detect_message_type_timer_ =
 92 |     create_wall_timer(std::chrono::duration<double>(0.001),
 93 |                       std::bind(&MessageTfFrameTransformer::detectMessageType, this));
 94 | }
 95 | 
 96 | 
 97 | void MessageTfFrameTransformer::detectMessageType() {
 98 | 
 99 |   // check if topic to subscribe exists
100 |   std::string resolved_input_topic = this->get_node_topics_interface()->resolve_topic_name(kInputTopic);
101 |   const auto all_topics_and_types = get_topic_names_and_types();
102 |   if (all_topics_and_types.count(resolved_input_topic)) {
103 | 
104 |     detect_message_type_timer_->cancel();
105 |     msg_type_ = all_topics_and_types.at(resolved_input_topic)[0];
106 |     RCLCPP_DEBUG(this->get_logger(), "Detected message type '%s'", msg_type_.c_str());
107 | 
108 |     // setup publisher with correct message type
109 |     if (false) {}
110 | #define MESSAGE_TYPE(TYPE, NAME)                                               \
111 |     else if (msg_type_ == #NAME) {                                             \
112 |       publisher_ = this->create_publisher<TYPE>(kOutputTopic, 10);             \
113 |     }
114 | #include "message_tf_frame_transformer/message_types.macro"
115 | #undef MESSAGE_TYPE
116 |     else {
117 |       RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
118 |         "Transforming message type '%s' is not supported",
119 |         msg_type_.c_str());
120 |       detect_message_type_timer_->reset();
121 |       return;
122 |     }
123 | 
124 |     // setup generic subscriber with correct message type (will have missed at least one message)
125 |     subscriber_ = this->create_generic_subscription(
126 |       kInputTopic,
127 |       msg_type_,
128 |       10,
129 |       std::bind(&MessageTfFrameTransformer::transformGeneric, this, std::placeholders::_1)
130 |     );
131 | 
132 |     RCLCPP_WARN(this->get_logger(),
133 |       "Subscribed to message type '%s' on topic '%s'; first message may have been missed",
134 |       msg_type_.c_str(),
135 |       subscriber_->get_topic_name()
136 |     );
137 |   }
138 | }
139 | 
140 | 
141 | void MessageTfFrameTransformer::transformGeneric(const std::shared_ptr<const rclcpp::SerializedMessage>& serialized_msg) {
142 | 
143 |   if (false) {}
144 | #define MESSAGE_TYPE(TYPE, NAME)                                               \
145 |   else if (msg_type_ == #NAME) {                                               \
146 |                                                                                \
147 |     /* instantiate generic message as message of concrete type */              \
148 |     TYPE msg;                                                                  \
149 |     rclcpp::Serialization<TYPE> serializer;                                    \
150 |     serializer.deserialize_message(serialized_msg.get(), &msg);                \
151 |                                                                                \
152 |     /* pass message to transform callback */                                   \
153 |     this->transform<TYPE>(msg);                                                \
154 |   }
155 | #include "message_tf_frame_transformer/message_types.macro"
156 | #undef MESSAGE_TYPE
157 | }
158 | 
159 | 
160 | }  // namespace message_tf_frame_transformer
161 | 
162 | 
163 | int main(int argc, char *argv[]) {
164 | 
165 |   rclcpp::init(argc, argv);
166 |   rclcpp::spin(std::make_shared<message_tf_frame_transformer::MessageTfFrameTransformer>());
167 |   rclcpp::shutdown();
168 | 
169 |   return 0;
170 | }
171 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # message_tf_frame_transformer
  2 | 
  3 | <p align="center">
  4 |   <img src="https://img.shields.io/github/v/release/ika-rwth-aachen/message_tf_frame_transformer"/>
  5 |   <img src="https://img.shields.io/github/license/ika-rwth-aachen/message_tf_frame_transformer"/>
  6 |   <a href="https://github.com/ika-rwth-aachen/message_tf_frame_transformer/actions/workflows/industrial_ci.yml"><img src="https://github.com/ika-rwth-aachen/message_tf_frame_transformer/actions/workflows/industrial_ci.yml/badge.svg"/></a>
  7 |   <a href="https://github.com/ika-rwth-aachen/message_tf_frame_transformer/actions/workflows/docker-ros.yml"><img src="https://github.com/ika-rwth-aachen/message_tf_frame_transformer/actions/workflows/docker-ros.yml/badge.svg"/></a>
  8 |   <img src="https://img.shields.io/badge/ROS-humble|jazzy|kilted|rolling-293754"/>
  9 |   <a href="https://github.com/ika-rwth-aachen/message_tf_frame_transformer"><img src="https://img.shields.io/github/stars/ika-rwth-aachen/message_tf_frame_transformer?style=social"/></a>
 10 | </p>
 11 | 
 12 | The *message_tf_frame_transformer* package provides a ROS node to transform 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 messages to a target coordinate frame.
 13 | 
 14 | - [Installation](#installation)
 15 |   - [docker-ros](#docker-ros)
 16 | - [Usage](#usage)
 17 | - [Supported Message Types](#supported-message-types)
 18 | - [Nodes](#nodes)
 19 | - [Acknowledgements](#acknowledgements)
 20 | 
 21 | 
 22 | ## Installation
 23 | 
 24 | The *message_tf_frame_transformer* package is released as an official ROS package and can easily be installed via a package manager.
 25 | 
 26 | ```bash
 27 | sudo apt install ros-$ROS_DISTRO-message-tf-frame-transformer
 28 | ```
 29 | 
 30 | 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).
 31 | 
 32 | ```bash
 33 | # message_tf_frame_transformer$
 34 | rosdep install -r --ignore-src --from-paths .
 35 | 
 36 | # workspace$
 37 | colcon build --packages-up-to message_tf_frame_transformer --cmake-args -DCMAKE_BUILD_TYPE=Release
 38 | ```
 39 | 
 40 | ### docker-ros
 41 | 
 42 | *message_tf_frame_transformer* is also available as a Docker image, containerized through [*docker-ros*](https://github.com/ika-rwth-aachen/docker-ros).
 43 | 
 44 | ```bash
 45 | docker run --rm ghcr.io/ika-rwth-aachen/message_tf_frame_transformer:latest # or distro-specific tags, e.g., :ros2-rolling
 46 | ```
 47 | 
 48 | 
 49 | ## Usage
 50 | 
 51 | 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.
 52 | 
 53 | ```bash
 54 | ros2 run message_tf_frame_transformer message_tf_frame_transformer --ros-args \
 55 |   -r \~/input:=$INPUT_TOPIC \
 56 |   -r \~/transformed:=$OUTPUT_TOPIC \
 57 |   -p source_frame_id:=$SOURCE_FRAME_ID \
 58 |   -p target_frame_id:=$TARGET_FRAME_ID
 59 | ```
 60 | 
 61 | 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.
 62 | 
 63 | ```bash
 64 | ros2 launch message_tf_frame_transformer message_tf_frame_transformer.launch.xml \
 65 |   input_topic:=$INPUT_TOPIC \
 66 |   output_topic:=$OUTPUT_TOPIC \
 67 |   source_frame_id:=$SOURCE_FRAME_ID \
 68 |   target_frame_id:=$TARGET_FRAME_ID \
 69 |   x:=$X \
 70 |   y:=$Y \
 71 |   z:=$Z \
 72 |   roll:=$ROLL \
 73 |   pitch:=$PITCH \
 74 |   yaw:=$YAW
 75 | ```
 76 | 
 77 | 
 78 | ## Supported Message Types
 79 | 
 80 | 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.
 81 | 
 82 | | ROS Message |
 83 | | --- |
 84 | | [`geometry_msgs/msg/Point`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Point.html) |
 85 | | [`geometry_msgs/msg/Point32`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Point32.html) |
 86 | | [`geometry_msgs/msg/PointStamped`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/PointStamped.html) |
 87 | | [`geometry_msgs/msg/Polygon`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Polygon.html) |
 88 | | [`geometry_msgs/msg/PolygonStamped`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/PolygonStamped.html) |
 89 | | [`geometry_msgs/msg/Pose`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Pose.html) |
 90 | | [`geometry_msgs/msg/PoseStamped`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/PoseStamped.html) |
 91 | | [`geometry_msgs/msg/PoseWithCovariance`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/PoseWithCovariance.html) |
 92 | | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/PoseWithCovarianceStamped.html) |
 93 | | [`geometry_msgs/msg/Quaternion`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Quaternion.html) |
 94 | | [`geometry_msgs/msg/QuaternionStamped`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/QuaternionStamped.html) |
 95 | | [`geometry_msgs/msg/Transform`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Transform.html) |
 96 | | [`geometry_msgs/msg/TransformStamped`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/TransformStamped.html) |
 97 | | [`geometry_msgs/msg/Vector3`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Vector3.html) |
 98 | | [`geometry_msgs/msg/Vector3Stamped`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Vector3Stamped.html) |
 99 | | [`geometry_msgs/msg/Wrench`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/Wrench.html) |
100 | | [`geometry_msgs/msg/WrenchStamped`](https://docs.ros.org/en/ros2_packages/jazzy/api/geometry_msgs/msg/WrenchStamped.html) |
101 | | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) |
102 | 
103 | ### Adding Support for a New Message Type
104 | 
105 | 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!
106 | 
107 | 1. [`message_types.hpp`](./include/message_tf_frame_transformer/message_types.hpp)
108 |    - include required message headers
109 | 1. [`message_types.macro`](./include/message_tf_frame_transformer/message_types.macro)
110 |    - define information about the new message type by calling the `MESSAGE_TYPE` macro
111 |       - `TYPE`: ROS message type (e.g. `geometry_msgs::msg::PointStamped`)
112 |       - `NAME`: ROS message type name (e.g. `geometry_msgs/msg/PointStamped`)
113 | 
114 | 
115 | ## Nodes
116 | 
117 | | Package | Node | Description |
118 | | --- | --- | --- |
119 | | `message_tf_frame_transformer` | `message_tf_frame_transformer` | transform arbitrary ROS messages to a different coordinate frame |
120 | 
121 | ### message_tf_frame_transformer/message_tf_frame_transformer
122 | 
123 | #### Subscribed Topics
124 | 
125 | | Topic | Type | Description |
126 | | --- | --- | --- |
127 | | `~/input` | see [Supported Message Types](#supported-message-types) | message to transform |
128 | 
129 | #### Published Topics
130 | 
131 | | Topic | Type | Description |
132 | | --- | --- | --- |
133 | | `~/transformed` | see [Supported Message Types](#supported-message-types) | transformed message |
134 | 
135 | #### Services
136 | 
137 | \-
138 | 
139 | #### Actions
140 | 
141 | \-
142 | 
143 | #### Parameters
144 | 
145 | | Parameter | Type | Description |
146 | | --- | --- | --- |
147 | | `~/target_frame_id` | `string` | target frame ID |
148 | | `~/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)) |
149 | 
150 | 
151 | ## Acknowledgements
152 | 
153 | 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).
154 | 
155 | 


--------------------------------------------------------------------------------