Skip to content

Commit 14ddfc3

Browse files
kuralmekuralme
committed
TF prefix helper added
1 parent f5a3f2f commit 14ddfc3

File tree

3 files changed

+162
-0
lines changed

3 files changed

+162
-0
lines changed

‎control_toolbox/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -183,6 +183,9 @@ if(BUILD_TESTING)
183183
ament_add_gmock(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp)
184184
target_link_libraries(pid_ros_publisher_tests control_toolbox rclcpp_lifecycle::rclcpp_lifecycle)
185185

186+
ament_add_gmock(tf_utils_tests test/tf_utils_tests.cpp)
187+
target_link_libraries(tf_utils_tests control_toolbox)
188+
186189
## Control Filters
187190
### Gravity Compensation
188191
add_rostest_with_parameters_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp

‎control_toolbox/include/control_toolbox/tf_utils.hpp‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
// Copyright (c) 2008, Willow Garage, Inc.
2+
// All rights reserved.
3+
//
4+
// Software License Agreement (BSD License 2.0)
5+
//
6+
// Redistribution and use in source and binary forms, with or without
7+
// modification, are permitted provided that the following conditions
8+
// are met:
9+
//
10+
// * Redistributions of source code must retain the above copyright
11+
// notice, this list of conditions and the following disclaimer.
12+
// * Redistributions in binary form must reproduce the above
13+
// copyright notice, this list of conditions and the following
14+
// disclaimer in the documentation and/or other materials provided
15+
// with the distribution.
16+
// * Neither the name of the Willow Garage nor the names of its
17+
// contributors may be used to endorse or promote products derived
18+
// from this software without specific prior written permission.
19+
//
20+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21+
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22+
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23+
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24+
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25+
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26+
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27+
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28+
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29+
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30+
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31+
// POSSIBILITY OF SUCH DAMAGE.
32+
33+
#ifndef CONTROL_TOOLBOX__TF_UTILS_HPP_
34+
#define CONTROL_TOOLBOX__TF_UTILS_HPP_
35+
36+
#include <string>
37+
38+
#include <rclcpp/rclcpp.hpp>
39+
40+
namespace control_toolbox
41+
{
42+
/**
43+
* @brief Apply a TF prefix to a given frame.
44+
* @param tf_prefix_enabled Whether to apply the TF prefix
45+
* @param prefix TF prefix
46+
* @param frame Frame name
47+
* @param node_ns Node namespace to use as prefix if prefix is empty
48+
* @return The prefixed frame name if prefix is not empty, otherwise the original frame name
49+
*/
50+
inline std::string applyTFPrefix(
51+
bool tf_prefix_enabled, std::string prefix, const std::string & node_ns,
52+
const std::string & frame)
53+
{
54+
if (!tf_prefix_enabled)
55+
{
56+
return frame;
57+
}
58+
59+
// fallback to node namespace if explicit prefix not set
60+
if (prefix.empty())
61+
{
62+
prefix = node_ns;
63+
}
64+
65+
// normalize: remove leading '/' and ensure trailing '/'
66+
if (!prefix.empty())
67+
{
68+
if (prefix.front() == '/')
69+
{
70+
prefix.erase(0, 1);
71+
}
72+
if (prefix.back() != '/')
73+
{
74+
prefix.push_back('/');
75+
}
76+
}
77+
78+
return prefix + frame;
79+
}
80+
81+
} // namespace control_toolbox
82+
83+
#endif // CONTROL_TOOLBOX__TF_UTILS_HPP_

‎control_toolbox/test/tf_utils_tests.cpp‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
// Copyright (c) 2008, Willow Garage, Inc.
2+
// All rights reserved.
3+
//
4+
// Software License Agreement (BSD License 2.0)
5+
//
6+
// Redistribution and use in source and binary forms, with or without
7+
// modification, are permitted provided that the following conditions
8+
// are met:
9+
//
10+
// * Redistributions of source code must retain the above copyright
11+
// notice, this list of conditions and the following disclaimer.
12+
// * Redistributions in binary form must reproduce the above
13+
// copyright notice, this list of conditions and the following
14+
// disclaimer in the documentation and/or other materials provided
15+
// with the distribution.
16+
// * Neither the name of the Willow Garage nor the names of its
17+
// contributors may be used to endorse or promote products derived
18+
// from this software without specific prior written permission.
19+
//
20+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21+
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22+
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23+
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24+
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25+
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26+
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27+
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28+
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29+
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30+
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31+
// POSSIBILITY OF SUCH DAMAGE.
32+
33+
#include <gmock/gmock.h>
34+
#include "control_toolbox/tf_utils.hpp"
35+
36+
TEST(ApplyTFPrefixTest, DisabledPrefix)
37+
{
38+
EXPECT_EQ(control_toolbox::applyTFPrefix(false, "", "/ns", "base_link"), "base_link");
39+
}
40+
41+
TEST(ApplyTFPrefixTest, EmptyExplicitUsesNamespace)
42+
{
43+
EXPECT_EQ(control_toolbox::applyTFPrefix(true, "", "/my_ns", "odom"), "my_ns/odom");
44+
}
45+
46+
TEST(ApplyTFPrefixTest, ExplicitPrefixUsed)
47+
{
48+
EXPECT_EQ(control_toolbox::applyTFPrefix(true, "robot1", "/ns", "base"), "robot1/base");
49+
}
50+
51+
TEST(ApplyTFPrefixTest, LeadingSlashRemoved)
52+
{
53+
EXPECT_EQ(control_toolbox::applyTFPrefix(true, "/robot2", "/ns", "link"), "robot2/link");
54+
}
55+
56+
TEST(ApplyTFPrefixTest, TrailingSlashAdded)
57+
{
58+
EXPECT_EQ(control_toolbox::applyTFPrefix(true, "robot3", "/ns", "odom"), "robot3/odom");
59+
}
60+
61+
TEST(ApplyTFPrefixTest, BothSlashesNormalized)
62+
{
63+
EXPECT_EQ(
64+
control_toolbox::applyTFPrefix(true, "/robot4/", "/ns", "base_link"), "robot4/base_link");
65+
}
66+
67+
TEST(ApplyTFPrefixTest, NodeNamespaceWithSlash)
68+
{
69+
EXPECT_EQ(control_toolbox::applyTFPrefix(true, "", "/robot_ns/", "odom"), "robot_ns/odom");
70+
}
71+
72+
int main(int argc, char ** argv)
73+
{
74+
testing::InitGoogleTest(&argc, argv);
75+
return RUN_ALL_TESTS();
76+
}

0 commit comments

Comments
 (0)