Add tf prefix helper and test (#2803) (#2867)

Author: mergify[bot]

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/tf_prefix.hpp

@@ -0,0 +1,56 @@
+// 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 CONTROLLER_INTERFACE__TF_PREFIX_HPP_
+#define CONTROLLER_INTERFACE__TF_PREFIX_HPP_
+
+#include <string>
+
+namespace controller_interface
+{
+/**
+ * @brief Resolve the TF prefix with normalized slashes
+ * @param prefix The TF prefix
+ * @param node_ns Node namespace to use as prefix if prefix is empty
+ * @return Prefix to be prepended
+ */
+inline std::string resolve_tf_prefix(const std::string & prefix, const std::string & node_ns)
+{
+  if (prefix.empty())
+  {
+    return "";
+  }
+
+  std::string nprefix = prefix;
+  std::size_t pos = nprefix.find("~");
+  if (pos != std::string::npos)
+  {
+    nprefix.replace(pos, 1, node_ns);
+  }
+
+  // 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__TF_PREFIX_HPP_

controller_interface/test/test_controller_tf_prefix.cpp

@@ -0,0 +1,44 @@
+// 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/tf_prefix.hpp"
+#include "test_controller_tf_prefix.hpp"
+
+TEST_F(TestControllerTFPrefix, EmptyPrefixReturnsEmpty)
+{
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("", "/ns"), "");
+}
+
+TEST_F(TestControllerTFPrefix, ExplicitPrefixUsed)
+{
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("robot", "/ns"), "robot/");
+}
+
+TEST_F(TestControllerTFPrefix, NormalizePrefixSlashes)
+{
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("/robot1", "/ns"), "robot1/");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("robot2//", "/ns"), "robot2//");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("/robot3/", "/ns"), "robot3/");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("/", "/ns"), "");
+}
+
+TEST_F(TestControllerTFPrefix, TildePrefixResolvesToNamespace)
+{
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("~", "/ns"), "ns/");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("~/", "/ns"), "ns/");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("~/robot", "/ns"), "ns/robot/");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("/~/robot/", "ns"), "ns/robot/");
+}

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_