Skip to content

Commit

Permalink
Remove front_steering from steering library
Browse files Browse the repository at this point in the history 
To Accommodate controllers that are not only steering at front or rear
this change remove the `front_steering` variable from
steering_controller_library, as a byproduct of that the notion of
front or rear wheel radius is also removed from dependant controllers
and the library has know "traction_joints_names" and
"steering_joints_names" instead of "front_wheels_names" and
"rear_wheels_names".

Signed-off-by: Quique Llorente <[email protected]>
  • Loading branch information
@qinqon
qinqon committed Jun 10, 2024
1 parent b245155 commit 18b4545
Show file tree
Hide file tree
Showing 28 changed files with 294 additions and 350 deletions.
16 changes: 3 additions & 13 deletions ackermann_steering_controller/src/ackermann_steering_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,21 +31,11 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom
{
ackermann_params_ = ackermann_param_listener_->get_params();

const double front_wheels_radius = ackermann_params_.front_wheels_radius;
const double rear_wheels_radius = ackermann_params_.rear_wheels_radius;
const double front_wheel_track = ackermann_params_.front_wheel_track;
const double rear_wheel_track = ackermann_params_.rear_wheel_track;
const double traction_wheels_radius = ackermann_params_.traction_wheels_radius;
const double traction_wheel_track = ackermann_params_.traction_wheel_track;
const double wheelbase = ackermann_params_.wheelbase;

if (params_.front_steering)
{
odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track);
}
else
{
odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track);
}

odometry_.set_wheel_params(traction_wheels_radius, wheelbase, traction_wheel_track);
odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);

set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);
Expand Down
30 changes: 25 additions & 5 deletions ackermann_steering_controller/src/ackermann_steering_controller.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,29 @@
ackermann_steering_controller:
traction_wheel_track:
{
type: double,
default_value: 0.0,
description: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
read_only: false,
validation: {
gt<>: [0.0]
}
}
front_wheel_track:
{
type: double,
default_value: 0.0,
description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
description: "DEPRECATED: use 'traction_wheel_track'",
read_only: false,
validation: {
gt<>: [0.0]
}
}

rear_wheel_track:
{
type: double,
default_value: 0.0,
description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
description: "DEPRECATED: use 'traction_wheel_track'",
read_only: false,
validation: {
gt<>: [0.0]
Expand All @@ -32,11 +41,22 @@ ackermann_steering_controller:
}
}

traction_wheels_radius:
{
type: double,
default_value: 0.0,
description: "Traction wheels radius.",
read_only: false,
validation: {
gt<>: [0.0]
}
}

front_wheels_radius:
{
type: double,
default_value: 0.0,
description: "Front wheels radius.",
description: "DEPRECATED: use 'traction_wheels_radius'",
read_only: false,
validation: {
gt<>: [0.0]
Expand All @@ -47,7 +67,7 @@ ackermann_steering_controller:
{
type: double,
default_value: 0.0,
description: "Rear wheels radius.",
description: "DEPRECATED: use 'traction_wheels_radius'",
read_only: false,
validation: {
gt<>: [0.0]
Expand Down
11 changes: 4 additions & 7 deletions ackermann_steering_controller/test/ackermann_steering_controller_params.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,15 +2,12 @@ test_ackermann_steering_controller:
ros__parameters:

reference_timeout: 2.0
front_steering: true
open_loop: false
velocity_rolling_window_size: 10
position_feedback: false
rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
front_wheels_names: [front_right_steering_joint, front_left_steering_joint]
traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint]
steering_joints_names: [front_right_steering_joint, front_left_steering_joint]

wheelbase: 3.24644
front_wheel_track: 2.12321
rear_wheel_track: 1.76868
front_wheels_radius: 0.45
rear_wheels_radius: 0.45
traction_wheel_track: 1.76868
traction_wheels_radius: 0.45
15 changes: 6 additions & 9 deletions ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,13 @@
test_ackermann_steering_controller:
ros__parameters:
reference_timeout: 2.0
front_steering: true
open_loop: false
velocity_rolling_window_size: 10
position_feedback: false
rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint]
front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint]
rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint]
front_wheels_state_names: [front_right_steering_joint, front_left_steering_joint]
traction_joints_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint]
steering_joints_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint]
traction_joints_state_names: [rear_right_wheel_joint, rear_left_wheel_joint]
steering_joints_state_names: [front_right_steering_joint, front_left_steering_joint]
wheelbase: 3.24644
front_wheel_track: 2.12321
rear_wheel_track: 1.76868
front_wheels_radius: 0.45
rear_wheels_radius: 0.45
traction_wheel_track: 1.76868
traction_wheels_radius: 0.45
27 changes: 12 additions & 15 deletions ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,18 +32,15 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);

