Added republisher node for tf messages with frame prefix

include/hector_multi_robot_tools/prefix_tf_republisher_node.hpp

@@ -0,0 +1,27 @@
+#ifndef PREFIX_TF_REPUBLISHER_NODE_H
+#define PREFIX_TF_REPUBLISHER_NODE_H
+
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+namespace hector_multi_robot_tools {
+
+class PrefixTfRepublisherNode : public rclcpp::Node {
+public:
+    PrefixTfRepublisherNode();
+    PrefixTfRepublisherNode(const rclcpp::NodeOptions& options);
+private:
+    void tfMessageCallback(const tf2_msgs::msg::TFMessage& msg) const;
+
+    static void prependFramePrefix(tf2_msgs::msg::TFMessage& tf_message, std::string prefix);
+    static std::string stripLeadingSlash(const std::string& frame_id);
+
+    rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_;
+    rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_;
+
+    std::string frame_prefix_;
+};
+
+}
+
+#endif

launch/prefix_tf_republisher.launch.yaml

@@ -0,0 +1,9 @@
+launch:
+- arg:
+    name: namespace
+
+- node:
+    pkg: "hector_multi_robot_tools"
+    exec: "prefix_tf_republisher"
+    name: "prefix_tf_republisher"
+    namespace: "$(var namespace)"

package.xml

@@ -9,6 +9,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>tf2_msgs</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/prefix_tf_republisher.cpp

@@ -0,0 +1,11 @@
+#include <hector_multi_robot_tools/prefix_tf_republisher_node.hpp>
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+
+    const auto node = std::make_shared<hector_multi_robot_tools::PrefixTfRepublisherNode>();
+    spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/prefix_tf_republisher_node.cpp

@@ -1,9 +1,47 @@
-int main( int argc, char **argv )
+#include <hector_multi_robot_tools/prefix_tf_republisher_node.hpp>
+
+namespace hector_multi_robot_tools {
+PrefixTfRepublisherNode::PrefixTfRepublisherNode() : PrefixTfRepublisherNode(rclcpp::NodeOptions()) {
+}
+
+PrefixTfRepublisherNode::PrefixTfRepublisherNode(const rclcpp::NodeOptions &options)
+    : rclcpp::Node("prefix_tf_republisher_node", options)
 {
-//   rclcpp::init( argc, argv );
-//
-//
-//   rclcpp::spin( node );
-//   rclcpp::shutdown();
-  return 0;
+    if (get_effective_namespace() == "/") {
+        RCLCPP_FATAL_STREAM(get_logger(), "Node is started in global namespace. This will lead to a topic loop.");
+        rclcpp::shutdown();
+    }
+
+    rcl_interfaces::msg::ParameterDescriptor frame_prefix_descriptor;
+    frame_prefix_descriptor.description = "Frame prefix to prepend on frame_id (default: node namespace)";
+    frame_prefix_descriptor.read_only = true;
+    std::string default_frame_prefix = stripLeadingSlash(get_effective_namespace());
+    frame_prefix_ = declare_parameter("frame_prefix", default_frame_prefix);
+    RCLCPP_INFO_STREAM(get_logger(), "Republishing tf frame ids with prefix '" << frame_prefix_ << "'");
+
+    pub_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", 10);
+    sub_ = create_subscription<tf2_msgs::msg::TFMessage>("tf", 10,
+        std::bind( &PrefixTfRepublisherNode::tfMessageCallback, this, std::placeholders::_1 ));
+}
+
+void PrefixTfRepublisherNode::tfMessageCallback(const tf2_msgs::msg::TFMessage &msg) const {
+    tf2_msgs::msg::TFMessage msg_copy = msg;
+    prependFramePrefix(msg_copy, frame_prefix_);
+    pub_->publish(msg_copy);
+}
+
+void PrefixTfRepublisherNode::prependFramePrefix(tf2_msgs::msg::TFMessage &tf_message, std::string prefix) {
+    for (auto& msg: tf_message.transforms) {
+        msg.child_frame_id = prefix + "/" + msg.child_frame_id;
+        msg.header.frame_id = prefix + "/" + msg.header.frame_id;
+    }
+}
+
+std::string PrefixTfRepublisherNode::stripLeadingSlash(const std::string &frame_id) {
+    if (!frame_id.empty() && frame_id.front() == '/') {
+        return frame_id.substr(1, frame_id.size() - 1);
+    } else {
+        return frame_id;
+    }
+}
 }