From b93dd0e8d45340e510e74b5be84a618b15a95787 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Tue, 30 Jul 2024 14:16:05 +0200 Subject: [PATCH] purePursuit: migrate parameters to library (#23438) --- .../airframes/4012_gz_rover_ackermann | 8 +-- src/lib/pure_pursuit/CMakeLists.txt | 3 +- src/lib/pure_pursuit/PurePursuit.cpp | 34 ++++++++++--- src/lib/pure_pursuit/PurePursuit.hpp | 49 +++++++++++++++--- src/lib/pure_pursuit/PurePursuitTest.cpp | 51 +++++++++++-------- src/lib/pure_pursuit/module.yaml | 37 ++++++++++++++ .../RoverAckermannGuidance.cpp | 40 +++++++-------- .../RoverAckermannGuidance.hpp | 19 +++---- src/modules/rover_ackermann/module.yaml | 37 -------------- 9 files changed, 168 insertions(+), 110 deletions(-) create mode 100644 src/lib/pure_pursuit/module.yaml diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 91c44baff3..6f81b7036a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -15,9 +15,6 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge param set-default NAV_ACC_RAD 0.5 param set-default RA_ACC_RAD_GAIN 2 param set-default RA_ACC_RAD_MAX 3 -param set-default RA_LOOKAHD_GAIN 1 -param set-default RA_LOOKAHD_MAX 10 -param set-default RA_LOOKAHD_MIN 1 param set-default RA_MAX_ACCEL 1.5 param set-default RA_MAX_JERK 15 param set-default RA_MAX_SPEED 3 @@ -30,6 +27,11 @@ param set-default RA_SPEED_I 0.01 param set-default RA_SPEED_P 2 param set-default RA_WHEEL_BASE 0.321 +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 + # Simulated sensors param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 0 diff --git a/src/lib/pure_pursuit/CMakeLists.txt b/src/lib/pure_pursuit/CMakeLists.txt index 1547d3dcdb..19154fb5a5 100644 --- a/src/lib/pure_pursuit/CMakeLists.txt +++ b/src/lib/pure_pursuit/CMakeLists.txt @@ -36,4 +36,5 @@ px4_add_library(pure_pursuit PurePursuit.hpp ) -px4_add_unit_gtest(SRC PurePursuitTest.cpp LINKLIBS pure_pursuit) +px4_add_functional_gtest(SRC PurePursuitTest.cpp LINKLIBS pure_pursuit) +set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/module.yaml) diff --git a/src/lib/pure_pursuit/PurePursuit.cpp b/src/lib/pure_pursuit/PurePursuit.cpp index 5313efb393..324a504959 100644 --- a/src/lib/pure_pursuit/PurePursuit.cpp +++ b/src/lib/pure_pursuit/PurePursuit.cpp @@ -34,21 +34,43 @@ #include "PurePursuit.hpp" #include + +PurePursuit::PurePursuit(ModuleParams *parent) : ModuleParams(parent) +{ + _param_handles.lookahead_gain = param_find("PP_LOOKAHD_GAIN"); + _param_handles.lookahead_max = param_find("PP_LOOKAHD_MAX"); + _param_handles.lookahead_min = param_find("PP_LOOKAHD_MIN"); + updateParams(); +} + +void PurePursuit::updateParams() +{ + param_get(_param_handles.lookahead_gain, &_params.lookahead_gain); + param_get(_param_handles.lookahead_max, &_params.lookahead_max); + param_get(_param_handles.lookahead_min, &_params.lookahead_min); + + ModuleParams::updateParams(); + +} + float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, - const float lookahead_distance) + const float vehicle_speed) { // Check input validity - if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || lookahead_distance < FLT_EPSILON - || !PX4_ISFINITE(lookahead_distance) || !prev_wp_ned.isAllFinite()) { + if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || vehicle_speed < -FLT_EPSILON + || !PX4_ISFINITE(vehicle_speed) || !prev_wp_ned.isAllFinite()) { return NAN; } + _lookahead_distance = math::constrain(_params.lookahead_gain * vehicle_speed, + _params.lookahead_min, _params.lookahead_max); + // Pure pursuit const Vector2f curr_pos_to_curr_wp = curr_wp_ned - curr_pos_ned; const Vector2f prev_wp_to_curr_wp = curr_wp_ned - prev_wp_ned; - if (curr_pos_to_curr_wp.norm() < lookahead_distance + if (curr_pos_to_curr_wp.norm() < _lookahead_distance || prev_wp_to_curr_wp.norm() < FLT_EPSILON) { // Target current waypoint if closer to it than lookahead or waypoints overlap return atan2f(curr_pos_to_curr_wp(1), curr_pos_to_curr_wp(0)); @@ -61,11 +83,11 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2 const Vector2f curr_pos_to_path = distance_on_line_segment - prev_wp_to_curr_pos; // Shortest vector from the current position to the path - if (curr_pos_to_path.norm() > lookahead_distance) { // Target closest point on path if there is no intersection point + if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point return atan2f(curr_pos_to_path(1), curr_pos_to_path(0)); } - const float line_extension = sqrt(powf(lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(), + const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(), 2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension * prev_wp_to_curr_wp_u; diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp index 6e62032961..66b9b8ce7c 100644 --- a/src/lib/pure_pursuit/PurePursuit.hpp +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -31,7 +31,10 @@ * ****************************************************************************/ +#pragma once + #include +#include using namespace matrix; @@ -50,6 +53,10 @@ using namespace matrix; * Pure pursuit is a path following algorithm that uses the intersection between the path and * a circle (the radius of which is referred to as lookahead distance) around the vehicle as * the target point for the vehicle. + * The lookahead distance is defined as v * k. + * v: Vehicle ground speed [m/s] + * k: Tuning parameter + * The lookahead distance is further constrained between an upper and lower threshhold. * C * / * __/__ @@ -68,24 +75,52 @@ using namespace matrix; * ⌄ * (+- 3.14159 rad) * - * Input: Current/prev waypoint and the vehicle position in NED frame as well as the lookahead distance. - * Output: Calculates the intersection points and returns the heading towards the point that is closer to the current waypoint. + * Input: Current/prev waypoint and the vehicle position in NED frame as well as the vehicle speed. + * Output: Calculates the intersection points as described above and returns the heading towards the point that is closer to the current waypoint. */ -class PurePursuit +class PurePursuit : public ModuleParams { public: + PurePursuit(ModuleParams *parent); + ~PurePursuit() = default; + /** * @brief Return heading towards the intersection point between a circle with a radius of - * lookahead_distance around the vehicle and an extended line segment from the previous to the current waypoint. + * vehicle_speed * PP_LOOKAHD_GAIN around the vehicle and an extended line segment from the previous to the current waypoint. * Exceptions: - * Will return heading towards the current waypoint if it is closer to the vehicle than the lookahead. + * Will return heading towards the current waypoint if it is closer to the vehicle than the lookahead or if the waypoints overlap. * Will return heading towards the closest point on the path if there are no intersection points (crosstrack error bigger than lookahead). * Will return NAN if input is invalid. * @param curr_wp_ned North/East coordinates of current waypoint in NED frame [m]. * @param prev_wp_ned North/East coordinates of previous waypoint in NED frame [m]. * @param curr_pos_ned North/East coordinates of current position of the vehicle in NED frame [m]. - * @param lookahead_distance Radius of circle around vehicle [m]. + * @param vehicle_speed Vehicle speed [m/s]. + * @param RA_LOOKAHD_GAIN Tuning parameter [-] + * @param RA_LOOKAHD_MAX Maximum lookahead distance [m] + * @param RA_LOOKAHD_MIN Minimum lookahead distance [m] */ float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, - const float lookahead_distance); + float vehicle_speed); + + float getLookaheadDistance() {return _lookahead_distance;}; + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + + struct { + param_t lookahead_gain; + param_t lookahead_max; + param_t lookahead_min; + } _param_handles{}; + + struct { + float lookahead_gain{1.f}; + float lookahead_max{10.f}; + float lookahead_min{1.f}; + } _params{}; +private: + float _lookahead_distance{0.f}; }; diff --git a/src/lib/pure_pursuit/PurePursuitTest.cpp b/src/lib/pure_pursuit/PurePursuitTest.cpp index 6d1ad1caae..c5b6897fbd 100644 --- a/src/lib/pure_pursuit/PurePursuitTest.cpp +++ b/src/lib/pure_pursuit/PurePursuitTest.cpp @@ -59,6 +59,15 @@ * | * ⌄ * (+- 3.14159 rad) + * + * NOTE: + * The tuning parameters for the pure pursuit algorithm are set to the following for all tests: + * PP_LOOKAHD_GAIN = 1.f + * PP_LOOKAHD_MAX = 10.f + * PP_LOOKAHD_MIN = 1.f + * This way passing the vehicle_speed in calcDesiredHeading function is equivalent to passing + * the lookahead distance. + * ******************************************************************/ #include @@ -66,9 +75,14 @@ using namespace matrix; -TEST(PurePursuitTest, InvalidLookaheadDistance) +class PurePursuitTest : public ::testing::Test +{ +public: + PurePursuit pure_pursuit{nullptr}; +}; + +TEST_F(PurePursuitTest, InvalidSpeed) { - PurePursuit pure_pursuit; // V C // / // / @@ -77,20 +91,16 @@ TEST(PurePursuitTest, InvalidLookaheadDistance) const Vector2f curr_wp_ned(10.f, 10.f); const Vector2f prev_wp_ned(0.f, 0.f); const Vector2f curr_pos_ned(10.f, 0.f); - // Zero lookahead - const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, 0.f); - // Negative lookahead - const float desired_heading2 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, -1.f); - // NaN lookahead - const float desired_heading3 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, NAN); + // Negative speed + const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, -1.f); + // NaN speed + const float desired_heading2 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, NAN); EXPECT_FALSE(PX4_ISFINITE(desired_heading1)); EXPECT_FALSE(PX4_ISFINITE(desired_heading2)); - EXPECT_FALSE(PX4_ISFINITE(desired_heading3)); } -TEST(PurePursuitTest, InvalidWaypoints) +TEST_F(PurePursuitTest, InvalidWaypoints) { - PurePursuit pure_pursuit; // V C // / // / @@ -115,9 +125,8 @@ TEST(PurePursuitTest, InvalidWaypoints) EXPECT_FALSE(PX4_ISFINITE(desired_heading3)); } -TEST(PurePursuitTest, OutOfLookahead) +TEST_F(PurePursuitTest, OutOfLookahead) { - PurePursuit pure_pursuit; const float lookahead_distance{5.f}; // V C // / @@ -133,13 +142,12 @@ TEST(PurePursuitTest, OutOfLookahead) const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, 10.f), lookahead_distance); - EXPECT_NEAR(desired_heading1, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to current waypoint - EXPECT_NEAR(desired_heading2, M_PI_F, FLT_EPSILON); // Fallback: Bearing to current waypoint + EXPECT_NEAR(desired_heading1, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path + EXPECT_NEAR(desired_heading2, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path } -TEST(PurePursuitTest, WaypointOverlap) +TEST_F(PurePursuitTest, WaypointOverlap) { - PurePursuit pure_pursuit; const float lookahead_distance{5.f}; // C/P // @@ -157,13 +165,12 @@ TEST(PurePursuitTest, WaypointOverlap) const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 0.f), Vector2f(0.f, 0.f), Vector2f(10.f, 10.f), lookahead_distance); - EXPECT_NEAR(desired_heading1, M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to current waypoint - EXPECT_NEAR(desired_heading2, -(M_PI_4_F + M_PI_2_F), FLT_EPSILON); // Fallback: Bearing to current waypoint + EXPECT_NEAR(desired_heading1, M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path + EXPECT_NEAR(desired_heading2, -(M_PI_4_F + M_PI_2_F), FLT_EPSILON); // Fallback: Bearing to closest point on path } -TEST(PurePursuitTest, CurrAndPrevSameNorthCoordinate) +TEST_F(PurePursuitTest, CurrAndPrevSameNorthCoordinate) { - PurePursuit pure_pursuit; const float lookahead_distance{5.f}; // P -- V -- C const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(0.f, @@ -190,5 +197,5 @@ TEST(PurePursuitTest, CurrAndPrevSameNorthCoordinate) EXPECT_NEAR(desired_heading1, M_PI_2_F, FLT_EPSILON); EXPECT_NEAR(desired_heading2, M_PI_2_F + M_PI_4_F, FLT_EPSILON); EXPECT_NEAR(desired_heading3, -(M_PI_2_F + M_PI_4_F), FLT_EPSILON); - EXPECT_NEAR(desired_heading4, M_PI_F, FLT_EPSILON); // Fallback: Bearing to current waypoint + EXPECT_NEAR(desired_heading4, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path } diff --git a/src/lib/pure_pursuit/module.yaml b/src/lib/pure_pursuit/module.yaml new file mode 100644 index 0000000000..5e7cc109d4 --- /dev/null +++ b/src/lib/pure_pursuit/module.yaml @@ -0,0 +1,37 @@ +module_name: Pure Pursuit + +parameters: + - group: Pure Pursuit + definitions: + PP_LOOKAHD_GAIN: + description: + short: Tuning parameter for the pure pursuit controller + long: Lower value -> More aggressive controller (beware overshoot/oscillations) + type: float + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + PP_LOOKAHD_MIN: + description: + short: Minimum lookahead distance for the pure pursuit controller + type: float + unit: m + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + PP_LOOKAHD_MAX: + description: + short: Maximum lookahead distance for the pure pursuit controller + type: float + unit: m + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 10 diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index 2862bcf181..db0495b67a 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -60,7 +60,6 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c { // Initializations float desired_speed{0.f}; - float desired_steering{0.f}; float vehicle_yaw{0.f}; float actual_speed{0.f}; bool mission_finished{false}; @@ -116,8 +115,10 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c } // Guidance logic - if (!mission_finished) { - // Calculate desired speed + if (mission_finished) { + _desired_steering = 0.f; + + } else { const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _prev_wp(0), _prev_wp(1)); @@ -125,7 +126,11 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c _curr_wp(0), _curr_wp(1)); - if (distance_to_curr_wp > _acceptance_radius) { + if (distance_to_curr_wp < _acceptance_radius) { // Catch delay command + desired_speed = 0.f; + + } else { + // Calculate desired speed if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get() && _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) { @@ -163,15 +168,10 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c } // Calculate desired steering - desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_lookahd_gain.get(), - _param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw); - desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get()); - _prev_desired_steering = desired_steering; - - } else { // Catch delay command - desired_steering = _prev_desired_steering; // Avoid steering on the spot - desired_speed = 0.f; + _desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_wheel_base.get(), + desired_speed, vehicle_yaw); + _desired_steering = math::constrain(_desired_steering, -_param_ra_max_steer_angle.get(), + _param_ra_max_steer_angle.get()); } } @@ -203,7 +203,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c // Return motor setpoints motor_setpoint motor_setpoint_temp; - motor_setpoint_temp.steering = math::interpolate(desired_steering, -_param_ra_max_steer_angle.get(), + motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), _param_ra_max_steer_angle.get(), -1.f, 1.f); motor_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); @@ -258,8 +258,8 @@ void RoverAckermannGuidance::updateWaypoints() } float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &next_wp_ned, const float &default_acceptance_radius, const float &acceptance_radius_gain, - const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle) + const Vector2f &next_wp_ned, const float default_acceptance_radius, const float acceptance_radius_gain, + const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) { // Setup variables const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned; @@ -287,14 +287,12 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned } float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, - const float &wheel_base, const float &desired_speed, const float &vehicle_yaw) + const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, const float vehicle_yaw) { // Calculate desired steering to reach lookahead point - const float lookahead_distance = math::constrain(lookahead_gain * desired_speed, - lookahead_min, lookahead_max); const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, - lookahead_distance); + desired_speed); + const float lookahead_distance = _pure_pursuit.getLookaheadDistance(); const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); // For logging _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index 0a73abceb5..592083556f 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -82,7 +82,7 @@ public: * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering. * @param nav_state Vehicle navigation state */ - motor_setpoint computeGuidance(const int nav_state); + motor_setpoint computeGuidance(int nav_state); /** * @brief Update global/NED waypoint coordinates and acceptance radius @@ -103,24 +103,20 @@ public: * @param max_steer_angle Rover maximum steer angle. */ float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &next_wp_ned, const float &default_acceptance_radius, const float &acceptance_radius_gain, - const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle); + const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain, + float acceptance_radius_max, float wheel_base, float max_steer_angle); /** * @brief Calculate and return desired steering input * @param curr_wp_ned Current waypoint in NED frame. * @param prev_wp_ned Previous waypoint in NED frame. * @param curr_pos_ned Current position of the vehicle in NED frame. - * @param lookahead_gain Tuning parameter for the lookahead distance pure pursuit controller. - * @param lookahead_min Minimum lookahead distance. - * @param lookahead_max Maximum lookahead distance. * @param wheel_base Rover wheelbase. * @param desired_speed Desired speed for the rover. * @param vehicle_yaw Current yaw of the rover. */ float calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, - const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base, - const float &desired_speed, const float &vehicle_yaw); + float wheel_base, float desired_speed, float vehicle_yaw); protected: /** @@ -144,14 +140,14 @@ private: MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates. - PurePursuit _pure_pursuit; // Pure pursuit library + PurePursuit _pure_pursuit{this}; // Pure pursuit library // Rover variables Vector2d _curr_pos{}; Vector2f _curr_pos_ned{}; PID_t _pid_throttle; hrt_abstime _timestamp{0}; - float _prev_desired_steering{0.f}; + float _desired_steering{0.f}; // Waypoint variables Vector2d _curr_wp{}; @@ -168,9 +164,6 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_ra_wheel_base, (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_lookahd_gain, - (ParamFloat) _param_ra_lookahd_max, - (ParamFloat) _param_ra_lookahd_min, (ParamFloat) _param_ra_acc_rad_max, (ParamFloat) _param_ra_acc_rad_gain, (ParamFloat) _param_ra_miss_vel_def, diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml index d8cb888316..6571d0eeeb 100644 --- a/src/modules/rover_ackermann/module.yaml +++ b/src/modules/rover_ackermann/module.yaml @@ -27,43 +27,6 @@ parameters: decimal: 2 default: 0.5236 - RA_LOOKAHD_GAIN: - description: - short: Tuning parameter for the pure pursuit controller - long: Lower value -> More aggressive controller (beware overshoot/oscillations) - type: float - min: 0.1 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RA_LOOKAHD_MAX: - description: - short: Maximum lookahead distance for the pure pursuit controller - long: | - This is the maximum crosstrack error before the controller starts - targeting the current waypoint rather then the path between the previous - and next waypoint. - type: float - unit: m - min: 0.1 - max: 100 - increment: 0.01 - decimal: 2 - default: 10 - - RA_LOOKAHD_MIN: - description: - short: Minimum lookahead distance for the pure pursuit controller - type: float - unit: m - min: 0.1 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - RA_ACC_RAD_MAX: description: short: Maximum acceptance radius for the waypoints