TF prefix helper added

control_toolbox/CMakeLists.txt

@@ -183,6 +183,9 @@ if(BUILD_TESTING)
   ament_add_gmock(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp)
   target_link_libraries(pid_ros_publisher_tests control_toolbox rclcpp_lifecycle::rclcpp_lifecycle)
 
+  ament_add_gmock(tf_utils_tests test/tf_utils_tests.cpp)
+  target_link_libraries(tf_utils_tests control_toolbox)
+
   ## Control Filters
   ### Gravity Compensation
   add_rostest_with_parameters_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp

control_toolbox/include/control_toolbox/tf_utils.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) 2025, ros2_control developers
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROL_TOOLBOX__TF_UTILS_HPP_
+#define CONTROL_TOOLBOX__TF_UTILS_HPP_
+
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace control_toolbox
+{
+/**
+* @brief Apply TF prefix to given frame
+* @param prefix TF prefix
+* @param node_ns Node namespace to use as prefix if prefix is empty
+* @param frame Frame name
+* @return The prefixed frame name if prefix is not empty, otherwise the original frame name
+*/
+inline std::string apply_tf_prefix(
+  const std::string & prefix, const std::string & node_ns, const std::string & frame)
+{
+  std::string nprefix = prefix.empty() ? node_ns : prefix;
+
+  // Normalize the prefix
+  if (!nprefix.empty())
+  {
+    // ensure trailing '/'
+    if (nprefix.back() != '/')
+    {
+      nprefix.push_back('/');
+    }
+    // remove leading '/'
+    if (nprefix.front() == '/')
+    {
+      nprefix.erase(0, 1);
+    }
+  }
+  return nprefix + frame;
+}
+
+}  // namespace control_toolbox
+
+#endif  // CONTROL_TOOLBOX__TF_UTILS_HPP_

control_toolbox/test/tf_utils_tests.cpp

@@ -0,0 +1,55 @@
+// Copyright (c) 2025, ros2_control developers
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gmock/gmock.h>
+#include "control_toolbox/tf_utils.hpp"
+
+TEST(ApplyTFPrefixTest, UsesNamespaceWhenPrefixEmpty)
+{
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("", "/ns", "odom"), "ns/odom");
+}
+
+TEST(ApplyTFPrefixTest, UsesExplicitPrefix)
+{
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("robot", "/ns", "base"), "robot/base");
+}
+
+TEST(ApplyTFPrefixTest, NormalizesPrefixSlashes)
+{
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("/robot1", "/ns", "link"), "robot1/link");
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("robot2//", "/ns", "odom"), "robot2//odom");
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("/robot3/", "/ns", "base_link"), "robot3/base_link");
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("/", "/ns", "odom"), "odom");
+}
+
+TEST(ApplyTFPrefixTest, EmptyPrefixAndNamespace)
+{
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("", "", "odom"), "odom");
+}
+
+TEST(ApplyTFPrefixTest, FrameHasLeadingSlash)
+{
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("robot", "/ns", "/odom"), "robot//odom");
+}
+
+TEST(ApplyTFPrefixTest, ComplexPrefixAndNamespace)
+{
+  EXPECT_EQ(control_toolbox::apply_tf_prefix("/robot/", "/my_ns/", "odom"), "robot/odom");
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}