ASSERT_THAT(
controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_));
controller_->params_.traction_joints_names, testing::ElementsAreArray(traction_joints_names_));
ASSERT_THAT(
controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_));
ASSERT_EQ(controller_->params_.front_steering, front_steering_);
controller_->params_.steering_joints_names, testing::ElementsAreArray(steering_joints_names_));
ASSERT_EQ(controller_->params_.open_loop, open_loop_);
ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_);
ASSERT_EQ(controller_->params_.position_feedback, position_feedback_);
ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_);
ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_);
ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_);
ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_);
ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_);
ASSERT_EQ(controller_->ackermann_params_.traction_wheels_radius, traction_wheels_radius_);
ASSERT_EQ(controller_->ackermann_params_.traction_wheel_track, traction_wheel_track_);
}

TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
Expand All @@ -56,32 +53,32 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
EXPECT_EQ(
cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL],
rear_wheels_names_[0] + "/" + traction_interface_name_);
traction_joints_names_[0] + "/" + traction_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL],
rear_wheels_names_[1] + "/" + traction_interface_name_);
traction_joints_names_[1] + "/" + traction_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL],
front_wheels_names_[0] + "/" + steering_interface_name_);
steering_joints_names_[0] + "/" + steering_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_STEER_LEFT_WHEEL],
front_wheels_names_[1] + "/" + steering_interface_name_);
steering_joints_names_[1] + "/" + steering_interface_name_);
EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

auto state_if_conf = controller_->state_interface_configuration();
ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size());
EXPECT_EQ(
state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL],
controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_);
controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_);
EXPECT_EQ(
state_if_conf.names[STATE_TRACTION_LEFT_WHEEL],
controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_);
controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_);
EXPECT_EQ(
state_if_conf.names[STATE_STEER_RIGHT_WHEEL],
controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_);
controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_);
EXPECT_EQ(
state_if_conf.names[STATE_STEER_LEFT_WHEEL],
controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_);
controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_);
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

// check ref itfsTIME
Expand Down
39 changes: 19 additions & 20 deletions ackermann_steering_controller/test/test_ackermann_steering_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,22 +165,22 @@ class AckermannSteeringControllerFixture : public ::testing::Test
command_ifs.reserve(joint_command_values_.size());

