Skip to content

Commit cd6823b

Browse files
kuralmekuralme
authored
tf prefix helper used in steering controllers library (#2080)
1 parent 5cbd8c4 commit cd6823b

File tree

5 files changed

+140
-5
lines changed

5 files changed

+140
-5
lines changed

‎doc/release_notes.rst‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,3 +39,8 @@ joint_trajectory_controller
3939
don't contain velocity / acceleration information, but the trajectory does. This way, the segment
4040
up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043
4141
<https://github.com/ros-controls/ros2_controllers/pull/2043>`_)
42+
43+
44+
steering_controllers_library
45+
*****************************
46+
* Parameter ``tf_frame_prefix`` added with the similar functionality to other controllers. (`#2080 <https://github.com/ros-controls/ros2_controllers/pull/2080>`_).

‎steering_controllers_library/src/steering_controllers_library.cpp‎

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <utility>
2121
#include <vector>
2222

23+
#include "controller_interface/tf_prefix.hpp"
2324
#include "hardware_interface/types/hardware_interface_type_values.hpp"
2425
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
2526

@@ -275,9 +276,18 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
275276
return controller_interface::CallbackReturn::ERROR;
276277
}
277278

279+
// resolve prefix: substitute tilde (~) with the namespace if contains and normalize slashes (/)
280+
// Note: resolve_tf_prefix handles empty tf_frame_prefix by returning an empty string
281+
const std::string tf_prefix =
282+
controller_interface::resolve_tf_prefix(params_.tf_frame_prefix, get_node()->get_namespace());
283+
284+
// prepend resolved TF prefix to frame ids
285+
const std::string odom_frame_id = tf_prefix + params_.odom_frame_id;
286+
const std::string base_frame_id = tf_prefix + params_.base_frame_id;
287+
278288
odom_state_msg_.header.stamp = get_node()->now();
279-
odom_state_msg_.header.frame_id = params_.odom_frame_id;
280-
odom_state_msg_.child_frame_id = params_.base_frame_id;
289+
odom_state_msg_.header.frame_id = odom_frame_id;
290+
odom_state_msg_.child_frame_id = base_frame_id;
281291
odom_state_msg_.pose.pose.position.z = 0;
282292

283293
auto & covariance = odom_state_msg_.twist.covariance;

‎steering_controllers_library/src/steering_controllers_library.yaml‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -93,6 +93,14 @@ steering_controllers_library:
9393
read_only: false,
9494
}
9595

96+
tf_frame_prefix:
97+
{
98+
type: string,
99+
default_value: "",
100+
description: "(optional) Prefix to be prepended to odom_id and base_frame_id tf frames before publishing. Tilde('~') character in this prefix will be replaced with the node namespace.",
101+
read_only: true,
102+
}
103+
96104
twist_covariance_diagonal: {
97105
type: double_array,
98106
default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0],

‎steering_controllers_library/test/test_steering_controllers_library.cpp‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,112 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces)
7979
}
8080
}
8181

