Skip to content

Commit ed51226

Browse files
qinqonqinqon
committed
[STEERING]: Add swerve drive
Signed-off-by: Quique Llorente <[email protected]>
1 parent b245155 commit ed51226

File tree

43 files changed

+1857
-346
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

43 files changed

+1857
-346
lines changed

‎ackermann_steering_controller/src/ackermann_steering_controller.cpp

Lines changed: 3 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -31,21 +31,11 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom
3131
{
3232
ackermann_params_ = ackermann_param_listener_->get_params();
3333

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;
34+
const double traction_wheels_radius = ackermann_params_.traction_wheels_radius;
35+
const double traction_wheel_track = ackermann_params_.traction_wheel_track;
3836
const double wheelbase = ackermann_params_.wheelbase;
3937

40-
if (params_.front_steering)
41-
{
42-
odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track);
43-
}
44-
else
45-
{
46-
odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track);
47-
}
48-
38+
odometry_.set_wheel_params(traction_wheels_radius, wheelbase, traction_wheel_track);
4939
odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
5040

5141
set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);

‎ackermann_steering_controller/src/ackermann_steering_controller.yaml

Lines changed: 4 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,9 @@
11
ackermann_steering_controller:
2-
front_wheel_track:
2+
traction_wheel_track:
33
{
44
type: double,
55
default_value: 0.0,
6-
description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
7-
read_only: false,
8-
validation: {
9-
gt<>: [0.0]
10-
}
11-
}
12-
13-
rear_wheel_track:
14-
{
15-
type: double,
16-
default_value: 0.0,
17-
description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
6+
description: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
187
read_only: false,
198
validation: {
209
gt<>: [0.0]
@@ -32,22 +21,11 @@ ackermann_steering_controller:
3221
}
3322
}
3423

35-
front_wheels_radius:
36-
{
37-
type: double,
38-
default_value: 0.0,
39-
description: "Front wheels radius.",
40-
read_only: false,
41-
validation: {
42-
gt<>: [0.0]
43-
}
44-
}
45-
46-
rear_wheels_radius:
24+
traction_wheels_radius:
4725
{
4826
type: double,
4927
default_value: 0.0,
50-
description: "Rear wheels radius.",
28+
description: "Traction wheels radius.",
5129
read_only: false,
5230
validation: {
5331
gt<>: [0.0]

‎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+
wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
9+
steers_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_wheel_track: 1.76868
13+
traction_wheels_radius: 0.45

‎ackermann_steering_controller/test/ackermann_steering_controller_preceeding_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+
wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint]
8+
steers_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint]
9+
wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint]
10+
steers_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_wheel_track: 1.76868
13+
traction_wheels_radius: 0.45

‎ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp

Lines changed: 6 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -32,20 +32,17 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success)
3232
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
3333

3434
ASSERT_THAT(
35-
controller_->params_.rear_wheels_names,
36-
testing::ElementsAreArray(rear_wheels_preceeding_names_));
35+
controller_->params_.wheels_names,
36+
testing::ElementsAreArray(wheels_preceeding_names_));
3737
ASSERT_THAT(
38-
controller_->params_.front_wheels_names,
39-
testing::ElementsAreArray(front_wheels_preceeding_names_));
40-
ASSERT_EQ(controller_->params_.front_steering, front_steering_);
38+
controller_->params_.steers_names,
39+
testing::ElementsAreArray(steers_preceeding_names_));
4140
ASSERT_EQ(controller_->params_.open_loop, open_loop_);
4241
ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_);
4342
ASSERT_EQ(controller_->params_.position_feedback, position_feedback_);
4443
ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_);
45-
ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_);
46-
ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_);
47-
ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_);
48-
ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_);
44+
ASSERT_EQ(controller_->ackermann_params_.traction_wheels_radius, traction_wheels_radius_);
45+
ASSERT_EQ(controller_->ackermann_params_.traction_wheel_track, traction_wheel_track_);
4946
}
5047

5148
TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)

‎bicycle_steering_controller/src/bicycle_steering_controller.cpp

Lines changed: 2 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -32,18 +32,9 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet
3232
bicycle_params_ = bicycle_param_listener_->get_params();
3333

3434
const double wheelbase = bicycle_params_.wheelbase;
35-
const double front_wheel_radius = bicycle_params_.front_wheel_radius;
36-
const double rear_wheel_radius = bicycle_params_.rear_wheel_radius;
37-
38-
if (params_.front_steering)
39-
{
40-
odometry_.set_wheel_params(rear_wheel_radius, wheelbase);
41-
}
42-
else
43-
{
44-
odometry_.set_wheel_params(front_wheel_radius, wheelbase);
45-
}
35+
const double traction_wheel_radius = bicycle_params_.traction_wheel_radius;
4636

37+
odometry_.set_wheel_params(traction_wheel_radius, wheelbase);
4738
odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG);
4839

4940
set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);

‎bicycle_steering_controller/src/bicycle_steering_controller.yaml

Lines changed: 2 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -10,22 +10,11 @@ bicycle_steering_controller:
1010
}
1111
}
1212

13-
front_wheel_radius:
13+
traction_wheel_radius:
1414
{
1515
type: double,
1616
default_value: 0.0,
17-
description: "Front wheel radius.",
18-
read_only: false,
19-
validation: {
20-
gt<>: [0.0]
21-
}
22-
}
23-
24-
rear_wheel_radius:
25-
{
26-
type: double,
27-
default_value: 0.0,
28-
description: "Rear wheel radius.",
17+
description: "Steering wheel radius.",
2918
read_only: false,
3019
validation: {
3120
gt<>: [0.0]

‎bicycle_steering_controller/test/bicycle_steering_controller_params.yaml

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,13 +2,11 @@ test_bicycle_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_wheel_joint]
10-
front_wheels_names: [steering_axis_joint]
8+
wheels_names: [rear_wheel_joint]
9+
steers_names: [steering_axis_joint]
1110

1211
wheelbase: 3.24644
13-
front_wheel_radius: 0.45
14-
rear_wheel_radius: 0.45
12+
traction_wheel_radius: 0.45

‎bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml

Lines changed: 5 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,13 @@
11
test_bicycle_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_wheel_joint]
9-
front_wheels_names: [pid_controller/steering_axis_joint]
10-
rear_wheels_state_names: [rear_wheel_joint]
11-
front_wheels_state_names: [steering_axis_joint]
7+
wheels_names: [pid_controller/rear_wheel_joint]
8+
steers_names: [pid_controller/steering_axis_joint]
9+
wheels_state_names: [rear_wheel_joint]
10+
steers_state_names: [steering_axis_joint]
1211

1312
wheelbase: 3.24644
14-
front_wheel_radius: 0.45
15-
rear_wheel_radius: 0.45
13+
traction_wheel_radius: 0.45

‎bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -32,16 +32,14 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success)
3232
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
3333

3434
ASSERT_THAT(
35-
controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_));
35+
controller_->params_.wheels_names, testing::ElementsAreArray(wheels_names_));
3636
ASSERT_THAT(
37-
controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_));
38-
ASSERT_EQ(controller_->params_.front_steering, front_steering_);
37+
controller_->params_.steers_names, testing::ElementsAreArray(steers_names_));
3938
ASSERT_EQ(controller_->params_.open_loop, open_loop_);
4039
ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_);
4140
ASSERT_EQ(controller_->params_.position_feedback, position_feedback_);
4241
ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_);
43-
ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_);
44-
ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_);
42+
ASSERT_EQ(controller_->bicycle_params_.traction_wheel_radius, traction_wheel_radius_);
4543
}
4644

4745
TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)
@@ -53,19 +51,19 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)
5351
auto cmd_if_conf = controller_->command_interface_configuration();
5452
ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
5553
EXPECT_EQ(
56-
cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_);
54+
cmd_if_conf.names[CMD_TRACTION_WHEEL], wheels_names_[0] + "/" + traction_interface_name_);
5755
EXPECT_EQ(
58-
cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_);
56+
cmd_if_conf.names[CMD_STEER_WHEEL], steers_names_[0] + "/" + steering_interface_name_);
5957
EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
6058

6159
auto state_if_conf = controller_->state_interface_configuration();
6260
ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size());
6361
EXPECT_EQ(
6462
state_if_conf.names[STATE_TRACTION_WHEEL],
65-
controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_);
63+
controller_->wheels_state_names_[0] + "/" + traction_interface_name_);
6664
EXPECT_EQ(
6765
state_if_conf.names[STATE_STEER_AXIS],
68-
controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_);
66+
controller_->steers_state_names_[0] + "/" + steering_interface_name_);
6967
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
7068

7169
// check ref itfsTIME

0 commit comments

Comments
 (0)