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 12, 2024
1 parent b245155 commit 3965416
Show file tree
Hide file tree
Showing 28 changed files with 332 additions and 363 deletions.
36 changes: 24 additions & 12 deletions ackermann_steering_controller/src/ackermann_steering_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,22 +30,34 @@ void AckermannSteeringController::initialize_implementation_parameter_listener()
controller_interface::CallbackReturn AckermannSteeringController::configure_odometry()
{
ackermann_params_ = ackermann_param_listener_->get_params();

if (ackermann_params_.front_wheels_radius > 0.0) {
fprintf(stderr, "DEPRECATED parameter 'front_wheels_radius'\n");
return controller_interface::CallbackReturn::ERROR;
}

if (ackermann_params_.rear_wheels_radius > 0.0) {
fprintf(stderr, "DEPRECATED parameter 'rear_wheels_radius'\n");
return controller_interface::CallbackReturn::ERROR;
}

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 wheelbase = ackermann_params_.wheelbase;

if (params_.front_steering)
{
odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track);
if (ackermann_params_.front_wheel_track > 0.0) {
fprintf(stderr, "DEPRECATED parameter 'front_wheel_track'\n");
return controller_interface::CallbackReturn::ERROR;
}
else
{
odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track);

if (ackermann_params_.rear_wheel_track > 0.0) {
fprintf(stderr, "DEPRECATED parameter 'rear_wheel_track'\n");
return controller_interface::CallbackReturn::ERROR;
}



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;

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: 22 additions & 8 deletions ackermann_steering_controller/src/ackermann_steering_controller.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,31 +1,45 @@
ackermann_steering_controller:
front_wheel_track:
traction_wheel_track:
{
type: double,
default_value: 0.0,
description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
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: "DEPRECATED: use 'traction_wheel_track'",
read_only: false,
}
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,
}

wheelbase:
{
type: double,
default_value: 0.0,
description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase",
read_only: false,
validation: {
gt<>: [0.0]
}
}

wheelbase:
traction_wheels_radius:
{
type: double,
default_value: 0.0,
description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase",
description: "Traction wheels radius.",
read_only: false,
validation: {
gt<>: [0.0]
Expand All @@ -36,7 +50,7 @@ ackermann_steering_controller:
{
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 +61,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
Loading

0 comments on commit 3965416

Please sign in to comment.