mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-26 17:36:40 +08:00
purePursuit: migrate parameters to library (#23438)
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -34,21 +34,43 @@
|
||||
#include "PurePursuit.hpp"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
|
||||
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;
|
||||
|
||||
@@ -31,7 +31,10 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
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};
|
||||
};
|
||||
|
||||
@@ -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 <gtest/gtest.h>
|
||||
@@ -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
|
||||
}
|
||||
|
||||
@@ -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
|
||||
@@ -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<float>(desired_steering, -_param_ra_max_steer_angle.get(),
|
||||
motor_setpoint_temp.steering = math::interpolate<float>(_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;
|
||||
|
||||
@@ -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<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
|
||||
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
|
||||
(ParamFloat<px4::params::RA_LOOKAHD_GAIN>) _param_ra_lookahd_gain,
|
||||
(ParamFloat<px4::params::RA_LOOKAHD_MAX>) _param_ra_lookahd_max,
|
||||
(ParamFloat<px4::params::RA_LOOKAHD_MIN>) _param_ra_lookahd_min,
|
||||
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
|
||||
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
|
||||
(ParamFloat<px4::params::RA_MISS_VEL_DEF>) _param_ra_miss_vel_def,
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user