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/tf_prefix.hpp b/controller_interface/include/controller_interface/tf_prefix.hpp new file mode 100644 index 0000000000..3f769b10dc --- /dev/null +++ b/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 + +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_ 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..95be5f8cde --- /dev/null +++ b/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 + +#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/"); +} 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_