command_itfs_.emplace_back(hardware_interface::CommandInterface(
rear_wheels_names_[0], traction_interface_name_,
traction_joints_names_[0], traction_interface_name_,
&joint_command_values_[CMD_TRACTION_RIGHT_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());

command_itfs_.emplace_back(hardware_interface::CommandInterface(
rear_wheels_names_[1], steering_interface_name_,
traction_joints_names_[1], steering_interface_name_,
&joint_command_values_[CMD_TRACTION_LEFT_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());

command_itfs_.emplace_back(hardware_interface::CommandInterface(
front_wheels_names_[0], steering_interface_name_,
steering_joints_names_[0], steering_interface_name_,
&joint_command_values_[CMD_STEER_RIGHT_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());

command_itfs_.emplace_back(hardware_interface::CommandInterface(
front_wheels_names_[1], steering_interface_name_,
steering_joints_names_[1], steering_interface_name_,
&joint_command_values_[CMD_STEER_LEFT_WHEEL]));
command_ifs.emplace_back(command_itfs_.back());

Expand All @@ -189,22 +189,22 @@ class AckermannSteeringControllerFixture : public ::testing::Test
state_ifs.reserve(joint_state_values_.size());

state_itfs_.emplace_back(hardware_interface::StateInterface(
rear_wheels_names_[0], traction_interface_name_,
traction_joints_names_[0], traction_interface_name_,
&joint_state_values_[STATE_TRACTION_RIGHT_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());

state_itfs_.emplace_back(hardware_interface::StateInterface(
rear_wheels_names_[1], traction_interface_name_,
traction_joints_names_[1], traction_interface_name_,
&joint_state_values_[STATE_TRACTION_LEFT_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());

state_itfs_.emplace_back(hardware_interface::StateInterface(
front_wheels_names_[0], steering_interface_name_,
steering_joints_names_[0], steering_interface_name_,
&joint_state_values_[STATE_STEER_RIGHT_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());

state_itfs_.emplace_back(hardware_interface::StateInterface(
front_wheels_names_[1], steering_interface_name_,
steering_joints_names_[1], steering_interface_name_,
&joint_state_values_[STATE_STEER_LEFT_WHEEL]));
state_ifs.emplace_back(state_itfs_.back());

Expand Down Expand Up @@ -276,29 +276,28 @@ class AckermannSteeringControllerFixture : public ::testing::Test
protected:
// Controller-related parameters
double reference_timeout_ = 2.0;
bool front_steering_ = true;
bool open_loop_ = false;
unsigned int velocity_rolling_window_size_ = 10;
bool position_feedback_ = false;
std::vector<std::string> rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"};
std::vector<std::string> front_wheels_names_ = {
std::vector<std::string> traction_joints_names_ = {
"rear_right_wheel_joint", "rear_left_wheel_joint"};
std::vector<std::string> steering_joints_names_ = {
"front_right_steering_joint", "front_left_steering_joint"};
std::vector<std::string> joint_names_ = {
rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]};
traction_joints_names_[0], traction_joints_names_[1], steering_joints_names_[0],
steering_joints_names_[1]};

std::vector<std::string> rear_wheels_preceeding_names_ = {
std::vector<std::string> wheels_preceeding_names_ = {
"pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"};
std::vector<std::string> front_wheels_preceeding_names_ = {
std::vector<std::string> steers_preceeding_names_ = {
"pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"};
std::vector<std::string> preceeding_joint_names_ = {
rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1],
front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]};
wheels_preceeding_names_[0], wheels_preceeding_names_[1], steers_preceeding_names_[0],
steers_preceeding_names_[1]};

double wheelbase_ = 3.24644;
double front_wheel_track_ = 2.12321;
double rear_wheel_track_ = 1.76868;
double front_wheels_radius_ = 0.45;
double rear_wheels_radius_ = 0.45;
double traction_wheel_track_ = 1.76868;
double traction_wheels_radius_ = 0.45;

std::array<double, 4> joint_state_values_ = {0.5, 0.5, 0.0, 0.0};
std::array<double, 4> joint_command_values_ = {1.1, 3.3, 2.2, 4.4};
Expand Down
31 changes: 14 additions & 17 deletions ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,20 +32,17 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);

ASSERT_THAT(
controller_->params_.rear_wheels_names,
testing::ElementsAreArray(rear_wheels_preceeding_names_));
controller_->params_.traction_joints_names,
testing::ElementsAreArray(wheels_preceeding_names_));
ASSERT_THAT(
controller_->params_.front_wheels_names,
testing::ElementsAreArray(front_wheels_preceeding_names_));
ASSERT_EQ(controller_->params_.front_steering, front_steering_);
controller_->params_.steering_joints_names,
testing::ElementsAreArray(steers_preceeding_names_));
ASSERT_EQ(controller_->params_.open_loop, open_loop_);
ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_);
ASSERT_EQ(controller_->params_.position_feedback, position_feedback_);
ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_);
ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_);
ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_);
ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_);
ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_);
ASSERT_EQ(controller_->ackermann_params_.traction_wheels_radius, traction_wheels_radius_);
ASSERT_EQ(controller_->ackermann_params_.traction_wheel_track, traction_wheel_track_);
}

TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
Expand All @@ -58,32 +55,32 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
EXPECT_EQ(
cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL],
preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_);
preceeding_prefix_ + "/" + traction_joints_names_[0] + "/" + traction_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL],
preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_);
preceeding_prefix_ + "/" + traction_joints_names_[1] + "/" + traction_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL],
preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_);
preceeding_prefix_ + "/" + steering_joints_names_[0] + "/" + steering_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_STEER_LEFT_WHEEL],
preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_);
preceeding_prefix_ + "/" + steering_joints_names_[1] + "/" + steering_interface_name_);
EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

auto state_if_conf = controller_->state_interface_configuration();
ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size());
EXPECT_EQ(
state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL],
controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_);
controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_);
EXPECT_EQ(
state_if_conf.names[STATE_TRACTION_LEFT_WHEEL],
controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_);
controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_);
EXPECT_EQ(
state_if_conf.names[STATE_STEER_RIGHT_WHEEL],
controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_);
controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_);
EXPECT_EQ(
state_if_conf.names[STATE_STEER_LEFT_WHEEL],
controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_);
controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_);
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

// check ref itfs
Expand Down
13 changes: 2 additions & 11 deletions bicycle_steering_controller/src/bicycle_steering_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,18 +32,9 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet
bicycle_params_ = bicycle_param_listener_->get_params();

const double wheelbase = bicycle_params_.wheelbase;
const double front_wheel_radius = bicycle_params_.front_wheel_radius;
const double rear_wheel_radius = bicycle_params_.rear_wheel_radius;

if (params_.front_steering)
{
odometry_.set_wheel_params(rear_wheel_radius, wheelbase);
}
else
{
odometry_.set_wheel_params(front_wheel_radius, wheelbase);
}
const double traction_wheel_radius = bicycle_params_.traction_wheel_radius;

odometry_.set_wheel_params(traction_wheel_radius, wheelbase);
odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG);

set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);
Expand Down
Loading

0 comments on commit 18b4545

Please sign in to comment.