Integrate tricycle_controller into steering_controller_library

doc/migration.rst

@@ -41,3 +41,36 @@ omni_wheel_drive_controller
 *****************************
 * Instead of using ``tf_frame_prefix_enable:=false``, set an empty ``tf_frame_prefix:=""`` parameter instead. (`#2073 <https://github.com/ros-controls/ros2_controllers/pull/2073>`_).
 * For using node namespace as tf prefix: Set ``tf_frame_prefix:="~"``, where the ("~") character is substituted with node namespace. (`#2073 <https://github.com/ros-controls/ros2_controllers/pull/2073>`_).
+
+tricycle_steering_controller
+*****************************
+* Use tricycle_steering_controller instead of tricycle_controller for single-drive tricycle steering (`#1695 <https://github.com/ros-controls/ros2_controllers/pull/1695>`_).
+  Necessary changes can be found `here <https://github.com/ros-controls/gz_ros2_control/pull/865/changes>`_ and are summarized in the following:
+
+  * ROS parameters
+
+  .. code:: yaml
+
+    # parameters to be changed from tricycle_controller
+    traction_joint_name: traction_joint
+    steering_joint_name: steering_joint
+    wheel_radius: 0.3
+    cmd_vel_timeout: 500
+
+    # to tricycle_steering_controller
+    traction_joints_names: ["traction_joint"]
+    steering_joints_names: ["steering_joint"]
+    traction_wheels_radius: 0.3
+    reference_timeout: 0.5 # In seconds.
+    reduce_wheel_speed_until_steering_reached: true # is default false
+
+    # remove parameters
+    odom_only_twist: false
+    publish_ackermann_command: true
+
+  * ROS command topic changed from ``/tricycle_controller/cmd_vel`` to ``/tricycle_controller/reference``.
+  * Limiters: While tricycle_controller had limiters of the command interfaces does tricycle_steering_controller instead have limiters for the reference velocity. Update the URDF to enforce command limits from the ressource_manager, and add ``limits.angular.z`` and ``limits.linear.x`` if necessary.
+
+tricycle_controller
+*****************************
+See tricycle_steering_controller above.

doc/release_notes.rst

@@ -57,7 +57,16 @@ steering_controllers_library
 *****************************
 * Parameter ``tf_frame_prefix`` added with the similar functionality to other controllers. (`#2080 <https://github.com/ros-controls/ros2_controllers/pull/2080>`_).
 * Set odometry service added to be used at runtime. (`#2244 <https://github.com/ros-controls/ros2_controllers/pull/2244>`_).
+* Kinematics for single-drive tricycle added. (`#1695 <https://github.com/ros-controls/ros2_controllers/pull/1695>`_).
 
 magnetometer_broadcaster
 ************************
 New package to broadcast ``sensor_msgs/msg/MagneticField`` from state interfaces defined by the ``semantic_components::MagneticFieldSensor``.
+
+tricycle_controller
+*****************************
+* Deprecated in favor of the new tricycle_steering_controller. (`#1695 <https://github.com/ros-controls/ros2_controllers/pull/1695>`_).
+
+tricycle_steering_controller
+*****************************
+* Added drop-in replacement for tricycle_controller (single-drive tricycle). (`#1695 <https://github.com/ros-controls/ros2_controllers/pull/1695>`_).

steering_controllers_library/doc/userdoc.rst

@@ -38,7 +38,7 @@ Currently implemented kinematics
 --------------------------------------------------------------
 
 * Bicycle - with one steering and one drive joints;
-* Tricycle - with one steering and two drive joints;
+* Tricycle - with one steering and one or two drive joints;
 * Ackermann - with two steering and two drive joints.
 
 .. toctree::

steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp

@@ -98,6 +98,9 @@ class SteeringKinematics
 
   /**
    * \brief Updates the SteeringKinematics class with latest wheels position
+   *
+   * for bicycle or tricycle config with single traction
+   *
    * \param traction_wheel_vel  Traction wheel velocity [rad/s]
    * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
@@ -108,6 +111,9 @@ class SteeringKinematics
 
   /**
    * \brief Updates the SteeringKinematics class with latest wheels position
+   *
+   * for tricycle config with dual traction
+   *
    * \param right_traction_wheel_vel  Right traction wheel velocity [rad/s]
    * \param left_traction_wheel_vel  Left traction wheel velocity [rad/s]
    * \param steer_pos Steer wheel position [rad]
@@ -120,6 +126,9 @@ class SteeringKinematics
 
   /**
    * \brief Updates the SteeringKinematics class with latest wheels position
+   *
+   * for ackermann config
+   *
    * \param right_traction_wheel_vel  Right traction wheel velocity [rad/s]
    * \param left_traction_wheel_vel  Left traction wheel velocity [rad/s]
    * \param right_steer_pos Right steer wheel position [rad]
@@ -220,6 +229,20 @@ class SteeringKinematics
    * \param heading heading [rad]
    */
   void set_odometry(const double & x, const double & y, const double & heading);
