Integrate tricycle_controller into steering_controller_library

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -151,6 +151,18 @@ class SteeringOdometry
    */
   unsigned int get_odometry_type() const { return static_cast<unsigned int>(config_type_); }
 
+  /**
+   * \brief Set tricycle config
+   * \param number of traction wheels
+   */
+  void set_tricycle_config(const size_t nr_traction_wheels);
+
+  /**
+   * \brief Get tricycle config
+   * \return number of traction wheels
+   */
+  size_t get_tricycle_config() const { return tricycle_nr_traction_wheels_; }
+
   /**
    * \brief heading getter
    * \return heading [rad]
@@ -293,6 +305,7 @@ class SteeringOdometry
 
   /// Configuration type used for the forward kinematics
   int config_type_ = -1;
+  size_t tricycle_nr_traction_wheels_ = 1;
 
   /// Previous wheel position/state [rad]:
   double traction_wheel_old_pos_;

steering_controllers_library/src/steering_controllers_library.cpp

@@ -174,13 +174,21 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
   }
   else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG)
   {
-    if (params_.traction_joints_names.size() != 2)
+    switch (params_.traction_joints_names.size())
     {
-      RCLCPP_ERROR(
-        get_node()->get_logger(),
-        "Tricycle configuration requires exactly two traction joints, but %zu were provided",
-        params_.traction_joints_names.size());
-      return controller_interface::CallbackReturn::ERROR;
+      case 1:
+        odometry_.set_tricycle_config(1);
+        set_interface_numbers(2, 2, nr_ref_itfs_);
+        break;
+      case 2:
+        odometry_.set_tricycle_config(2);
+        break;
+      default:
+        RCLCPP_ERROR(
+          get_node()->get_logger(),
+          "Tricycle configuration requires exactly two traction joints, but %zu were provided",
+          params_.traction_joints_names.size());
+        return controller_interface::CallbackReturn::ERROR;
     }
   }
   else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG)
@@ -248,13 +256,14 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
     }
     else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG)
     {
-      if (params_.traction_joints_state_names.size() != 2)
+      if (params_.traction_joints_state_names.size() != odometry_.get_tricycle_config())
       {
         RCLCPP_ERROR(
           get_node()->get_logger(),
-          "Tricycle configuration requires exactly two traction joints, but %zu state interface "
+          "Current tricycle configuration requires exactly %zu traction joints, but %zu state "
+          "interface "
           "names were provided",
-          params_.traction_joints_state_names.size());
+          odometry_.get_tricycle_config(), params_.traction_joints_state_names.size());
         return controller_interface::CallbackReturn::ERROR;
       }
     }

steering_controllers_library/src/steering_odometry.cpp

@@ -224,6 +224,11 @@ void SteeringOdometry::set_odometry_type(const unsigned int type)
   config_type_ = static_cast<int>(type);
 }
 
+void SteeringOdometry::set_tricycle_config(const size_t nr_traction_wheels)
+{
+  tricycle_nr_traction_wheels_ = nr_traction_wheels;
+}
+
 double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz)
 {
   // phi can be nan if both v_bx and omega_bz are zero
@@ -238,21 +243,19 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
   // desired wheel speed and steering angle of the middle of traction and steering axis
   double Ws, phi, phi_IK = steer_pos_;
 
-#if 0
-  if (v_bx == 0 && omega_bz != 0)
+  // calculate steering angle
+  if (
+    config_type_ == TRICYCLE_CONFIG && get_tricycle_config() == 1 && is_close_to_zero(v_bx) &&
+    !is_close_to_zero(omega_bz))
   {
-    // TODO(anyone) this would be only possible if traction is on the steering axis
+    // turning on the spot
     phi = omega_bz > 0 ? M_PI_2 : -M_PI_2;
-    Ws = abs(omega_bz) * wheelbase_ / wheel_radius_;
   }
   else
   {
-    // TODO(anyone) this would be valid only if traction is on the steering axis
-    Ws = v_bx / (wheel_radius_ * std::cos(phi_IK));  // using the measured steering angle
+    phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
   }
-#endif
-  // steering angle
-  phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
+  // steering angle for inverse kinematics
   if (open_loop)
   {
     phi_IK = phi;
@@ -293,18 +296,35 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
   {
     std::vector<double> traction_commands;
     std::vector<double> steering_commands;
-    // double-traction axle
-    if (is_close_to_zero(phi_IK))
+
+    if (get_tricycle_config() == 1)
     {
-      // avoid division by zero
-      traction_commands = {Ws, Ws};
+      // if traction is on the steering axis
+      if (is_close_to_zero(v_bx) && !is_close_to_zero(omega_bz))
+      {
+        Ws = abs(omega_bz) * wheel_base_ / wheel_radius_;
+      }
+      else
+      {
+        Ws /= std::cos(phi_IK);
+      }
+      traction_commands = {Ws};
     }
     else
     {
-      const double turning_radius = wheel_base_ / std::tan(phi_IK);
-      const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius;
-      const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius;
-      traction_commands = {Wr, Wl};
+      // double-traction axle
+      if (is_close_to_zero(phi_IK))
+      {
+        // avoid division by zero
+        traction_commands = {Ws, Ws};
+      }
+      else
+      {
+        const double turning_radius = wheel_base_ / std::tan(phi_IK);
+        const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius;
+        const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius;
+        traction_commands = {Wr, Wl};
+      }
     }
     // simple steering
     steering_commands = {phi};

steering_controllers_library/test/test_steering_odometry.cpp

@@ -287,20 +287,43 @@ TEST(TestSteeringOdometry, tricycle_IK_linear)
   steering_odometry::SteeringOdometry odom(1);
   odom.set_wheel_params(1., 2., 1.);
   odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG);
+  odom.set_tricycle_config(2);
+
   odom.update_open_loop(1., 0., 1.);
   auto cmd = odom.get_commands(1., 0., true);
   auto cmd0 = std::get<0>(cmd);  // vel
-  EXPECT_EQ(cmd0[0], cmd0[1]);   // linear
-  EXPECT_GT(cmd0[0], 0);
+  ASSERT_THAT(cmd0.size(), 2);
+  EXPECT_EQ(cmd0[0], cmd0[1]);  // linear
+  EXPECT_GT(cmd0[0], 0.0);
   auto cmd1 = std::get<1>(cmd);  // steer
-  EXPECT_EQ(cmd1[0], 0);         // no steering
+  ASSERT_THAT(cmd1.size(), 1);
+  EXPECT_EQ(cmd1[0], 0.0);  // no steering
+}
+
+TEST(TestSteeringOdometry, tricycle_single_IK_linear)
+{
+  steering_odometry::SteeringOdometry odom(1);
+  odom.set_wheel_params(1., 2., 1.);
+  odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG);
+  odom.set_tricycle_config(1);
+
+  odom.update_open_loop(1., 0., 1.);
+  auto cmd = odom.get_commands(1., 0., true);
+  auto cmd0 = std::get<0>(cmd);  // vel
+  ASSERT_THAT(cmd0.size(), 1);
+  EXPECT_GT(cmd0[0], 0.0);       // linear
+  auto cmd1 = std::get<1>(cmd);  // steer
+  ASSERT_THAT(cmd1.size(), 1);
+  EXPECT_EQ(cmd1[0], 0.0);  // no steering
 }
 
 TEST(TestSteeringOdometry, tricycle_IK_left)
 {
   steering_odometry::SteeringOdometry odom(1);
   odom.set_wheel_params(1., 2., 1.);
   odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG);
+  odom.set_tricycle_config(2);
+
   odom.update_from_position(0., 0.2, 1.);  // assume already turn
   auto cmd = odom.get_commands(1., 0.1, false);
   auto cmd0 = std::get<0>(cmd);  // vel
@@ -315,6 +338,8 @@ TEST(TestSteeringOdometry, tricycle_IK_right)
   steering_odometry::SteeringOdometry odom(1);
   odom.set_wheel_params(1., 2., 1.);
   odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG);
+  odom.set_tricycle_config(2);
+
   odom.update_from_position(0., -0.2, 1.);  // assume already turn
   auto cmd = odom.get_commands(1., -0.1, false);
   auto cmd0 = std::get<0>(cmd);  // vel
@@ -329,6 +354,7 @@ TEST(TestSteeringOdometry, tricycle_IK_right_steering_limited)
   steering_odometry::SteeringOdometry odom(1);
   odom.set_wheel_params(1., 2., 1.);
   odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG);
+  odom.set_tricycle_config(2);
 
   {
     odom.update_from_position(0., -0.785, 1.);  // already steered
@@ -372,6 +398,8 @@ TEST(TestSteeringOdometry, tricycle_odometry)
   steering_odometry::SteeringOdometry odom(1);
   odom.set_wheel_params(1., 1., 1.);
   odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG);
+  odom.set_tricycle_config(2);
+
   ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1));
   EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3);
   EXPECT_NEAR(odom.get_angular(), .1, 1e-3);

tricycle_controller/doc/userdoc.rst

@@ -5,6 +5,10 @@
 tricycle_controller
 =====================
 
+.. note::
+
+    This controller is deprecated and will be removed in a future release. Please use the :ref:`tricycle_steering_controller_userdoc` instead.
+
 Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle.
 
 Input for control are robot base_link twist commands which are translated to traction and steering

tricycle_controller/src/tricycle_controller.cpp

@@ -52,6 +52,9 @@ TricycleController::TricycleController() : controller_interface::ControllerInter
 
 CallbackReturn TricycleController::on_init()
 {
+  RCLCPP_WARN(
+    get_node()->get_logger(),
+    "[Deprecated]: the `TricycleController` is replaced by 'TricycleSteeringController'");
   try
   {
     // Create the parameter listener and get the parameters

tricycle_steering_controller/doc/userdoc.rst

@@ -5,9 +5,12 @@
 tricycle_steering_controller
 =============================
 
-This controller implements the kinematics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable.
+This controller implements the kinematics with two axes and three wheels.
+
+Two possible configurations are supported:
+1. Two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering.
+2. A single traction wheel, which is also steerable: The controller expects a single command joint for traction and one commanding joint for steering.
 
-The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering.
 
 For more details on controller's execution and interfaces check the :ref:`Steering Controller Library <steering_controllers_library_userdoc>`.
 

tricycle_steering_controller/src/tricycle_steering_controller.cpp

@@ -72,6 +72,8 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome
   odometry_.set_wheel_params(traction_wheels_radius, wheelbase, traction_track_width);
   odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG);
 
+  // note: might get overwritten in the base class, because we don't have access to the number of
+  // joints here
   set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);
 
   RCLCPP_INFO(get_node()->get_logger(), "tricycle odom configure successful");