add support for non-stamped messages

ika-rwth-aachen · Apr 12, 2023 · 58fd78d · 58fd78d
1 parent 39a36c9
commit 58fd78d
Show file tree

Hide file tree

Showing 9 changed files with 186 additions and 27 deletions.
diff --git a/README.md b/README.md
@@ -46,20 +46,22 @@ catkin build -DCMAKE_BUILD_TYPE=Release message_tf_frame_transformer
 
 ## 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. The `frame_id` parameter is required, while the topics otherwise default to `~/input` and `~/transformed` in the node's private namespace.
+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 frame_id:=$TARGET_FRAME_ID
+  -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 \
-  _frame_id:=$TARGET_FRAME_ID
+  _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.
@@ -94,12 +96,19 @@ The *message_tf_frame_transformer* package is able to support any ROS message ty
 
 | ROS | ROS 2 |
 | --- | --- |
+| [`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) |
 
@@ -155,7 +164,8 @@ Through application of preprocessor macros, adding support for a new ROS message
 
 | Parameter | Type | Description |
 | --- | --- | --- |
-| `~/frame_id` | `string` | target frame ID |
+| `~/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

diff --git a/include/message_tf_frame_transformer/MessageTfFrameTransformer.h b/include/message_tf_frame_transformer/MessageTfFrameTransformer.h
@@ -29,6 +29,7 @@ SOFTWARE.
 
 #include <memory>
 #include <string>
+#include <type_traits>
 
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
@@ -39,6 +40,19 @@ SOFTWARE.
 
 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 <typename T, typename = std_msgs::Header>
+struct HasRosHeader : HasRosHeaderNo {};
+
+template <typename T>
+struct HasRosHeader<T, decltype(T::header)> : HasRosHeaderYes {};
+
+
 class MessageTfFrameTransformer : public nodelet::Nodelet {
 
  protected:
@@ -54,15 +68,25 @@ class MessageTfFrameTransformer : public nodelet::Nodelet {
   template <typename T>
   void transform(const typename T::ConstPtr& msg);
 
+  template <typename T>
+  void transform(const typename T::ConstPtr& msg, const HasRosHeaderYes&);
+
+  template <typename T>
+  void transform(const typename T::ConstPtr& msg, const HasRosHeaderNo&);
+
  protected:
 
   static const std::string kInputTopic;
 
   static const std::string kOutputTopic;
 
-  static const std::string kFrameIdParam;
+  static const std::string kSourceFrameIdParam;
+
+  static const std::string kTargetFrameIdParam;
+
+  std::string source_frame_id_;
 
-  std::string frame_id_;
+  std::string target_frame_id_;
 
   ros::NodeHandle node_handle_;
 
@@ -80,18 +104,49 @@ class MessageTfFrameTransformer : public nodelet::Nodelet {
 
 template <typename T>
 void MessageTfFrameTransformer::transform(const typename T::ConstPtr& msg) {
+  this->transform<T>(msg, HasRosHeader<T>());
+}
+
+template <typename T>
+void MessageTfFrameTransformer::transform(const typename T::ConstPtr& msg, const HasRosHeaderYes&) {
 
   // transform
   T tf_msg;
   try {
-    tf_buffer_.transform(*msg, tf_msg, frame_id_);
+    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 <typename T>
+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", msg->header.frame_id.c_str(), frame_id_.c_str(), e.what());
+    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'", msg->header.frame_id.c_str(), frame_id_.c_str());
+  NODELET_DEBUG("Publishing data transformed from '%s' to '%s'", source_frame_id_.c_str(), target_frame_id_.c_str());
   publisher_.publish(tf_msg);
 }
 

diff --git a/include/message_tf_frame_transformer/MessageTfFrameTransformer.ros2.hpp b/include/message_tf_frame_transformer/MessageTfFrameTransformer.ros2.hpp
@@ -29,6 +29,7 @@ SOFTWARE.
 
 #include <memory>
 #include <string>
+#include <type_traits>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/serialization.hpp>
@@ -38,6 +39,19 @@ SOFTWARE.
 
 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 <typename T, typename = std_msgs::msg::Header>
+struct HasRosHeader : HasRosHeaderNo {};
+
+template <typename T>
+struct HasRosHeader<T, decltype(T::header)> : HasRosHeaderYes {};
+
+
 class MessageTfFrameTransformer : public rclcpp::Node {
 
  public:
@@ -57,15 +71,25 @@ class MessageTfFrameTransformer : public rclcpp::Node {
   template <typename T>
   void transform(const T& msg);
 
+  template <typename T>
+  void transform(const T& msg, const HasRosHeaderYes&);
+
+  template <typename T>
+  void transform(const T& msg, const HasRosHeaderNo&);
+
  protected:
 
   static const std::string kInputTopic;
 
   static const std::string kOutputTopic;
 
-  static const std::string kFrameIdParam;
+  static const std::string kSourceFrameIdParam;
+
+  static const std::string kTargetFrameIdParam;
 
-  std::string frame_id_;
+  std::string source_frame_id_;
+
+  std::string target_frame_id_;
 
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 
@@ -83,23 +107,63 @@ class MessageTfFrameTransformer : public rclcpp::Node {
 
 template <typename T>
 void MessageTfFrameTransformer::transform(const T& msg) {
+  this->transform<T>(msg, HasRosHeader<T>());
+}
+
+template <typename T>
+void MessageTfFrameTransformer::transform(const T& msg, const HasRosHeaderYes&) {
 
   // transform
   T tf_msg;
   try {
-    tf_buffer_->transform(msg, tf_msg, frame_id_);
+    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(), frame_id_.c_str(), e.what()
+      "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(), frame_id_.c_str()
+    "Publishing data transformed from '%s' to '%s'", msg.header.frame_id.c_str(), target_frame_id_.c_str()
+  );
+  std::static_pointer_cast<rclcpp::Publisher<T>>(publisher_)->publish(tf_msg);
+}
+
+template <typename T>
+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<rclcpp::Publisher<T>>(publisher_)->publish(tf_msg);
 }

diff --git a/include/message_tf_frame_transformer/message_types.macro b/include/message_tf_frame_transformer/message_types.macro
@@ -13,12 +13,19 @@ Example:
 
 ============================================================================= */
 
+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.macro b/include/message_tf_frame_transformer/message_types.ros2.macro
@@ -13,12 +13,19 @@ Example:
 
 ============================================================================= */
 
+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
@@ -11,7 +11,8 @@
   <node pkg="nodelet" type="nodelet" name="$(anon message_tf_frame_transformer)" args="standalone message_tf_frame_transformer/MessageTfFrameTransformer" output="screen">
     <remap from="~input" to="$(arg input_topic)" />
     <remap from="~transformed" to="$(arg output_topic)" />
-    <param name="frame_id" value="$(arg target_frame_id)" />
+    <param name="source_frame_id" value="$(arg source_frame_id)" />
+    <param name="target_frame_id" value="$(arg target_frame_id)" />
   </node>
 
 </launch>
diff --git a/launch/message_tf_frame_transformer.launch.ros2.xml b/launch/message_tf_frame_transformer.launch.ros2.xml
@@ -16,7 +16,8 @@
   <node pkg="message_tf_frame_transformer" exec="message_tf_frame_transformer" name="message_tf_frame_transformer" output="screen">
     <remap from="~/input" to="$(var input_topic)" />
     <remap from="~/transformed" to="$(var output_topic)" />
-    <param name="frame_id" value="$(var target_frame_id)" />
+    <param name="source_frame_id" value="$(var source_frame_id)" />
+    <param name="target_frame_id" value="$(var target_frame_id)" />
   </node>
 
 </launch>
diff --git a/src/MessageTfFrameTransformer.cpp b/src/MessageTfFrameTransformer.cpp
@@ -41,7 +41,9 @@ const std::string MessageTfFrameTransformer::kInputTopic = "input";
 
 const std::string MessageTfFrameTransformer::kOutputTopic = "transformed";
 
-const std::string MessageTfFrameTransformer::kFrameIdParam = "frame_id";
+const std::string MessageTfFrameTransformer::kSourceFrameIdParam = "source_frame_id";
+
+const std::string MessageTfFrameTransformer::kTargetFrameIdParam = "target_frame_id";
 
 
 void MessageTfFrameTransformer::onInit() {
@@ -56,14 +58,16 @@ void MessageTfFrameTransformer::onInit() {
 
 void MessageTfFrameTransformer::loadParameters() {
 
-  bool found = private_node_handle_.getParam(kFrameIdParam, frame_id_);
+  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", kFrameIdParam.c_str());
+    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(), frame_id_.c_str(),
+               ros::names::resolve("~" + kInputTopic).c_str(), target_frame_id_.c_str(),
                ros::names::resolve("~" + kOutputTopic).c_str());
 }