Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

pure_pursuit: update library

Browse Source
This commit is contained in:
chfriedrich98
2025-02-25 14:51:26 +01:00
committed by chfriedrich98
parent fec240efba
commit d1b0be18b2
16 changed files with 277 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/CMakeLists.txt
+1
View File
@@ -163,6 +163,7 @@ set(msg_files
PowerButtonState.msg
PowerMonitor.msg
PpsCapture.msg
PurePursuitStatus.msg
PwmInput.msg
Px4ioStatus.msg
QshellReq.msg
msg/PurePursuitStatus.msg
+9
View File
@@ -0,0 +1,9 @@
uint64 timestamp # time since system start (microseconds)
float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
float32 target_bearing # [rad] Target bearing calculated by the pure pursuit controller
float32 crosstrack_error # [m] Shortest distance from the vehicle to the path (Positiv: Vehicle is on the right hand side with respect to the oriented path vector, Negativ: Left of the path)
float32 distance_to_waypoint # [m] Distance from the vehicle to the current waypoint
float32 bearing_to_waypoint # [rad] Bearing towards current waypoint
# TOPICS pure_pursuit_status
src/lib/pure_pursuit/CMakeLists.txt
+1 -2
View File
@@ -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)
src/lib/pure_pursuit/PurePursuit.cpp
+55 -46
View File
@@ -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
src/lib/pure_pursuit/PurePursuit.hpp
+32 -57
View File
@@ -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);
}
src/lib/pure_pursuit/PurePursuitTest.cpp
+99 -57
View File
@@ -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
}
src/modules/logger/logged_topics.cpp
+1
View File
@@ -99,6 +99,7 @@ void LoggedTopics::add_default_topics()
add_topic("parameter_update");
add_topic("position_controller_status", 500);
add_topic("position_controller_landing_status", 100);
add_optional_topic("pure_pursuit_status", 100);
add_topic("goto_setpoint", 200);
add_topic("position_setpoint_triplet", 200);
add_optional_topic("px4io_status");
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp
+22 -7
View File
@@ -41,6 +41,7 @@ AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModulePar
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_pure_pursuit_status_pub.advertise();
updateParams();
}
@@ -187,12 +188,16 @@ void AckermannPosVelControl::manualPositionMode()
}
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) *
vector_scaling * _pos_ctl_course_direction;
float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
@@ -215,10 +220,15 @@ void AckermannPosVelControl::offboardPositionMode()
const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
_speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
@@ -264,10 +274,15 @@ void AckermannPosVelControl::autoPositionMode()
_speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp,
_acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _nav_state,
_waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp
+8 -5
View File
@@ -64,6 +64,7 @@
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/pure_pursuit_status.h>
using namespace matrix;
@@ -192,11 +193,12 @@ private:
offboard_control_mode_s _offboard_control_mode{};
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
hrt_abstime _timestamp{0};
@@ -232,7 +234,6 @@ private:
SlewRate<float> _speed_setpoint;
// Class Instances
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
@@ -245,7 +246,9 @@ private:
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
src/modules/rover_ackermann/CMakeLists.txt
+1
View File
@@ -47,6 +47,7 @@ px4_add_module(
AckermannPosVelControl
px4_work_queue
rover_control
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_differential/CMakeLists.txt
+1
View File
@@ -47,6 +47,7 @@ px4_add_module(
DifferentialPosVelControl
px4_work_queue
rover_control
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp
+21 -7
View File
@@ -41,6 +41,7 @@ DifferentialPosVelControl::DifferentialPosVelControl(ModuleParams *parent) : Mod
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_pure_pursuit_status_pub.advertise();
updateParams();
}
@@ -201,12 +202,16 @@ void DifferentialPosVelControl::manualPositionMode()
}
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) *
vector_scaling * _pos_ctl_course_direction;
float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
@@ -229,10 +234,15 @@ void DifferentialPosVelControl::offboardPositionMode()
const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
_speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
@@ -269,8 +279,12 @@ void DifferentialPosVelControl::autoPositionMode()
}
// State machine
float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(_vehicle_speed_body_x));
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw);
if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) {
src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp
+3 -1
View File
@@ -187,6 +187,7 @@ private:
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
hrt_abstime _timestamp{0};
@@ -218,7 +219,6 @@ private:
SlewRate<float> _speed_setpoint;
// Class Instances
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance.
@@ -235,7 +235,9 @@ private:
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
src/modules/rover_mecanum/CMakeLists.txt
+1
View File
@@ -47,6 +47,7 @@ px4_add_module(
MecanumPosVelControl
px4_work_queue
rover_control
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp
+19 -6
View File
@@ -41,6 +41,7 @@ MecanumPosVelControl::MecanumPosVelControl(ModuleParams *parent) : ModuleParams(
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_pure_pursuit_status_pub.advertise();
updateParams();
}
@@ -235,11 +236,15 @@ void MecanumPosVelControl::manualPositionMode()
}
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, velocity_magnitude_setpoint);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
@@ -271,8 +276,12 @@ void MecanumPosVelControl::offboardPositionMode()
const float velocity_magnitude_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance(
_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f), _param_ro_speed_limit.get());
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, velocity_magnitude_setpoint);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
@@ -316,8 +325,12 @@ void MecanumPosVelControl::autoPositionMode()
const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
_nav_state);
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
velocity_magnitude);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
Vector2f desired_velocity(0.f, 0.f);
_speed_body_x_setpoint = _mission_finished ? 0.f : velocity_magnitude * cosf(bearing_setpoint_body_frame);
src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp
+3 -1
View File
@@ -177,6 +177,7 @@ private:
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
hrt_abstime _timestamp{0};
@@ -210,7 +211,6 @@ private:
SlewRate<float> _speed_y_setpoint;
// Class Instances
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
@@ -225,7 +225,9 @@ private:
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad