Skip to content

Commit 496e39d

Browse files
qinqonchristophfroehlichARK3r
authored
Remove front_steering from steering library (#1166)
Signed-off-by: Quique Llorente <[email protected]> Co-authored-by: Christoph Froehlich <[email protected]> Co-authored-by: Reza Kermani <[email protected]>
1 parent 646b7b3 commit 496e39d

32 files changed

+749
-422
lines changed

‎ackermann_steering_controller/src/ackermann_steering_controller.cpp‎

Lines changed: 67 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <limits>
16+
1517
#include "ackermann_steering_controller/ackermann_steering_controller.hpp"
1618

1719
namespace ackermann_steering_controller
@@ -31,21 +33,77 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom
3133
{
3234
ackermann_params_ = ackermann_param_listener_->get_params();
3335

34-
const double front_wheels_radius = ackermann_params_.front_wheels_radius;
35-
const double rear_wheels_radius = ackermann_params_.rear_wheels_radius;
36-
const double front_wheel_track = ackermann_params_.front_wheel_track;
37-
const double rear_wheel_track = ackermann_params_.rear_wheel_track;
38-
const double wheelbase = ackermann_params_.wheelbase;
36+
// TODO(anyone): Remove deprecated parameters
37+
// START OF DEPRECATED
38+
if (ackermann_params_.front_wheels_radius > 0.0)
39+
{
40+
RCLCPP_WARN(
41+
get_node()->get_logger(),
42+
"DEPRECATED parameter 'front_wheel_radius', set 'traction_wheels_radius' instead");
43+
ackermann_params_.traction_wheels_radius = ackermann_params_.front_wheels_radius;
44+
}
3945

40-
if (params_.front_steering)
46+
if (ackermann_params_.rear_wheels_radius > 0.0)
4147
{
42-
odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track);
48+
RCLCPP_WARN(
49+
get_node()->get_logger(),
50+
"DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheels_radius' instead");
51+
ackermann_params_.traction_wheels_radius = ackermann_params_.rear_wheels_radius;
4352
}
44-
else
53+
54+
if (ackermann_params_.front_wheel_track > 0.0)
4555
{
46-
odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track);
56+
RCLCPP_WARN(
57+
get_node()->get_logger(),
58+
"DEPRECATED parameter 'front_wheel_track', set 'traction_track_width' or "
59+
"'steering_track_width' instead");
60+
if (params_.front_steering)
61+
{
62+
ackermann_params_.steering_track_width = ackermann_params_.front_wheel_track;
63+
}
64+
else
65+
{
66+
ackermann_params_.traction_track_width = ackermann_params_.front_wheel_track;
67+
}
4768
}
4869

70+
if (ackermann_params_.rear_wheel_track > 0.0)
71+
{
72+
RCLCPP_WARN(
73+
get_node()->get_logger(),
74+
"DEPRECATED parameter 'rear_wheel_track', set 'traction_track_width' or "
75+
"'steering_track_width' instead");
76+
if (params_.front_steering)
77+
{
78+
ackermann_params_.traction_track_width = ackermann_params_.rear_wheel_track;
79+
}
80+
else
81+
{
82+
ackermann_params_.steering_track_width = ackermann_params_.rear_wheel_track;
83+
}
84+
}
85+
86+
if (ackermann_params_.traction_wheels_radius <= std::numeric_limits<double>::epsilon())
87+
{
88+
RCLCPP_FATAL(
89+
get_node()->get_logger(),
90+
"parameter 'traction_wheels_radius' is not set, cannot configure odometry");
91+
return controller_interface::CallbackReturn::ERROR;
92+
}
93+
// END OF DEPRECATED
94+
95+
if (ackermann_params_.steering_track_width <= std::numeric_limits<double>::epsilon())
96+
{
97+
ackermann_params_.steering_track_width = ackermann_params_.traction_track_width;
98+
}
99+
100+
const double traction_wheels_radius = ackermann_params_.traction_wheels_radius;
101+
const double traction_track_width = ackermann_params_.traction_track_width;
102+
const double steering_track_width = ackermann_params_.steering_track_width;
103+
const double wheelbase = ackermann_params_.wheelbase;
104+
105+
odometry_.set_wheel_params(
106+
traction_wheels_radius, wheelbase, steering_track_width, traction_track_width);
49107
odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
50108

51109
set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);

‎ackermann_steering_controller/src/ackermann_steering_controller.yaml‎

Lines changed: 37 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,24 +1,39 @@
11
ackermann_steering_controller:
2+
23
front_wheel_track:
34
{
45
type: double,
56
default_value: 0.0,
6-
description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
7+
description: "DEPRECATED: use 'traction_track_width' or 'steering_track_width'",
8+
read_only: false,
9+
}
10+
11+
steering_track_width:
12+
{
13+
type: double,
14+
default_value: 0.0,
15+
description: "(Optional) Steering wheel track length. If not set, 'traction_track_width' will be used.",
716
read_only: false,
8-
validation: {
9-
gt<>: [0.0]
10-
}
1117
}
1218

1319
rear_wheel_track:
1420
{
1521
type: double,
1622
default_value: 0.0,
17-
description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
23+
description: "DEPRECATED: use 'traction_track_width' or 'steering_track_width'",
1824
read_only: false,
19-
validation: {
20-
gt<>: [0.0]
21-
}
25+
}
26+
27+
traction_track_width:
28+
{
29+
type: double,
30+
default_value: 0.0,
31+
description: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
32+
read_only: false,
33+
# TODO(anyone): activate validation if front/rear wheel track is removed
34+
# validation: {
35+
# gt<>: [0.0]
36+
# }
2237
}
2338

2439
wheelbase:
@@ -32,24 +47,30 @@ ackermann_steering_controller:
3247
}
3348
}
3449

50+
traction_wheels_radius:
51+
{
52+
type: double,
53+
default_value: 0.0,
54+
description: "Traction wheels radius.",
55+
read_only: false,
56+
# TODO(anyone): activate validation if front/rear wheel radius is removed
57+
# validation: {
58+
# gt<>: [0.0]
59+
# }
60+
}
61+
3562
front_wheels_radius:
3663
{
3764
type: double,
3865
default_value: 0.0,
39-
description: "Front wheels radius.",
66+
description: "DEPRECATED: use 'traction_wheels_radius'",
4067
read_only: false,
41-
validation: {
42-
gt<>: [0.0]
43-
}
4468
}
4569

4670
rear_wheels_radius:
4771
{
4872
type: double,
4973
default_value: 0.0,
50-
description: "Rear wheels radius.",
74+
description: "DEPRECATED: use 'traction_wheels_radius'",
5175
read_only: false,
52-
validation: {
53-
gt<>: [0.0]
54-
}
5576
}

‎ackermann_steering_controller/test/ackermann_steering_controller_params.yaml‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -2,15 +2,12 @@ test_ackermann_steering_controller:
22
ros__parameters:
33

44
reference_timeout: 2.0
5-
front_steering: true
65
open_loop: false
76
velocity_rolling_window_size: 10
87
position_feedback: false
9-
rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
10-
front_wheels_names: [front_right_steering_joint, front_left_steering_joint]
8+
traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint]
9+
steering_joints_names: [front_right_steering_joint, front_left_steering_joint]
1110

1211
wheelbase: 3.24644
13-
front_wheel_track: 2.12321
14-
rear_wheel_track: 1.76868
15-
front_wheels_radius: 0.45
16-
rear_wheels_radius: 0.45
12+
traction_track_width: 1.76868
13+
traction_wheels_radius: 0.45

‎ackermann_steering_controller/test/ackermann_steering_controller_preceding_params.yaml‎

Lines changed: 6 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,13 @@
11
test_ackermann_steering_controller:
22
ros__parameters:
33
reference_timeout: 2.0
4-
front_steering: true
54
open_loop: false
65
velocity_rolling_window_size: 10
76
position_feedback: false
8-
rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint]
9-
front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint]
10-
rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint]
11-
front_wheels_state_names: [front_right_steering_joint, front_left_steering_joint]
7+
traction_joints_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint]
8+
steering_joints_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint]
9+
traction_joints_state_names: [rear_right_wheel_joint, rear_left_wheel_joint]
10+
steering_joints_state_names: [front_right_steering_joint, front_left_steering_joint]
1211
wheelbase: 3.24644
13-
front_wheel_track: 2.12321
14-
rear_wheel_track: 1.76868
15-
front_wheels_radius: 0.45
16-
rear_wheels_radius: 0.45
12+
traction_track_width: 1.76868
13+
traction_wheels_radius: 0.45

‎ackermann_steering_controller/test/test_ackermann_steering_controller.cpp‎

Lines changed: 12 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -31,18 +31,15 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success)
3131
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
3232

3333
ASSERT_THAT(
34-
controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_));
34+
controller_->params_.traction_joints_names, testing::ElementsAreArray(traction_joints_names_));
3535
ASSERT_THAT(
36-
controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_));
37-
ASSERT_EQ(controller_->params_.front_steering, front_steering_);
36+
controller_->params_.steering_joints_names, testing::ElementsAreArray(steering_joints_names_));
3837
ASSERT_EQ(controller_->params_.open_loop, open_loop_);
3938
ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_);
4039
ASSERT_EQ(controller_->params_.position_feedback, position_feedback_);
4140
ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_);
42-
ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_);
43-
ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_);
44-
ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_);
45-
ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_);
41+
ASSERT_EQ(controller_->ackermann_params_.traction_wheels_radius, traction_wheels_radius_);
42+
ASSERT_EQ(controller_->ackermann_params_.traction_track_width, traction_track_width_);
4643
}
4744

4845
TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
@@ -55,32 +52,32 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
5552
ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
5653
EXPECT_EQ(
5754
cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL],
58-
rear_wheels_names_[0] + "/" + traction_interface_name_);
55+
traction_joints_names_[0] + "/" + traction_interface_name_);
5956
EXPECT_EQ(
6057
cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL],
61-
rear_wheels_names_[1] + "/" + traction_interface_name_);
58+
traction_joints_names_[1] + "/" + traction_interface_name_);
6259
EXPECT_EQ(
6360
cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL],
64-
front_wheels_names_[0] + "/" + steering_interface_name_);
61+
steering_joints_names_[0] + "/" + steering_interface_name_);
6562
EXPECT_EQ(
6663
cmd_if_conf.names[CMD_STEER_LEFT_WHEEL],
67-
front_wheels_names_[1] + "/" + steering_interface_name_);
64+
steering_joints_names_[1] + "/" + steering_interface_name_);
6865
EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
6966

7067
auto state_if_conf = controller_->state_interface_configuration();
7168
ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size());
7269
EXPECT_EQ(
7370
state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL],
74-
controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_);
71+
controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_);
7572
EXPECT_EQ(
7673
state_if_conf.names[STATE_TRACTION_LEFT_WHEEL],
77-
controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_);
74+
controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_);
7875
EXPECT_EQ(
7976
state_if_conf.names[STATE_STEER_RIGHT_WHEEL],
80-
controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_);
77+
controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_);
8178
EXPECT_EQ(
8279
state_if_conf.names[STATE_STEER_LEFT_WHEEL],
83-
controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_);
80+
controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_);
8481
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
8582

8683
// check ref itfs

‎ackermann_steering_controller/test/test_ackermann_steering_controller.hpp‎

Lines changed: 17 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -152,25 +152,25 @@ class AckermannSteeringControllerFixture : public ::testing::Test
152152

153153
command_itfs_.emplace_back(
154154
hardware_interface::CommandInterface(
155-
rear_wheels_names_[0], traction_interface_name_,
155+
traction_joints_names_[0], traction_interface_name_,
156156
&joint_command_values_[CMD_TRACTION_RIGHT_WHEEL]));
157157
command_ifs.emplace_back(command_itfs_.back());
158158

159159
command_itfs_.emplace_back(
160160
hardware_interface::CommandInterface(
161-
rear_wheels_names_[1], steering_interface_name_,
161+
traction_joints_names_[1], steering_interface_name_,
162162
&joint_command_values_[CMD_TRACTION_LEFT_WHEEL]));
163163
command_ifs.emplace_back(command_itfs_.back());
164164

