mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 00:43:35 +08:00
pure_pursuit: update library
This commit is contained in:
chfriedrich98
committed by
chfriedrich98
chfriedrich98
parent
fec240efba
commit
d1b0be18b2
@@ -36,5 +36,4 @@ px4_add_library(pure_pursuit
|
||||
PurePursuit.hpp
|
||||
)
|
||||
|
||||
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)
|
||||
px4_add_unit_gtest(SRC PurePursuitTest.cpp LINKLIBS pure_pursuit)
|
||||
|
||||
@@ -32,64 +32,73 @@
|
||||
****************************************************************************/
|
||||
|
||||
#include "PurePursuit.hpp"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
|
||||
PurePursuit::PurePursuit(ModuleParams *parent) : ModuleParams(parent)
|
||||
using namespace matrix;
|
||||
namespace PurePursuit
|
||||
{
|
||||
_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 vehicle_speed)
|
||||
float calcTargetBearing(pure_pursuit_status_s &pure_pursuit_status, const float lookahead_gain,
|
||||
const float lookahead_max, const float lookahead_min, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
|
||||
const Vector2f &curr_pos_ned, const float vehicle_speed)
|
||||
{
|
||||
// Check input validity
|
||||
if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || vehicle_speed < -FLT_EPSILON
|
||||
|| !PX4_ISFINITE(vehicle_speed) || !prev_wp_ned.isAllFinite()) {
|
||||
if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || !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 float lookahead_distance = math::constrain(lookahead_gain * fabsf(vehicle_speed), lookahead_min, lookahead_max);
|
||||
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
|
||||
|| 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));
|
||||
}
|
||||
|
||||
const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned;
|
||||
const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero();
|
||||
_distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u;
|
||||
_curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos;
|
||||
const Vector2f position_along_path = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) *
|
||||
prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
|
||||
const Vector2f curr_pos_to_path = position_along_path -
|
||||
prev_wp_to_curr_pos; // Shortest vector from the current position to the path
|
||||
const float crosstrack_error = sign(prev_wp_to_curr_wp(1) * curr_pos_to_path(
|
||||
0) - prev_wp_to_curr_wp(0) * curr_pos_to_path(1)) * curr_pos_to_path.norm();
|
||||
const float bearing_to_curr_waypoint = matrix::wrap_pi(atan2f(curr_pos_to_curr_wp(1), curr_pos_to_curr_wp(0)));
|
||||
float target_bearing{NAN};
|
||||
|
||||
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));
|
||||
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
|
||||
target_bearing = bearing_to_curr_waypoint;
|
||||
|
||||
} else if (fabsf(crosstrack_error) >
|
||||
lookahead_distance) { // Path segment is outside of lookahead (No intersection point)
|
||||
|
||||
const Vector2f prev_wp_to_closest_point_on_path = curr_pos_to_path + prev_wp_to_curr_pos;
|
||||
const Vector2f curr_wp_to_closest_point_on_path = curr_pos_to_path - curr_pos_to_curr_wp;
|
||||
|
||||
if (prev_wp_to_closest_point_on_path * prev_wp_to_curr_wp <
|
||||
FLT_EPSILON) { // Target previous waypoint if closest point is on the the extended path segment "behind" previous waypoint
|
||||
target_bearing = matrix::wrap_pi(atan2f(-prev_wp_to_curr_pos(1), -prev_wp_to_curr_pos(0)));
|
||||
|
||||
} else if (curr_wp_to_closest_point_on_path * prev_wp_to_curr_wp >
|
||||
FLT_EPSILON) { // Target current waypoint if closest point is on the extended path segment "ahead" of current waypoint
|
||||
target_bearing = bearing_to_curr_waypoint;
|
||||
|
||||
} else { // Target closest point on path
|
||||
target_bearing = matrix::wrap_pi(atan2f(curr_pos_to_path(1), curr_pos_to_path(0)));
|
||||
}
|
||||
|
||||
|
||||
} else { // Regular pure pursuit
|
||||
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 = position_along_path + line_extension *
|
||||
prev_wp_to_curr_wp_u;
|
||||
const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos;
|
||||
target_bearing = matrix::wrap_pi(atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0)));
|
||||
}
|
||||
|
||||
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;
|
||||
const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos;
|
||||
return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0));
|
||||
|
||||
pure_pursuit_status.lookahead_distance = lookahead_distance;
|
||||
pure_pursuit_status.target_bearing = target_bearing;
|
||||
pure_pursuit_status.crosstrack_error = crosstrack_error;
|
||||
pure_pursuit_status.distance_to_waypoint = curr_pos_to_curr_wp.norm();
|
||||
pure_pursuit_status.bearing_to_waypoint = bearing_to_curr_waypoint;
|
||||
return target_bearing;
|
||||
|
||||
}
|
||||
} // Pure Pursuit
|
||||
|
||||
@@ -31,13 +31,6 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
/**
|
||||
* @file PurePursuit.hpp
|
||||
*
|
||||
@@ -76,55 +69,37 @@ using namespace matrix;
|
||||
* (+- 3.14159 rad)
|
||||
*
|
||||
* 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.
|
||||
* Output: Calculates the intersection points as described above and returns the bearing towards the point that is closer to the current waypoint.
|
||||
*/
|
||||
class PurePursuit : public ModuleParams
|
||||
|
||||
#pragma once
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/topics/pure_pursuit_status.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
namespace PurePursuit
|
||||
{
|
||||
public:
|
||||
PurePursuit(ModuleParams *parent);
|
||||
~PurePursuit() = default;
|
||||
|
||||
/**
|
||||
* @brief Return heading towards the intersection point between a circle with a radius of
|
||||
* 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 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 vehicle_speed Vehicle speed [m/s].
|
||||
* @param PP_LOOKAHD_GAIN Tuning parameter [-]
|
||||
* @param PP_LOOKAHD_MAX Maximum lookahead distance [m]
|
||||
* @param PP_LOOKAHD_MIN Minimum lookahead distance [m]
|
||||
*/
|
||||
float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
|
||||
float vehicle_speed);
|
||||
|
||||
float getLookaheadDistance() {return _lookahead_distance;};
|
||||
float getCrosstrackError() {return _curr_pos_to_path.norm();};
|
||||
float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();};
|
||||
|
||||
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}; // Radius of the circle around the vehicle
|
||||
Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
|
||||
Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path
|
||||
};
|
||||
/**
|
||||
* @brief Return bearing towards the intersection point between a circle with a radius of
|
||||
* vehicle_speed * lookahead_gain around the vehicle and an extended line segment from the previous to the current waypoint.
|
||||
* Exceptions:
|
||||
* Will return bearing towards the current waypoint if it is closer to the vehicle than the lookahead or if the waypoints overlap.
|
||||
* Will return bearing 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 pure_pursuit_status Pure pursuit struct
|
||||
* @param lookahead_gain Tuning parameter [-]
|
||||
* @param lookahead_max Maximum lookahead distance [m]
|
||||
* @param lookahead_min Minimum lookahead distance [m]
|
||||
* @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 vehicle_speed Vehicle speed [m/s].
|
||||
* @return Target bearing [rad]
|
||||
*/
|
||||
float calcTargetBearing(pure_pursuit_status_s &pure_pursuit_status, float lookahead_gain, float lookahead_max,
|
||||
float lookahead_min, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
|
||||
float vehicle_speed);
|
||||
}
|
||||
|
||||
@@ -72,130 +72,172 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <lib/pure_pursuit/PurePursuit.hpp>
|
||||
#include <uORB/topics/pure_pursuit_status.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
class PurePursuitTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
PurePursuit pure_pursuit{nullptr};
|
||||
};
|
||||
|
||||
TEST_F(PurePursuitTest, InvalidSpeed)
|
||||
TEST(PurePursuitTest, InvalidSpeed)
|
||||
{
|
||||
// V C
|
||||
// /
|
||||
// /
|
||||
// /
|
||||
// P
|
||||
pure_pursuit_status_s pure_pursuit{};
|
||||
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);
|
||||
// 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));
|
||||
const float target_bearing1 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, curr_wp_ned, prev_wp_ned,
|
||||
curr_pos_ned, NAN);
|
||||
EXPECT_FALSE(PX4_ISFINITE(target_bearing1));
|
||||
}
|
||||
|
||||
TEST_F(PurePursuitTest, InvalidWaypoints)
|
||||
TEST(PurePursuitTest, InvalidWaypoints)
|
||||
{
|
||||
// V C
|
||||
// /
|
||||
// /
|
||||
// /
|
||||
// P
|
||||
pure_pursuit_status_s pure_pursuit{};
|
||||
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);
|
||||
const float lookahead_distance{5.f};
|
||||
// Prev WP is NAN
|
||||
const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, Vector2f(NAN, NAN), curr_pos_ned,
|
||||
lookahead_distance);
|
||||
const float target_bearing1 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, curr_wp_ned, Vector2f(NAN,
|
||||
NAN), curr_pos_ned,
|
||||
lookahead_distance);
|
||||
// Curr WP is NAN
|
||||
const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(NAN, NAN), prev_wp_ned, curr_pos_ned,
|
||||
lookahead_distance);
|
||||
const float target_bearing2 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(NAN, NAN),
|
||||
prev_wp_ned, curr_pos_ned,
|
||||
lookahead_distance);
|
||||
|
||||
// Curr Pos is NAN
|
||||
const float desired_heading3 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, Vector2f(NAN, NAN),
|
||||
lookahead_distance);
|
||||
EXPECT_FALSE(PX4_ISFINITE(desired_heading1));
|
||||
EXPECT_FALSE(PX4_ISFINITE(desired_heading2));
|
||||
EXPECT_FALSE(PX4_ISFINITE(desired_heading3));
|
||||
const float target_bearing3 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, curr_wp_ned, prev_wp_ned,
|
||||
Vector2f(NAN, NAN), lookahead_distance);
|
||||
EXPECT_FALSE(PX4_ISFINITE(target_bearing1));
|
||||
EXPECT_FALSE(PX4_ISFINITE(target_bearing2));
|
||||
EXPECT_FALSE(PX4_ISFINITE(target_bearing3));
|
||||
}
|
||||
|
||||
TEST_F(PurePursuitTest, OutOfLookahead)
|
||||
TEST(PurePursuitTest, OutOfLookahead)
|
||||
{
|
||||
pure_pursuit_status_s pure_pursuit{};
|
||||
const float lookahead_distance{5.f};
|
||||
// V C
|
||||
// /
|
||||
// /
|
||||
// /
|
||||
// P
|
||||
const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(10.f, 10.f), Vector2f(0.f, 0.f), Vector2f(10.f,
|
||||
0.f),
|
||||
lookahead_distance);
|
||||
const float target_bearing1 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(10.f, 10.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(10.f, 0.f), lookahead_distance);
|
||||
// V
|
||||
//
|
||||
// P ----- C
|
||||
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 closest point on path
|
||||
EXPECT_NEAR(desired_heading2, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
|
||||
const float target_bearing2 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(10.f, 10.f), lookahead_distance);
|
||||
// V
|
||||
//
|
||||
// P ------ C
|
||||
const float target_bearing3 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 10.f), Vector2f(10.f, 0.f), lookahead_distance);
|
||||
// V
|
||||
//
|
||||
// P ------ C
|
||||
const float target_bearing4 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 10.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(10.f, 20.f), lookahead_distance);
|
||||
|
||||
EXPECT_NEAR(target_bearing1, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
|
||||
EXPECT_NEAR(target_bearing2, -M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
|
||||
EXPECT_NEAR(target_bearing3, M_PI_F - atan2f(10, 10), FLT_EPSILON); // Fallback: Bearing to previous waypoint
|
||||
EXPECT_NEAR(target_bearing4, -M_PI_F + atan2f(10, 10), FLT_EPSILON); // Fallback: Bearing to current waypoint
|
||||
}
|
||||
|
||||
TEST_F(PurePursuitTest, WaypointOverlap)
|
||||
TEST(PurePursuitTest, WaypointOverlap)
|
||||
{
|
||||
pure_pursuit_status_s pure_pursuit{};
|
||||
const float lookahead_distance{5.f};
|
||||
// C/P
|
||||
//
|
||||
//
|
||||
//
|
||||
// V
|
||||
const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(10.f, 10.f), Vector2f(10.f, 10.f), Vector2f(0.f,
|
||||
0.f),
|
||||
lookahead_distance);
|
||||
const float target_bearing1 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(10.f, 10.f),
|
||||
Vector2f(10.f, 10.f), Vector2f(0.f, 0.f), lookahead_distance);
|
||||
// V
|
||||
//
|
||||
//
|
||||
//
|
||||
// C/P
|
||||
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 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
|
||||
const float target_bearing2 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 0.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(10.f, 10.f), lookahead_distance);
|
||||
EXPECT_NEAR(target_bearing1, M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
|
||||
EXPECT_NEAR(target_bearing2, -(M_PI_4_F + M_PI_2_F), FLT_EPSILON); // Fallback: Bearing to closest point on path
|
||||
}
|
||||
|
||||
TEST_F(PurePursuitTest, CurrAndPrevSameNorthCoordinate)
|
||||
TEST(PurePursuitTest, CurrAndPrevSameNorthCoordinate)
|
||||
{
|
||||
pure_pursuit_status_s 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,
|
||||
10.f),
|
||||
lookahead_distance);
|
||||
const float target_bearing1 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(0.f, 10.f), lookahead_distance);
|
||||
|
||||
// V
|
||||
// P ------ C
|
||||
const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f),
|
||||
Vector2f(5.f / sqrtf(2.f), 10.f),
|
||||
lookahead_distance);
|
||||
const float target_bearing2 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(5.f / sqrtf(2.f), 10.f), lookahead_distance);
|
||||
// V
|
||||
// C ------ P
|
||||
const float desired_heading3 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 0.f), Vector2f(0.f, 20.f),
|
||||
Vector2f(5.f / sqrtf(2.f), 10.f),
|
||||
lookahead_distance);
|
||||
const float target_bearing3 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 0.f),
|
||||
Vector2f(0.f, 20.f), Vector2f(5.f / sqrtf(2.f), 10.f), lookahead_distance);
|
||||
// V
|
||||
//
|
||||
// P ------ C
|
||||
const float desired_heading4 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f,
|
||||
10.f),
|
||||
lookahead_distance);
|
||||
const float target_bearing4 = PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(10.f, 10.f), lookahead_distance);
|
||||
|
||||
EXPECT_NEAR(target_bearing1, M_PI_2_F, FLT_EPSILON);
|
||||
EXPECT_NEAR(target_bearing2, M_PI_2_F + M_PI_4_F, FLT_EPSILON);
|
||||
EXPECT_NEAR(target_bearing3, -(M_PI_2_F + M_PI_4_F), FLT_EPSILON);
|
||||
EXPECT_NEAR(target_bearing4, -M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
|
||||
}
|
||||
|
||||
TEST(PurePursuitTest, CrosstrackError)
|
||||
{
|
||||
pure_pursuit_status_s pure_pursuit{};
|
||||
const float lookahead_distance{5.f};
|
||||
// V
|
||||
//
|
||||
// P ----- C
|
||||
PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(10.f, 10.f), lookahead_distance);
|
||||
const float crosstrack1 = pure_pursuit.crosstrack_error;
|
||||
|
||||
// V
|
||||
// P ----- C
|
||||
PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(5.f, 10.f), lookahead_distance);
|
||||
const float crosstrack2 = pure_pursuit.crosstrack_error;
|
||||
|
||||
// P ----- C
|
||||
//
|
||||
// V
|
||||
PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(-10.f, 10.f), lookahead_distance);
|
||||
const float crosstrack3 = pure_pursuit.crosstrack_error;
|
||||
|
||||
// P ----- C
|
||||
// V
|
||||
PurePursuit::calcTargetBearing(pure_pursuit, 1.f, 10.f, 1.f, Vector2f(0.f, 20.f),
|
||||
Vector2f(0.f, 0.f), Vector2f(-5.f, 10.f), lookahead_distance);
|
||||
const float crosstrack4 = pure_pursuit.crosstrack_error;
|
||||
|
||||
EXPECT_NEAR(crosstrack1, -10.f, FLT_EPSILON);
|
||||
EXPECT_NEAR(crosstrack2, -5.f, FLT_EPSILON);
|
||||
EXPECT_NEAR(crosstrack3, 10.f, FLT_EPSILON);
|
||||
EXPECT_NEAR(crosstrack4, 5.f, FLT_EPSILON);
|
||||
|
||||
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 closest point on path
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user