Add tf prefix helper and test

controller_interface/CMakeLists.txt

@@ -117,6 +117,11 @@ if(BUILD_TESTING)
     hardware_interface::hardware_interface
     ${std_msgs_TARGETS}
   )
+
+  ament_add_gmock(test_controller_tf_prefix test/test_controller_tf_prefix.cpp)
+  target_link_libraries(test_controller_tf_prefix
+    controller_interface
+  )
 endif()
 
 install(

controller_interface/include/controller_interface/helpers.hpp

@@ -95,6 +95,39 @@ inline bool interface_list_contains_interface_type(
          interface_type_list.end();
 }
 
+/**
+ * @brief Apply tf prefix
+ * @param tf_prefix_enabled  Whether tf prefixing is enabled
+ * @param prefix The tf prefix to apply
+ * @param node_ns Node namespace to use as prefix if prefix is empty
+ * @return Slash normalized prefix
+ */
+inline std::string apply_tf_prefix(
+  const bool tf_prefix_enabled, const std::string & prefix, const std::string & node_ns)
+{
+  if (!tf_prefix_enabled)
+  {
+    return std::string{};
+  }
+  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;
+}
+
 }  // namespace controller_interface
 
 #endif  // CONTROLLER_INTERFACE__HELPERS_HPP_

controller_interface/test/test_controller_tf_prefix.cpp

@@ -0,0 +1,52 @@
+// 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 "controller_interface/helpers.hpp"
+#include "test_controller_tf_prefix.hpp"
+
+TEST_F(TestControllerTFPrefix, DisabledPrefixReturnsEmpty)
+{
+  EXPECT_EQ(controller_interface::apply_tf_prefix(false, "robot", "/ns"), "");
+  EXPECT_EQ(controller_interface::apply_tf_prefix(false, "", "/ns"), "");
+}
+
+TEST_F(TestControllerTFPrefix, EmptyPrefixUsesNamespace)
+{
+  EXPECT_EQ(controller_interface::apply_tf_prefix(true, "", "/ns"), "ns/");
+}
+
+TEST_F(TestControllerTFPrefix, ExplicitPrefixUsed)
+{
+  EXPECT_EQ(controller_interface::apply_tf_prefix(true, "robot", "/ns"), "robot/");
+}
+
+TEST_F(TestControllerTFPrefix, NormalizesPrefixSlashes)
+{
+  EXPECT_EQ(controller_interface::apply_tf_prefix(true, "/robot1", "/ns"), "robot1/");
+  EXPECT_EQ(controller_interface::apply_tf_prefix(true, "robot2//", "/ns"), "robot2//");
+  EXPECT_EQ(controller_interface::apply_tf_prefix(true, "/robot3/", "/ns"), "robot3/");
+  EXPECT_EQ(controller_interface::apply_tf_prefix(true, "/", "/ns"), "");
+}
+
+TEST_F(TestControllerTFPrefix, EmptyPrefixAndNamespace)
+{
+  EXPECT_EQ(controller_interface::apply_tf_prefix(true, "", ""), "");
+}
+
+TEST_F(TestControllerTFPrefix, ComplexNamespace)
+{
+  EXPECT_EQ(controller_interface::apply_tf_prefix(true, "", "/ns/"), "ns/");
+}

controller_interface/test/test_controller_tf_prefix.hpp

@@ -0,0 +1,34 @@
+// 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 TEST_CONTROLLER_TF_PREFIX_HPP_
+#define TEST_CONTROLLER_TF_PREFIX_HPP_
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+class TestControllerTFPrefix : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    // placeholder
+  }
+  void TearDown() override
+  {
+    // placeholder
+  }
+};
+
+#endif  // TEST_CONTROLLER_TF_PREFIX_HPP_