quick and dirty tf_prefix support for foxy

ros · Mar 4, 2021 · bc56c8f · bc56c8f
1 parent d8b471b
commit bc56c8f
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 4 deletions.
diff --git a/include/robot_state_publisher/robot_state_publisher.hpp b/include/robot_state_publisher/robot_state_publisher.hpp
@@ -101,6 +101,7 @@ class RobotStatePublisher : public rclcpp::Node
   MimicMap mimic_;
   bool use_tf_static_;
   bool ignore_timestamp_;
+  std::string tf_prefix_;
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
 };
 

diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp
@@ -125,6 +125,12 @@ RobotStatePublisher::RobotStatePublisher(const rclcpp::NodeOptions & options)
     RCLCPP_WARN(get_logger(), "use_tf_static is deprecated and will be removed in the future");
   }
 
+  // set tf_prefix and add trailing slash if its set
+  tf_prefix_ = this->declare_parameter("tf_prefix", "");
+  if (tf_prefix_.size()) {
+    tf_prefix_ = tf_prefix_ + "/";
+  }
+
   // ignore_timestamp_ == true, joint_state messages are accepted, no matter their timestamp
   ignore_timestamp_ = this->declare_parameter("ignore_timestamp", false);
 
@@ -241,8 +247,8 @@ void RobotStatePublisher::publishTransforms(
       geometry_msgs::msg::TransformStamped tf_transform =
         kdlToTransform(seg->second.segment.pose(jnt.second));
       tf_transform.header.stamp = time;
-      tf_transform.header.frame_id = seg->second.root;
-      tf_transform.child_frame_id = seg->second.tip;
+      tf_transform.header.frame_id = tf_prefix_ + seg->second.root;
+      tf_transform.child_frame_id = tf_prefix_ + seg->second.tip;
       tf_transforms.push_back(tf_transform);
     }
   }
@@ -264,8 +270,8 @@ void RobotStatePublisher::publishFixedTransforms()
     }
     tf_transform.header.stamp = now;
 
-    tf_transform.header.frame_id = seg.second.root;
-    tf_transform.child_frame_id = seg.second.tip;
+    tf_transform.header.frame_id = tf_prefix_ + seg.second.root;
+    tf_transform.child_frame_id = tf_prefix_ + seg.second.tip;
     tf_transforms.push_back(tf_transform);
   }
   if (use_tf_static_) {
@@ -373,6 +379,16 @@ rcl_interfaces::msg::SetParametersResult RobotStatePublisher::parameterUpdate(
         break;
       }
       use_tf_static_ = parameter.as_bool();
+    } else if (parameter.get_name() == "tf_prefix") {
+      if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_STRING) {
+        result.successful = false;
+        result.reason = "tf_prefix must be a string";
+        break;
+      }
+      tf_prefix_ = parameter.as_string() + "/";
+      if (tf_prefix_.size()) {
+        tf_prefix_ = tf_prefix_ + "/";
+      }
     } else if (parameter.get_name() == "ignore_timestamp") {
       if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) {
         result.successful = false;