steering_controllers_library: Add `reduce_wheel_speed_until_steering_…

…reached` parameter (#1314)

doc/release_notes.rst

@@ -66,6 +66,7 @@ steering_controllers_library
 * Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 <https://github.com/ros-controls/ros2_controllers/pull/927>`_).
 * A fix for Ackermann steering odometry was added (`#921 <https://github.com/ros-controls/ros2_controllers/pull/921>`_).
 * Do not reset the steering wheels to ``0.0`` on timeout (`#1289 <https://github.com/ros-controls/ros2_controllers/pull/1289>`_).
+* New parameter ``reduce_wheel_speed_until_steering_reached`` was added. If set to true, then the wheel speed(s) is reduced until the steering angle has been reached. Only considered if ``open_loop = false`` (`#1314 <https://github.com/ros-controls/ros2_controllers/pull/1314>`_).
 
 tricycle_controller
 ************************

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -192,10 +192,13 @@ class SteeringOdometry
    * \param v_bx     Desired linear velocity of the robot in x_b-axis direction
    * \param omega_bz Desired angular velocity of the robot around x_z-axis
    * \param open_loop If false, the IK will be calculated using measured steering angle
+   * \param reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle
+   * has been reached
    * \return Tuple of velocity commands and steering commands
    */
   std::tuple<std::vector<double>, std::vector<double>> get_commands(
-    const double v_bx, const double omega_bz, const bool open_loop = true);
+    const double v_bx, const double omega_bz, const bool open_loop = true,
+    const bool reduce_wheel_speed_until_steering_reached = false);
 
   /**
    *  \brief Reset poses, heading, and accumulators

steering_controllers_library/src/steering_controllers_library.cpp

@@ -353,7 +353,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate(
 }
 
 controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers(
-  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
   auto current_ref = *(input_ref_.readFromRT());
 
@@ -386,8 +386,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
     const auto timeout =
       age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0);
 
-    auto [traction_commands, steering_commands] =
-      odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop);
+    auto [traction_commands, steering_commands] = odometry_.get_commands(
+      last_linear_velocity_, last_angular_velocity_, params_.open_loop,
+      params_.reduce_wheel_speed_until_steering_reached);
+
     if (params_.front_steering)
     {
       for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)

steering_controllers_library/src/steering_controllers_library.yaml

@@ -63,6 +63,13 @@ steering_controllers_library:
     read_only: false,
   }
 
+  reduce_wheel_speed_until_steering_reached: {
+    type: bool,
+    default_value: false,
+    description: "Reduce wheel speed until the steering angle has been reached.",
+    read_only: false,
+  }
+
   velocity_rolling_window_size: {
     type: int,
     default_value: 10,

steering_controllers_library/src/steering_odometry.cpp

@@ -217,7 +217,8 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome
 }
 
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
-  const double v_bx, const double omega_bz, const bool open_loop)
+  const double v_bx, const double omega_bz, const bool open_loop,
+  const bool reduce_wheel_speed_until_steering_reached)
 {
   // desired wheel speed and steering angle of the middle of traction and steering axis
   double Ws, phi, phi_IK = steer_pos_;
@@ -244,6 +245,29 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
   // wheel speed
   Ws = v_bx / wheel_radius_;
 
+  if (!open_loop && reduce_wheel_speed_until_steering_reached)
+  {
+    // Reduce wheel speed until the target angle has been reached
+    double phi_delta = abs(steer_pos_ - phi);
+    double scale;
+    const double min_phi_delta = M_PI / 6.;
+    if (phi_delta < min_phi_delta)
+    {
+      scale = 1;
+    }
+    else if (phi_delta >= 1.5608)
+    {
+      // cos(1.5608) = 0.01
+      scale = 0.01 / cos(min_phi_delta);
+    }
+    else
+    {
+      // TODO(anyone): find the best function, e.g convex power functions
+      scale = cos(phi_delta) / cos(min_phi_delta);
+    }
+    Ws *= scale;
+  }
+
   if (config_type_ == BICYCLE_CONFIG)
   {
     std::vector<double> traction_commands = {Ws};

steering_controllers_library/test/test_steering_odometry.cpp

@@ -116,13 +116,59 @@ TEST(TestSteeringOdometry, ackermann_IK_right)
   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
-  EXPECT_LT(cmd0[0], cmd0[1]);   // right (inner) < left outer)
+  EXPECT_LT(cmd0[0], cmd0[1]);   // right (inner) < left (outer)
   EXPECT_GT(cmd0[0], 0);
   auto cmd1 = std::get<1>(cmd);                     // steer
   EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1]));  // abs right (inner) > abs left (outer)
   EXPECT_LT(cmd1[0], 0);
 }
 
+TEST(TestSteeringOdometry, ackermann_IK_right_steering_limited)
+{
+  steering_odometry::SteeringOdometry odom(1);
+  odom.set_wheel_params(1., 2., 1.);
+  odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
+
+  {
+    odom.update_from_position(0., -0.785, 1.);  // already steered
+    auto cmd = odom.get_commands(1., -0.5, false, true);
+    auto vel_cmd_steered = std::get<0>(cmd);            // vel
+    EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]);  // right (inner) < left (outer)
+    EXPECT_GT(vel_cmd_steered[0], 0);
+    auto cmd1 = std::get<1>(cmd);                     // steer
+    EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1]));  // abs right (inner) > abs left (outer)
+    EXPECT_LT(cmd1[0], 0);
+  }
+
+  std::vector<double> vel_cmd_not_steered;
+  {
+    odom.update_from_position(0., -0.1, 1.);  // not fully steered
+    auto cmd = odom.get_commands(1., -0.5, false, false);
+    vel_cmd_not_steered = std::get<0>(cmd);                     // vel
+    EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]);  // right (inner) < left (outer)
+    EXPECT_GT(vel_cmd_not_steered[0], 0);
+    auto cmd1 = std::get<1>(cmd);                     // steer
+    EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1]));  // abs right (inner) > abs left (outer)
+    EXPECT_LT(cmd1[0], 0);
+  }
+
+  {
+    odom.update_from_position(0., -0.1, 1.);  // not fully steered
+    auto cmd = odom.get_commands(1., -0.5, false, true);
+    auto cmd0 = std::get<0>(cmd);  // vel
+    EXPECT_LT(cmd0[0], cmd0[1]);   // right (inner) < left (outer)
+    EXPECT_GT(cmd0[0], 0);
+    // vel should be less than vel_cmd_not_steered now
+    for (size_t i = 0; i < cmd0.size(); ++i)
+    {
+      EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]);
+    }
+    auto cmd1 = std::get<1>(cmd);                     // steer
+    EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1]));  // abs right (inner) > abs left (outer)
+    EXPECT_LT(cmd1[0], 0);
+  }
+}
+
 // ----------------- bicycle -----------------
 
 TEST(TestSteeringOdometry, bicycle_IK_linear)
@@ -164,6 +210,62 @@ TEST(TestSteeringOdometry, bicycle_IK_right)
   EXPECT_LT(cmd1[0], 0);           // left steering
 }
 
+TEST(TestSteeringOdometry, bicycle_IK_right_steering_limited)
+{
+  steering_odometry::SteeringOdometry odom(1);
+  odom.set_wheel_params(1., 2., 1.);
+  odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG);
+
+  {
+    odom.update_from_position(0., -0.785, 1.);  // already steered
+    auto cmd = odom.get_commands(1., -0.5, false, true);
+    auto vel_cmd_steered = std::get<0>(cmd);    // vel
+    EXPECT_DOUBLE_EQ(vel_cmd_steered[0], 1.0);  // equals linear
+    auto cmd1 = std::get<1>(cmd);               // steer
+    EXPECT_LT(cmd1[0], 0);
+  }
+
+  std::vector<double> vel_cmd_not_steered;
+  {
+    odom.update_from_position(0., -0.1, 1.);  // not fully steered
+    auto cmd = odom.get_commands(1., -0.5, false, false);
+    vel_cmd_not_steered = std::get<0>(cmd);         // vel
+    EXPECT_DOUBLE_EQ(vel_cmd_not_steered[0], 1.0);  // equals linear
+    auto cmd1 = std::get<1>(cmd);                   // steer
+    EXPECT_LT(cmd1[0], 0);
+  }
+
+  std::vector<double> vel_cmd_not_steered_limited;
+  {
+    odom.update_from_position(0., -0.1, 1.);  // not fully steered
+    auto cmd = odom.get_commands(1., -0.5, false, true);
+    vel_cmd_not_steered_limited = std::get<0>(cmd);  // vel
+    EXPECT_GT(vel_cmd_not_steered_limited[0], 0);
+    // vel should be less than vel_cmd_not_steered now
+    for (size_t i = 0; i < vel_cmd_not_steered_limited.size(); ++i)
+    {
+      EXPECT_LT(vel_cmd_not_steered_limited[i], vel_cmd_not_steered[i]);
+    }
+    auto cmd1 = std::get<1>(cmd);  // steer
+    EXPECT_LT(cmd1[0], 0);
+  }
+
+  {
+    // larger error -> check min of scale
+    odom.update_from_position(0., M_PI, 1.);  // not fully steered
+    auto cmd = odom.get_commands(1., -0.5, false, true);
+    auto cmd0 = std::get<0>(cmd);  // vel
+    EXPECT_GT(cmd0[0], 0);
+    // vel should be less than vel_cmd_not_steered_limited now
+    for (size_t i = 0; i < cmd0.size(); ++i)
+    {
+      EXPECT_LT(cmd0[i], vel_cmd_not_steered_limited[i]);
+    }
+    auto cmd1 = std::get<1>(cmd);  // steer
+    EXPECT_LT(cmd1[0], 0);
+  }
+}
+
 TEST(TestSteeringOdometry, bicycle_odometry)
 {
   steering_odometry::SteeringOdometry odom(1);
@@ -214,12 +316,55 @@ TEST(TestSteeringOdometry, tricycle_IK_right)
   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
-  EXPECT_LT(cmd0[0], cmd0[1]);   // right (inner) < left outer)
+  EXPECT_LT(cmd0[0], cmd0[1]);   // right (inner) < left (outer)
   EXPECT_GT(cmd0[0], 0);
   auto cmd1 = std::get<1>(cmd);  // steer
   EXPECT_LT(cmd1[0], 0);         // right steering
 }
 
+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.update_from_position(0., -0.785, 1.);  // already steered
+    auto cmd = odom.get_commands(1., -0.5, false, true);
+    auto vel_cmd_steered = std::get<0>(cmd);            // vel
+    EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]);  // right (inner) < left (outer)
+    EXPECT_GT(vel_cmd_steered[0], 0);
+    auto cmd1 = std::get<1>(cmd);  // steer
+    EXPECT_LT(cmd1[0], 0);
+  }
+
+  std::vector<double> vel_cmd_not_steered;
+  {
+    odom.update_from_position(0., -0.1, 1.);  // not fully steered
+    auto cmd = odom.get_commands(1., -0.5, false, false);
+    vel_cmd_not_steered = std::get<0>(cmd);                     // vel
+    EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]);  // right (inner) < left (outer)
+    EXPECT_GT(vel_cmd_not_steered[0], 0);
+    auto cmd1 = std::get<1>(cmd);  // steer
+    EXPECT_LT(cmd1[0], 0);
+  }
+
+  {
+    odom.update_from_position(0., -0.1, 1.);  // not fully steered
+    auto cmd = odom.get_commands(1., -0.5, false, true);
+    auto cmd0 = std::get<0>(cmd);  // vel
+    EXPECT_LT(cmd0[0], cmd0[1]);   // right (inner) < left (outer)
+    EXPECT_GT(cmd0[0], 0);
+    // vel should be less than vel_cmd_not_steered now
+    for (size_t i = 0; i < cmd0.size(); ++i)
+    {
+      EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]);
+    }
+    auto cmd1 = std::get<1>(cmd);  // steer
+    EXPECT_LT(cmd1[0], 0);
+  }
+}
+
 TEST(TestSteeringOdometry, tricycle_odometry)
 {
   steering_odometry::SteeringOdometry odom(1);