From 96220390596d0b9bc9d4a24e44d2879c08bdee7d Mon Sep 17 00:00:00 2001 From: kuralme Date: Mon, 10 Nov 2025 23:30:51 -0500 Subject: [PATCH 1/3] helper and test added --- controller_interface/CMakeLists.txt | 5 ++ .../include/controller_interface/helpers.hpp | 33 ++++++++++++ .../test/test_controller_tf_prefix.cpp | 52 +++++++++++++++++++ .../test/test_controller_tf_prefix.hpp | 34 ++++++++++++ 4 files changed, 124 insertions(+) create mode 100644 controller_interface/test/test_controller_tf_prefix.cpp create mode 100644 controller_interface/test/test_controller_tf_prefix.hpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 52105df453..1b40460361 100644 --- a/controller_interface/CMakeLists.txt +++ b/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( diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index f296b665d0..c2128f3d76 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/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_ diff --git a/controller_interface/test/test_controller_tf_prefix.cpp b/controller_interface/test/test_controller_tf_prefix.cpp new file mode 100644 index 0000000000..1374862643 --- /dev/null +++ b/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 + +#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/"); +} diff --git a/controller_interface/test/test_controller_tf_prefix.hpp b/controller_interface/test/test_controller_tf_prefix.hpp new file mode 100644 index 0000000000..47ce863ab6 --- /dev/null +++ b/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 +#include + +class TestControllerTFPrefix : public ::testing::Test +{ +public: + void SetUp() override + { + // placeholder + } + void TearDown() override + { + // placeholder + } +}; + +#endif // TEST_CONTROLLER_TF_PREFIX_HPP_ From 24f9e9b3cd50caa0b2032d7f270cfe2471dde41b Mon Sep 17 00:00:00 2001 From: kuralme Date: Wed, 12 Nov 2025 23:55:08 -0500 Subject: [PATCH 2/3] helper name changed, prefix enable flag removed --- .../include/controller_interface/helpers.hpp | 35 ++++++++----------- .../test/test_controller_tf_prefix.cpp | 32 ++++++----------- 2 files changed, 26 insertions(+), 41 deletions(-) diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index c2128f3d76..0670f177ec 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -96,34 +96,29 @@ inline bool interface_list_contains_interface_type( } /** - * @brief Apply tf prefix - * @param tf_prefix_enabled Whether tf prefixing is enabled - * @param prefix The tf prefix to apply + * @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 Slash normalized prefix + * @return Prefix to be prepended */ -inline std::string apply_tf_prefix( - const bool tf_prefix_enabled, const std::string & prefix, const std::string & node_ns) +inline std::string resolve_tf_prefix(const std::string & prefix, const std::string & node_ns) { - if (!tf_prefix_enabled) + if (prefix.empty()) { return std::string{}; } - std::string nprefix = prefix.empty() ? node_ns : prefix; - // Normalize the prefix - if (!nprefix.empty()) + std::string nprefix = prefix == "~" ? node_ns : prefix; + + // ensure trailing '/' + if (nprefix.back() != '/') { - // ensure trailing '/' - if (nprefix.back() != '/') - { - nprefix.push_back('/'); - } - // remove leading '/' - if (nprefix.front() == '/') - { - nprefix.erase(0, 1); - } + nprefix.push_back('/'); + } + // remove leading '/' + if (nprefix.front() == '/') + { + nprefix.erase(0, 1); } return nprefix; } diff --git a/controller_interface/test/test_controller_tf_prefix.cpp b/controller_interface/test/test_controller_tf_prefix.cpp index 1374862643..ee76352821 100644 --- a/controller_interface/test/test_controller_tf_prefix.cpp +++ b/controller_interface/test/test_controller_tf_prefix.cpp @@ -17,36 +17,26 @@ #include "controller_interface/helpers.hpp" #include "test_controller_tf_prefix.hpp" -TEST_F(TestControllerTFPrefix, DisabledPrefixReturnsEmpty) +TEST_F(TestControllerTFPrefix, EmptyPrefixReturnsEmpty) { - EXPECT_EQ(controller_interface::apply_tf_prefix(false, "robot", "/ns"), ""); - EXPECT_EQ(controller_interface::apply_tf_prefix(false, "", "/ns"), ""); + EXPECT_EQ(controller_interface::resolve_tf_prefix("", "/ns"), ""); } -TEST_F(TestControllerTFPrefix, EmptyPrefixUsesNamespace) +TEST_F(TestControllerTFPrefix, TildePrefixUsesNamespace) { - EXPECT_EQ(controller_interface::apply_tf_prefix(true, "", "/ns"), "ns/"); + EXPECT_EQ(controller_interface::resolve_tf_prefix("~", "/ns"), "ns/"); + EXPECT_EQ(controller_interface::resolve_tf_prefix("~", "/ns/"), "ns/"); } TEST_F(TestControllerTFPrefix, ExplicitPrefixUsed) { - EXPECT_EQ(controller_interface::apply_tf_prefix(true, "robot", "/ns"), "robot/"); + EXPECT_EQ(controller_interface::resolve_tf_prefix("robot", "/ns"), "robot/"); } -TEST_F(TestControllerTFPrefix, NormalizesPrefixSlashes) +TEST_F(TestControllerTFPrefix, NormalizePrefixSlashes) { - 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/"); + 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"), ""); } From cbd37306e3586f3c600efa751659dd0cace9b7f1 Mon Sep 17 00:00:00 2001 From: kuralme Date: Thu, 13 Nov 2025 10:00:22 -0500 Subject: [PATCH 3/3] extend tilde (~) substitution with node namespace to full prefix --- .../include/controller_interface/helpers.hpp | 7 ++++++- .../test/test_controller_tf_prefix.cpp | 14 ++++++++------ 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index 0670f177ec..c4584f84f2 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -108,7 +108,12 @@ inline std::string resolve_tf_prefix(const std::string & prefix, const std::stri return std::string{}; } - std::string nprefix = prefix == "~" ? node_ns : prefix; + 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() != '/') diff --git a/controller_interface/test/test_controller_tf_prefix.cpp b/controller_interface/test/test_controller_tf_prefix.cpp index ee76352821..ff9259749a 100644 --- a/controller_interface/test/test_controller_tf_prefix.cpp +++ b/controller_interface/test/test_controller_tf_prefix.cpp @@ -22,12 +22,6 @@ TEST_F(TestControllerTFPrefix, EmptyPrefixReturnsEmpty) EXPECT_EQ(controller_interface::resolve_tf_prefix("", "/ns"), ""); } -TEST_F(TestControllerTFPrefix, TildePrefixUsesNamespace) -{ - EXPECT_EQ(controller_interface::resolve_tf_prefix("~", "/ns"), "ns/"); - EXPECT_EQ(controller_interface::resolve_tf_prefix("~", "/ns/"), "ns/"); -} - TEST_F(TestControllerTFPrefix, ExplicitPrefixUsed) { EXPECT_EQ(controller_interface::resolve_tf_prefix("robot", "/ns"), "robot/"); @@ -40,3 +34,11 @@ TEST_F(TestControllerTFPrefix, NormalizePrefixSlashes) 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/"); +}