[STEERING]: Add swerve drive

Signed-off-by: Quique Llorente <[email protected]>

ackermann_steering_controller/src/ackermann_steering_controller.cpp

@@ -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);

ackermann_steering_controller/src/ackermann_steering_controller.yaml

@@ -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]
@@ -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]

ackermann_steering_controller/test/ackermann_steering_controller_params.yaml

@@ -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

ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml

@@ -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

ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp

@@ -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)

bicycle_steering_controller/src/bicycle_steering_controller.cpp

@@ -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);

bicycle_steering_controller/src/bicycle_steering_controller.yaml

@@ -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]

bicycle_steering_controller/test/bicycle_steering_controller_params.yaml

@@ -2,13 +2,11 @@ test_bicycle_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_wheel_joint]
-    front_wheels_names: [steering_axis_joint]
+    wheels_names: [rear_wheel_joint]
+    steers_names: [steering_axis_joint]
 
     wheelbase: 3.24644
-    front_wheel_radius: 0.45
-    rear_wheel_radius: 0.45
+    traction_wheel_radius: 0.45

bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml

@@ -1,15 +1,13 @@
 test_bicycle_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_wheel_joint]
-    front_wheels_names: [pid_controller/steering_axis_joint]
-    rear_wheels_state_names: [rear_wheel_joint]
-    front_wheels_state_names: [steering_axis_joint]
+    wheels_names: [pid_controller/rear_wheel_joint]
+    steers_names: [pid_controller/steering_axis_joint]
+    wheels_state_names: [rear_wheel_joint]
+    steers_state_names: [steering_axis_joint]
 
     wheelbase: 3.24644
-    front_wheel_radius: 0.45
-    rear_wheel_radius: 0.45
+    traction_wheel_radius: 0.45

bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

@@ -32,16 +32,14 @@ TEST_F(BicycleSteeringControllerTest, 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_->bicycle_params_.wheelbase, wheelbase_);
-  ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_);
-  ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_);
+  ASSERT_EQ(controller_->bicycle_params_.traction_wheel_radius, traction_wheel_radius_);
 }
 
 TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)
@@ -53,19 +51,19 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)
   auto cmd_if_conf = controller_->command_interface_configuration();
   ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
   EXPECT_EQ(
-    cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_);
+    cmd_if_conf.names[CMD_TRACTION_WHEEL], wheels_names_[0] + "/" + traction_interface_name_);
   EXPECT_EQ(
-    cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_);
+    cmd_if_conf.names[CMD_STEER_WHEEL], steers_names_[0] + "/" + 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_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_STEER_AXIS],
-    controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_);
+    controller_->steers_state_names_[0] + "/" + steering_interface_name_);
   EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 
   // check ref itfsTIME

bicycle_steering_controller/test/test_bicycle_steering_controller.hpp

@@ -162,23 +162,23 @@ class BicycleSteeringControllerFixture : public ::testing::Test
     command_ifs.reserve(joint_command_values_.size());
 
     command_itfs_.emplace_back(hardware_interface::CommandInterface(
-      rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_WHEEL]));
+      wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_WHEEL]));
     command_ifs.emplace_back(command_itfs_.back());
 
     command_itfs_.emplace_back(hardware_interface::CommandInterface(
-      front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL]));
+      steers_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL]));
     command_ifs.emplace_back(command_itfs_.back());
 
     std::vector<hardware_interface::LoanedStateInterface> state_ifs;
     state_itfs_.reserve(joint_state_values_.size());
     state_ifs.reserve(joint_state_values_.size());
 
     state_itfs_.emplace_back(hardware_interface::StateInterface(
-      rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_WHEEL]));
+      wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_WHEEL]));
     state_ifs.emplace_back(state_itfs_.back());
 
     state_itfs_.emplace_back(hardware_interface::StateInterface(
-      front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS]));
+      steers_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS]));
     state_ifs.emplace_back(state_itfs_.back());
 
     controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
@@ -251,18 +251,17 @@ class BicycleSteeringControllerFixture : public ::testing::Test
   bool open_loop_ = false;
   unsigned int velocity_rolling_window_size_ = 10;
   bool position_feedback_ = false;
-  std::vector<std::string> rear_wheels_names_ = {"rear_wheel_joint"};
-  std::vector<std::string> front_wheels_names_ = {"steering_axis_joint"};
-  std::vector<std::string> joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]};
+  std::vector<std::string> wheels_names_ = {"rear_wheel_joint"};
+  std::vector<std::string> steers_names_ = {"steering_axis_joint"};
+  std::vector<std::string> joint_names_ = {wheels_names_[0], steers_names_[0]};
 
-  std::vector<std::string> rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"};
-  std::vector<std::string> front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"};
+  std::vector<std::string> wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"};
+  std::vector<std::string> steers_preceeding_names_ = {"pid_controller/steering_axis_joint"};
   std::vector<std::string> preceeding_joint_names_ = {
-    rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]};
+    wheels_preceeding_names_[0], steers_preceeding_names_[0]};
 
   double wheelbase_ = 3.24644;
-  double front_wheels_radius_ = 0.45;
-  double rear_wheels_radius_ = 0.45;
+  double traction_wheel_radius_ = 0.45;
 
   std::array<double, 2> joint_state_values_ = {3.3, 0.5};
   std::array<double, 2> joint_command_values_ = {1.1, 2.2};

bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp

@@ -32,18 +32,16 @@ TEST_F(BicycleSteeringControllerTest, 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_->bicycle_params_.wheelbase, wheelbase_);
-  ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_);
-  ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_);
+  ASSERT_EQ(controller_->bicycle_params_.traction_wheel_radius, traction_wheel_radius_);
 }
 
 TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)
@@ -56,20 +54,20 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)
   ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
   EXPECT_EQ(
     cmd_if_conf.names[CMD_TRACTION_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_STEER_WHEEL],
-    preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_);
+    preceeding_prefix_ + "/" + steers_names_[0] + "/" + 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_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_STEER_AXIS],
-    controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_);
+    controller_->steers_state_names_[0] + "/" + steering_interface_name_);
   EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 
   // check ref itfs

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -135,8 +135,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
   double last_linear_velocity_ = 0.0;
   double last_angular_velocity_ = 0.0;
 
-  std::vector<std::string> rear_wheels_state_names_;
-  std::vector<std::string> front_wheels_state_names_;
+  std::vector<std::string> wheels_state_names_;
+  std::vector<std::string> steers_state_names_;
 
 private:
   // callback for topic interface

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -36,6 +36,7 @@ namespace steering_odometry
 const unsigned int BICYCLE_CONFIG = 0;
 const unsigned int TRICYCLE_CONFIG = 1;
 const unsigned int ACKERMANN_CONFIG = 2;
+const unsigned int SWERVE_CONFIG = 3;
 /**
  * \brief The Odometry class handles odometry readings
  * (2D pose and velocity with related timestamp)