82+
// TF prefix tests
83+
TEST_F(SteeringControllersLibraryTest, configure_succeeds_tf_prefix_no_namespace)
84+
{
85+
std::string odom_id = "odom";
86+
std::string base_link_id = "base_link";
87+
std::string frame_prefix = "test_prefix";
88+
89+
auto node_options = controller_->define_custom_node_options();
90+
node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
91+
node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
92+
node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
93+
94+
SetUpController("test_steering_controllers_library", node_options);
95+
96+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
97+
98+
auto odometry_message = controller_->odom_state_msg_;
99+
std::string test_odom_frame_id = odometry_message.header.frame_id;
100+
std::string test_base_frame_id = odometry_message.child_frame_id;
101+
102+
// frame_prefix is not blank so should be prepended to the frame id's
103+
ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id);
104+
ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id);
105+
}
106+
TEST_F(SteeringControllersLibraryTest, configure_succeeds_tf_blank_prefix_no_namespace)
107+
{
108+
std::string odom_id = "odom";
109+
std::string base_link_id = "base_link";
110+
std::string frame_prefix = "";
111+
112+
auto node_options = controller_->define_custom_node_options();
113+
node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
114+
node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
115+
node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
116+
117+
SetUpController("test_steering_controllers_library", node_options);
118+
119+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
120+
121+
auto odometry_message = controller_->odom_state_msg_;
122+
std::string test_odom_frame_id = odometry_message.header.frame_id;
123+
std::string test_base_frame_id = odometry_message.child_frame_id;
124+
125+
// frame_prefix is blank so nothing added to the frame id's
126+
ASSERT_EQ(test_odom_frame_id, odom_id);
127+
ASSERT_EQ(test_base_frame_id, base_link_id);
128+
}
129+
TEST_F(SteeringControllersLibraryTest, configure_succeeds_tf_prefix_set_namespace)
130+
{
131+
std::string test_namespace = "/test_namespace";
132+
133+
std::string odom_id = "odom";
134+
std::string base_link_id = "base_link";
135+
std::string frame_prefix = "test_prefix";
136+
137+
auto node_options = controller_->define_custom_node_options();
138+
node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
139+
node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
140+
node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
141+
node_options.append_parameter_override(
142+
"traction_joints_names", std::vector<std::string>{"joint1", "joint2"});
143+
node_options.append_parameter_override(
144+
"steering_joints_names", std::vector<std::string>{"joint3", "joint4"});
145+
146+
SetUpController("test_steering_controllers_library", node_options, test_namespace);
147+
148+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
149+
150+
auto odometry_message = controller_->odom_state_msg_;
151+
std::string test_odom_frame_id = odometry_message.header.frame_id;
152+
std::string test_base_frame_id = odometry_message.child_frame_id;
153+
154+
// frame_prefix is not blank so should be prepended to the frame id's instead of the namespace
155+
ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id);
156+
ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id);
157+
}
158+
TEST_F(SteeringControllersLibraryTest, configure_succeeds_tf_tilde_prefix_set_namespace)
159+
{
160+
std::string test_namespace = "/test_namespace";
161+
std::string odom_id = "odom";
162+
std::string base_link_id = "base_link";
163+
std::string frame_prefix = "~";
164+
165+
auto node_options = controller_->define_custom_node_options();
166+
node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
167+
node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
168+
node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
169+
node_options.append_parameter_override(
170+
"traction_joints_names", std::vector<std::string>{"joint1", "joint2"});
171+
node_options.append_parameter_override(
172+
"steering_joints_names", std::vector<std::string>{"joint3", "joint4"});
173+
174+
SetUpController("test_steering_controllers_library", node_options, test_namespace);
175+
176+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
177+
178+
auto odometry_message = controller_->odom_state_msg_;
179+
std::string test_odom_frame_id = odometry_message.header.frame_id;
180+
std::string test_base_frame_id = odometry_message.child_frame_id;
181+
std::string ns_prefix = test_namespace.erase(0, 1) + "/";
182+
183+
// frame_prefix has tilde (~) character so node namespace should be prepended to the frame id's
184+
ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id);
185+
ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id);
186+
}
187+
82188
// Tests controller update_reference_from_subscribers and
83189
// for position_feedback behavior
84190
// when too old msg is sent i.e age_of_last_command > ref_timeout case

‎steering_controllers_library/test/test_steering_controllers_library.hpp‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,10 @@ class TestableSteeringControllersLibrary
7070
: public steering_controllers_library::SteeringControllersLibrary
7171
{
7272
FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces);
73+
FRIEND_TEST(SteeringControllersLibraryTest, configure_succeeds_tf_prefix_no_namespace);
74+
FRIEND_TEST(SteeringControllersLibraryTest, configure_succeeds_tf_blank_prefix_no_namespace);
75+
FRIEND_TEST(SteeringControllersLibraryTest, configure_succeeds_tf_prefix_set_namespace);
76+
FRIEND_TEST(SteeringControllersLibraryTest, configure_succeeds_tf_tilde_prefix_set_namespace);
7377
FRIEND_TEST(SteeringControllersLibraryTest, test_position_feedback_ref_timeout);
7478
FRIEND_TEST(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout);
7579

@@ -151,14 +155,16 @@ class SteeringControllersLibraryFixture : public ::testing::Test
151155
void TearDown() { controller_.reset(nullptr); }
152156

153157
protected:
154-
void SetUpController(const std::string controller_name = "test_steering_controllers_library")
158+
void SetUpController(
159+
const std::string controller_name = "test_steering_controllers_library",
160+
const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions(), const std::string ns = "")
155161
{
156162
controller_interface::ControllerInterfaceParams params;
157163
params.controller_name = controller_name;
158164
params.robot_description = "";
159165
params.update_rate = 0;
160-
params.node_namespace = "";
161-
params.node_options = controller_->define_custom_node_options();
166+
params.node_namespace = ns;
167+
params.node_options = node_options;
162168
ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK);
163169

164170
if (position_feedback_ == true)

0 commit comments

Comments
 (0)