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

Jump to bottom

[wip][STEERING]: Add swerve drive #1148

Closed
wants to merge 2 commits into from
Closed

[wip][STEERING]: Add swerve drive #1148

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c83dce3
Remove front_steering from steering library
qinqon May 6, 2024
5b2ba7f
[STEERING]: Add swerve drive
qinqon May 6, 2024
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
16 changes: 3 additions & 13 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,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: 4 additions & 26 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,20 +1,9 @@
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",
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: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase",
read_only: false,
validation: {
gt<>: [0.0]
Expand All @@ -32,22 +21,11 @@ ackermann_steering_controller:
}
}

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

rear_wheels_radius:
traction_wheels_radius:
{
type: double,
default_value: 0.0,
description: "Rear wheels radius.",
description: "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
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]
wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
steers_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]
wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint]
steers_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint]
wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint]
steers_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 @@ -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_.wheels_names, testing::ElementsAreArray(wheels_names_));
ASSERT_THAT(
controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_));
ASSERT_EQ(controller_->params_.front_steering, front_steering_);
controller_->params_.steers_names, testing::ElementsAreArray(steers_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_);
wheels_names_[0] + "/" + traction_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL],
rear_wheels_names_[1] + "/" + traction_interface_name_);
wheels_names_[1] + "/" + traction_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL],
front_wheels_names_[0] + "/" + steering_interface_name_);
steers_names_[0] + "/" + steering_interface_name_);
EXPECT_EQ(
cmd_if_conf.names[CMD_STEER_LEFT_WHEEL],
front_wheels_names_[1] + "/" + steering_interface_name_);
steers_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_->wheels_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_->wheels_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_->steers_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_->steers_state_names_[1] + "/" + steering_interface_name_);
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);

// check ref itfsTIME
Expand Down
37 changes: 17 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 @@ -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_,
wheels_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_,
wheels_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_,
steers_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_,
steers_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_,
wheels_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_,
wheels_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_,
steers_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_,
steers_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,26 @@ 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> wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"};
std::vector<std::string> steers_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]};
wheels_names_[0], wheels_names_[1], steers_names_[0], steers_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 @@ -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_.wheels_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_.steers_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_ + "/" + wheels_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_ + "/" + wheels_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_ + "/" + steers_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_ + "/" + steers_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_->wheels_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_->wheels_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_->steers_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_->steers_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
Open in desktop
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
15 changes: 2 additions & 13 deletions bicycle_steering_controller/src/bicycle_steering_controller.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,22 +10,11 @@ bicycle_steering_controller:
}
}

front_wheel_radius:
traction_wheel_radius:
{
type: double,
default_value: 0.0,
description: "Front wheel radius.",
read_only: false,
validation: {
gt<>: [0.0]
}
}

rear_wheel_radius:
{
type: double,
default_value: 0.0,
description: "Rear wheel radius.",
description: "Steering wheel radius.",
read_only: false,
validation: {
gt<>: [0.0]
Expand Down
Loading
Loading