165165
command_itfs_.emplace_back(
166166
hardware_interface::CommandInterface(
167-
front_wheels_names_[0], steering_interface_name_,
167+
steering_joints_names_[0], steering_interface_name_,
168168
&joint_command_values_[CMD_STEER_RIGHT_WHEEL]));
169169
command_ifs.emplace_back(command_itfs_.back());
170170

171171
command_itfs_.emplace_back(
172172
hardware_interface::CommandInterface(
173-
front_wheels_names_[1], steering_interface_name_,
173+
steering_joints_names_[1], steering_interface_name_,
174174
&joint_command_values_[CMD_STEER_LEFT_WHEEL]));
175175
command_ifs.emplace_back(command_itfs_.back());
176176

@@ -180,25 +180,25 @@ class AckermannSteeringControllerFixture : public ::testing::Test
180180

181181
state_itfs_.emplace_back(
182182
hardware_interface::StateInterface(
183-
rear_wheels_names_[0], traction_interface_name_,
183+
traction_joints_names_[0], traction_interface_name_,
184184
&joint_state_values_[STATE_TRACTION_RIGHT_WHEEL]));
185185
state_ifs.emplace_back(state_itfs_.back());
186186

187187
state_itfs_.emplace_back(
188188
hardware_interface::StateInterface(
189-
rear_wheels_names_[1], traction_interface_name_,
189+
traction_joints_names_[1], traction_interface_name_,
190190
&joint_state_values_[STATE_TRACTION_LEFT_WHEEL]));
191191
state_ifs.emplace_back(state_itfs_.back());
192192

193193
state_itfs_.emplace_back(
194194
hardware_interface::StateInterface(
195-
front_wheels_names_[0], steering_interface_name_,
195+
steering_joints_names_[0], steering_interface_name_,
196196
&joint_state_values_[STATE_STEER_RIGHT_WHEEL]));
197197
state_ifs.emplace_back(state_itfs_.back());
198198

199199
state_itfs_.emplace_back(
200200
hardware_interface::StateInterface(
201-
front_wheels_names_[1], steering_interface_name_,
201+
steering_joints_names_[1], steering_interface_name_,
202202
&joint_state_values_[STATE_STEER_LEFT_WHEEL]));
203203
state_ifs.emplace_back(state_itfs_.back());
204204

@@ -277,29 +277,25 @@ class AckermannSteeringControllerFixture : public ::testing::Test
277277
protected:
278278
// Controller-related parameters
279279
double reference_timeout_ = 2.0;
280-
bool front_steering_ = true;
281280
bool open_loop_ = false;
282281
unsigned int velocity_rolling_window_size_ = 10;
283282
bool position_feedback_ = false;
284-
std::vector<std::string> rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"};
285-
std::vector<std::string> front_wheels_names_ = {
283+
std::vector<std::string> traction_joints_names_ = {
284+
"rear_right_wheel_joint", "rear_left_wheel_joint"};
285+
std::vector<std::string> steering_joints_names_ = {
286286
"front_right_steering_joint", "front_left_steering_joint"};
287287
std::vector<std::string> joint_names_ = {
288-
rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]};
288+
traction_joints_names_[0], traction_joints_names_[1], steering_joints_names_[0],
289+
steering_joints_names_[1]};
289290

290-
std::vector<std::string> rear_wheels_preceding_names_ = {
291+
std::vector<std::string> traction_joints_preceding_names_ = {
291292
"pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"};
292-
std::vector<std::string> front_wheels_preceding_names_ = {
293+
std::vector<std::string> steering_joints_preceding_names_ = {
293294
"pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"};
294-
std::vector<std::string> preceding_joint_names_ = {
295-
rear_wheels_preceding_names_[0], rear_wheels_preceding_names_[1],
296-
front_wheels_preceding_names_[0], front_wheels_preceding_names_[1]};
297295

298296
double wheelbase_ = 3.24644;
299-
double front_wheel_track_ = 2.12321;
300-
double rear_wheel_track_ = 1.76868;
301-
double front_wheels_radius_ = 0.45;
302-
double rear_wheels_radius_ = 0.45;
297+
double traction_track_width_ = 1.76868;
298+
double traction_wheels_radius_ = 0.45;
303299

304300
std::array<double, 4> joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}};
305301
std::array<double, 4> joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}};

0 commit comments

Comments
 (0)