+  /**
+   * \brief Set tricycle config
+   * \param nr_traction_wheels number of traction wheels
+   */
+  void set_tricycle_nr_traction_wheels(const size_t nr_traction_wheels)
+  {
+    tricycle_nr_traction_wheels_ = nr_traction_wheels;
+  }
+
+  /**
+   * \brief Get tricycle config
+   * \return number of traction wheels
+   */
+  size_t get_tricycle_nr_traction_wheels() const { return tricycle_nr_traction_wheels_; }
 
 private:
   /**
@@ -296,6 +319,7 @@ class SteeringKinematics
 
   /// 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

@@ -99,13 +99,23 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
   }
   else if (odometry_.get_odometry_type() == steering_kinematics::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_nr_traction_wheels(1);
+        set_interface_numbers(2, 2);
+        break;
+      case 2:
+        odometry_.set_tricycle_nr_traction_wheels(2);
+        set_interface_numbers(3, 3);
+        break;
+      default:
+        RCLCPP_ERROR(
+          get_node()->get_logger(),
+          "Tricycle configuration requires one (for single-drive) or two (for dual-drive) traction "
+          "joints, but %zu were provided",
+          params_.traction_joints_names.size());
+        return controller_interface::CallbackReturn::ERROR;
     }
   }
   else if (odometry_.get_odometry_type() == steering_kinematics::ACKERMANN_CONFIG)
@@ -173,13 +183,14 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
     }
     else if (odometry_.get_odometry_type() == steering_kinematics::TRICYCLE_CONFIG)
     {
-      if (params_.traction_joints_state_names.size() != 2)
+      if (params_.traction_joints_state_names.size() != odometry_.get_tricycle_nr_traction_wheels())
       {
         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_nr_traction_wheels(), params_.traction_joints_state_names.size());
         return controller_interface::CallbackReturn::ERROR;
       }
     }

steering_controllers_library/src/steering_kinematics.cpp

@@ -126,6 +126,10 @@ bool SteeringKinematics::update_from_velocity(
 {
   steer_pos_ = steer_pos;
   double linear_velocity = traction_wheel_vel * wheel_radius_;
+  if (config_type_ == TRICYCLE_CONFIG && get_tricycle_nr_traction_wheels() == 1)
+  {
+    linear_velocity *= std::cos(steer_pos);
+  }
   const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_;
 
   return update_odometry(linear_velocity, angular_velocity, dt);
@@ -239,21 +243,18 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringKinematics::get_com
   // 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_nr_traction_wheels() == 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 = SteeringKinematics::convert_twist_to_steering_angle(v_bx, omega_bz);
   }
-#endif
-  // steering angle
-  phi = SteeringKinematics::convert_twist_to_steering_angle(v_bx, omega_bz);
   if (open_loop)
   {
     phi_IK = phi;
@@ -290,18 +291,40 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringKinematics::get_com
   {
     std::vector<double> traction_commands;
     std::vector<double> steering_commands;
-    // double-traction axle
-    if (is_close_to_zero(phi_IK))
+
+    if (get_tricycle_nr_traction_wheels() == 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(wheel_track_traction_))
+      {
+        throw std::runtime_error(
+          "Tricycle configuration with double-traction axle requires non-zero wheel track");
+      }
+      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_controllers_library.cpp

@@ -21,7 +21,7 @@
 #include "test_steering_controllers_library.hpp"
 
 class SteeringControllersLibraryTest
-: public SteeringControllersLibraryFixture<TestableSteeringControllersLibrary>
+: public SteeringControllersLibraryFixture<TestableAckermannSteeringController>
 {
 };
 

steering_controllers_library/test/test_steering_controllers_library.hpp

@@ -63,7 +63,7 @@ static constexpr double WHEELS_TRACK_ = 2.12321;
 static constexpr double WHEELS_RADIUS_ = 0.45;
 
 // subclassing and friending so we can access member variables
-class TestableSteeringControllersLibrary
+class TestableAckermannSteeringController
 : public steering_controllers_library::SteeringControllersLibrary
 {
   FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces);