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,73 @@
+// Copyright (c) 2025, ros2_control developers
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of the Willow Garage nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#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,73 @@
+// Copyright (c) 2025, ros2_control developers
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of the Willow Garage nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#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();
+}