Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Remove front_steering from steering library #1166

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Remove front_steering from steering library #1166

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 23 additions & 9 deletions ackermann_steering_controller/src/ackermann_steering_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,21 +31,35 @@ 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 wheelbase = ackermann_params_.wheelbase;
if (ackermann_params_.front_wheels_radius > 0.0)
{
fprintf(stderr, "DEPRECATED parameter 'front_wheels_radius'\n");
return controller_interface::CallbackReturn::ERROR;
}

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

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

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
34 changes: 21 additions & 13 deletions ackermann_steering_controller/src/ackermann_steering_controller.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,24 +1,27 @@
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,
validation: {
gt<>: [0.0]
}
}

wheelbase:
Expand All @@ -32,24 +35,29 @@ ackermann_steering_controller:
}
}

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

front_wheels_radius:
{
type: double,
default_value: 0.0,
description: "DEPRECATED: use 'traction_wheels_radius'",
read_only: false,
}

rear_wheels_radius:
{
type: double,
default_value: 0.0,
description: "Rear wheels radius.",
description: "DEPRECATED: use 'traction_wheels_radius'",
read_only: false,
validation: {
gt<>: [0.0]
}
}
11 changes: 4 additions & 7 deletions ackermann_steering_controller/test/ackermann_steering_controller_params.yaml
View file
Open in desktop
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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,18 +30,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 @@ -54,32 +51,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 itfs
Expand Down
39 changes: 19 additions & 20 deletions ackermann_steering_controller/test/test_ackermann_steering_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,22 +151,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 @@ -175,22 +175,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 @@ -269,29 +269,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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,20 +30,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 @@ -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],
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
Loading