From d1b0be18b2c037290ebe05d6554986f2e250ada1 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 25 Feb 2025 14:51:26 +0100 Subject: [PATCH] pure_pursuit: update library --- msg/CMakeLists.txt | 1 + msg/PurePursuitStatus.msg | 9 + src/lib/pure_pursuit/CMakeLists.txt | 3 +- src/lib/pure_pursuit/PurePursuit.cpp | 101 ++++++------ src/lib/pure_pursuit/PurePursuit.hpp | 89 ++++------ src/lib/pure_pursuit/PurePursuitTest.cpp | 156 +++++++++++------- src/modules/logger/logged_topics.cpp | 1 + .../AckermannPosVelControl.cpp | 29 +++- .../AckermannPosVelControl.hpp | 13 +- src/modules/rover_ackermann/CMakeLists.txt | 1 + src/modules/rover_differential/CMakeLists.txt | 1 + .../DifferentialPosVelControl.cpp | 28 +++- .../DifferentialPosVelControl.hpp | 4 +- src/modules/rover_mecanum/CMakeLists.txt | 1 + .../MecanumPosVelControl.cpp | 25 ++- .../MecanumPosVelControl.hpp | 4 +- 16 files changed, 277 insertions(+), 189 deletions(-) create mode 100644 msg/PurePursuitStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 8ef4407504..53207a1dde 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -163,6 +163,7 @@ set(msg_files PowerButtonState.msg PowerMonitor.msg PpsCapture.msg + PurePursuitStatus.msg PwmInput.msg Px4ioStatus.msg QshellReq.msg diff --git a/msg/PurePursuitStatus.msg b/msg/PurePursuitStatus.msg new file mode 100644 index 0000000000..ef55854ed3 --- /dev/null +++ b/msg/PurePursuitStatus.msg @@ -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 diff --git a/src/lib/pure_pursuit/CMakeLists.txt b/src/lib/pure_pursuit/CMakeLists.txt index 19154fb5a5..1547d3dcdb 100644 --- a/src/lib/pure_pursuit/CMakeLists.txt +++ b/src/lib/pure_pursuit/CMakeLists.txt @@ -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) diff --git a/src/lib/pure_pursuit/PurePursuit.cpp b/src/lib/pure_pursuit/PurePursuit.cpp index e28e7b0924..2cdf71f3ec 100644 --- a/src/lib/pure_pursuit/PurePursuit.cpp +++ b/src/lib/pure_pursuit/PurePursuit.cpp @@ -32,64 +32,73 @@ ****************************************************************************/ #include "PurePursuit.hpp" -#include - - -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 diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp index 5740d0184c..10b3ff842d 100644 --- a/src/lib/pure_pursuit/PurePursuit.hpp +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -31,13 +31,6 @@ * ****************************************************************************/ -#pragma once - -#include -#include - -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 +#include +#include +#include + +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); +} diff --git a/src/lib/pure_pursuit/PurePursuitTest.cpp b/src/lib/pure_pursuit/PurePursuitTest.cpp index c5b6897fbd..d1e1c1784b 100644 --- a/src/lib/pure_pursuit/PurePursuitTest.cpp +++ b/src/lib/pure_pursuit/PurePursuitTest.cpp @@ -72,130 +72,172 @@ #include #include +#include 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 } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 84d8e3eadd..85dd467c33 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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"); diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp index e9938b5bc8..b5f2e8b96e 100644 --- a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp @@ -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); } } diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp index a993be28a5..d731b897b1 100644 --- a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp @@ -64,6 +64,7 @@ #include #include #include +#include using namespace matrix; @@ -192,11 +193,12 @@ private: offboard_control_mode_s _offboard_control_mode{}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; // Variables hrt_abstime _timestamp{0}; @@ -232,7 +234,6 @@ private: SlewRate _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) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_ra_acc_rad_max, (ParamFloat) _param_ra_acc_rad_gain, diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 9ad139d6e5..a68bbf9e72 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -47,6 +47,7 @@ px4_add_module( AckermannPosVelControl px4_work_queue rover_control + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 8fafc30bab..6a73cd74c4 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -47,6 +47,7 @@ px4_add_module( DifferentialPosVelControl px4_work_queue rover_control + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp index 32532f4c56..98a715b7b9 100644 --- a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp @@ -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()) { diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp index 0ae8a24eae..58f57b1c07 100644 --- a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp @@ -187,6 +187,7 @@ private: uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; // Variables hrt_abstime _timestamp{0}; @@ -218,7 +219,6 @@ private: SlewRate _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) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_nav_acc_rad diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index cf5e5b9187..0dfcb5fd48 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -47,6 +47,7 @@ px4_add_module( MecanumPosVelControl px4_work_queue rover_control + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp index a5ee63496e..2ad95e8bcf 100644 --- a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp @@ -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); diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp index 4659588fdf..089bc79b1a 100644 --- a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp @@ -177,6 +177,7 @@ private: uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; // Variables hrt_abstime _timestamp{0}; @@ -210,7 +211,6 @@ private: SlewRate _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) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_nav_acc_rad