diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 0275230fdc..74cd0402f1 100644 --- a/doc/release_notes.rst +++ b/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 `_). * A fix for Ackermann steering odometry was added (`#921 `_). * Do not reset the steering wheels to ``0.0`` on timeout (`#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 `_). tricycle_controller ************************ diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 5b67797b79..ddf9fcdec8 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/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> 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 diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 3c14013f40..3d4164080c 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/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++) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 711a780458..4e7deef698 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/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, diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 8b8bb8bfd8..b2bf100255 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/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> 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> 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 traction_commands = {Ws}; diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index e3c8db6c15..3d6d97f15f 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/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 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 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 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 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);