diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c87e75fb695e..b40481d028e4 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -173,6 +173,7 @@ set(msg_files PowerButtonState.msg PowerMonitor.msg PpsCapture.msg + PurePursuit.msg PwmInput.msg Px4ioStatus.msg QshellReq.msg diff --git a/msg/PurePursuit.msg b/msg/PurePursuit.msg new file mode 100644 index 000000000000..2785026c1191 --- /dev/null +++ b/msg/PurePursuit.msg @@ -0,0 +1,8 @@ +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: Right of the path, Negativ: Left of the path) +float32 distance_to_waypoint # [m] Distance from the vehicle to the current waypoint + +# TOPICS pure_pursuit diff --git a/src/lib/pure_pursuit/PurePursuit.cpp b/src/lib/pure_pursuit/PurePursuit.cpp index e28e7b0924a8..b867eae46d60 100644 --- a/src/lib/pure_pursuit/PurePursuit.cpp +++ b/src/lib/pure_pursuit/PurePursuit.cpp @@ -40,6 +40,7 @@ PurePursuit::PurePursuit(ModuleParams *parent) : ModuleParams(parent) _param_handles.lookahead_gain = param_find("PP_LOOKAHD_GAIN"); _param_handles.lookahead_max = param_find("PP_LOOKAHD_MAX"); _param_handles.lookahead_min = param_find("PP_LOOKAHD_MIN"); + _pure_pursuit_pub.advertise(); updateParams(); } @@ -53,43 +54,85 @@ void PurePursuit::updateParams() } -float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, - const float vehicle_speed) +float PurePursuit::updatePurePursuit(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, const float vehicle_speed) +{ + _target_bearing = calcTargetBearing(curr_wp_ned, prev_wp_ned, curr_pos_ned, vehicle_speed); + + if (PX4_ISFINITE(_target_bearing)) { + publishPurePursuit(); + } + + return _target_bearing; +} + +float PurePursuit::calcTargetBearing(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 Vector2f curr_pos_to_curr_wp = curr_wp_ned - curr_pos_ned; + _lookahead_distance = math::constrain(_params.lookahead_gain * fabsf(vehicle_speed), + _params.lookahead_min, _params.lookahead_max); + _curr_pos_to_curr_wp = curr_wp_ned - curr_pos_ned; const Vector2f prev_wp_to_curr_wp = curr_wp_ned - prev_wp_ned; + 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_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 = _distance_along_path - + prev_wp_to_curr_pos; // Shortest vector from the current position to the path + _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(); - if (curr_pos_to_curr_wp.norm() < _lookahead_distance + if (_curr_pos_to_curr_wp.norm() < _lookahead_distance || prev_wp_to_curr_wp.norm() < FLT_EPSILON) { // Target current waypoint if closer to it than lookahead or waypoints overlap - return atan2f(curr_pos_to_curr_wp(1), curr_pos_to_curr_wp(0)); - } + return matrix::wrap_pi(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; + } else { + + if (_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 + return 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 + return matrix::wrap_pi(atan2f(_curr_pos_to_curr_wp(1), _curr_pos_to_curr_wp(0))); + + } else { // Target closest point on path + return matrix::wrap_pi(atan2f(curr_pos_to_path(1), curr_pos_to_path(0))); + } + + + } else { + 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_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; + return matrix::wrap_pi(atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0))); + } - if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point - return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0)); } - const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(), - 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)); +} +void PurePursuit::publishPurePursuit() +{ + pure_pursuit_s pure_pursuit{}; + pure_pursuit.timestamp = hrt_absolute_time(); + pure_pursuit.target_bearing = _target_bearing; + pure_pursuit.lookahead_distance = _lookahead_distance; + pure_pursuit.crosstrack_error = _crosstrack_error; + pure_pursuit.distance_to_waypoint = _curr_pos_to_curr_wp.norm(); + _pure_pursuit_pub.publish(pure_pursuit); } diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp index 5740d0184c7a..683fe8de7a90 100644 --- a/src/lib/pure_pursuit/PurePursuit.hpp +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -35,6 +35,8 @@ #include #include +#include +#include using namespace matrix; @@ -84,27 +86,37 @@ class PurePursuit : public ModuleParams PurePursuit(ModuleParams *parent); ~PurePursuit() = default; + + /** + * @brief Calculate and return the target bearing using the pure pursuit path following logic and publish pure pursuit logging message. + * @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 Bearing towards the intersection point [rad] + */ + float updatePurePursuit(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, + float vehicle_speed); + /** - * @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. + * @brief Return bearing towards the intersection point between a circle with a radius of + * abs(vehicle_speed) * PP_LOOKAHD_GAIN around the vehicle and a 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. + * Return bearing towards the current waypoint if it is closer to the vehicle than the lookahead or if the waypoints overlap. + * Return bearing towards the closest point on the path if there are no intersection points (crosstrack error bigger than lookahead). + * 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] + * @return Bearing towards the intersection point [rad]. */ - float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, - float vehicle_speed); + float calcTargetBearing(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();}; + float getDistanceAlongPath() {return _distance_along_path.norm();}; + float getCrosstrackError() {return _crosstrack_error;}; protected: /** @@ -112,6 +124,9 @@ class PurePursuit : public ModuleParams */ void updateParams() override; + // uORB Publication + uORB::Publication _pure_pursuit_pub{ORB_ID(pure_pursuit)}; + struct { param_t lookahead_gain; param_t lookahead_max; @@ -124,7 +139,14 @@ class PurePursuit : public ModuleParams 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 Publish pure pursuit message + */ + void publishPurePursuit(); + + float _target_bearing{NAN}; + float _lookahead_distance{NAN}; // Radius of the circle around the vehicle + float _crosstrack_error{NAN}; // Shortest distance from the current position to the path (Positiv: right of path, Negativ: left of path) + Vector2f _curr_pos_to_curr_wp{}; + Vector2f _distance_along_path{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp }; diff --git a/src/lib/pure_pursuit/PurePursuitTest.cpp b/src/lib/pure_pursuit/PurePursuitTest.cpp index c5b6897fbdaf..6d99b985f858 100644 --- a/src/lib/pure_pursuit/PurePursuitTest.cpp +++ b/src/lib/pure_pursuit/PurePursuitTest.cpp @@ -65,7 +65,7 @@ * PP_LOOKAHD_GAIN = 1.f * PP_LOOKAHD_MAX = 10.f * PP_LOOKAHD_MIN = 1.f - * This way passing the vehicle_speed in calcDesiredHeading function is equivalent to passing + * This way passing the vehicle_speed in calcTargetBearing function is equivalent to passing * the lookahead distance. * ******************************************************************/ @@ -91,12 +91,9 @@ TEST_F(PurePursuitTest, InvalidSpeed) 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); + const float desired_heading1 = pure_pursuit.calcTargetBearing(curr_wp_ned, prev_wp_ned, curr_pos_ned, NAN); EXPECT_FALSE(PX4_ISFINITE(desired_heading1)); - EXPECT_FALSE(PX4_ISFINITE(desired_heading2)); } TEST_F(PurePursuitTest, InvalidWaypoints) @@ -111,14 +108,14 @@ TEST_F(PurePursuitTest, InvalidWaypoints) 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, + const float desired_heading1 = pure_pursuit.calcTargetBearing(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, + const float desired_heading2 = pure_pursuit.calcTargetBearing(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), + const float desired_heading3 = pure_pursuit.calcTargetBearing(curr_wp_ned, prev_wp_ned, Vector2f(NAN, NAN), lookahead_distance); EXPECT_FALSE(PX4_ISFINITE(desired_heading1)); EXPECT_FALSE(PX4_ISFINITE(desired_heading2)); @@ -133,17 +130,30 @@ TEST_F(PurePursuitTest, OutOfLookahead) // / // / // 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 desired_heading1 = pure_pursuit.calcTargetBearing(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 desired_heading2 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 10.f), lookahead_distance); + + // // V + // // + // // P ----- C + const float desired_heading3 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + -10.f), lookahead_distance); + + // // V + // // + // // P ----- C + const float desired_heading4 = pure_pursuit.calcTargetBearing(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 30.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 + EXPECT_NEAR(desired_heading3, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to previous waypoint + EXPECT_NEAR(desired_heading4, -(M_PI_2_F + M_PI_4_F), FLT_EPSILON); // Fallback: Bearing to current waypoint } TEST_F(PurePursuitTest, WaypointOverlap) @@ -154,17 +164,15 @@ TEST_F(PurePursuitTest, WaypointOverlap) // // // 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 desired_heading1 = pure_pursuit.calcTargetBearing(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); + const float desired_heading2 = pure_pursuit.calcTargetBearing(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 } @@ -173,29 +181,25 @@ TEST_F(PurePursuitTest, CurrAndPrevSameNorthCoordinate) { 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 desired_heading1 = pure_pursuit.calcTargetBearing(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 desired_heading2 = pure_pursuit.calcTargetBearing(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 desired_heading3 = pure_pursuit.calcTargetBearing(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 desired_heading4 = pure_pursuit.calcTargetBearing(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, 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 + 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 3ff1c321e79b..ba0187fa064e 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", 100); add_topic("goto_setpoint", 200); add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index 5fea7e5174a7..463d55c090ee 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -129,7 +129,7 @@ void RoverAckermann::Run() // 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(); + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceAlongPath(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( rover_ackermann_setpoint.forward_speed_setpoint) * vector_scaling * _pos_ctl_course_direction; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index e8c30083c405..b37b92bfe35c 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -157,26 +157,29 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); // Global waypoint coordinates - _prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid - _curr_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if current waypoint is invalid - _next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + } else { + _curr_wp = _curr_pos; } + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + } else { + _prev_wp = _curr_pos; } if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + } else { + _next_wp = _home_position; } // NED waypoint coordinates @@ -281,7 +284,7 @@ float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, con const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, const float vehicle_yaw, const float max_steering, const bool armed) { - const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, + const float desired_heading = pure_pursuit.updatePurePursuit(curr_wp_ned, prev_wp_ned, curr_pos_ned, desired_speed); const float lookahead_distance = pure_pursuit.getLookaheadDistance(); const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index c5fe294cd3e2..0e572b1704eb 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -173,12 +173,12 @@ void RoverDifferential::Run() // 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(); + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceAlongPath(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( rover_differential_setpoint.forward_speed_setpoint) * vector_scaling * _pos_ctl_course_direction; // Calculate yaw setpoint - const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, + const float yaw_setpoint = _posctl_pure_pursuit.calcTargetBearing(target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index bb45e6b7b99c..fe60ef7ca311 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -62,7 +62,7 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f } // State machine - float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + float desired_yaw = _pure_pursuit.updatePurePursuit(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, math::max(forward_speed, 0.f)); const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw); @@ -193,7 +193,7 @@ void RoverDifferentialGuidance::updateWaypoints() _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); } else { - _curr_wp = Vector2d(0, 0); + _curr_wp = _curr_pos; } if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index b0729acc024d..8c4d00339862 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -209,9 +209,9 @@ void RoverMecanum::Run() // 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(); + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceAlongPath(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; - const float desired_heading = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + const float desired_heading = _posctl_pure_pursuit.calcTargetBearing(target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, desired_velocity_magnitude); const float heading_error = matrix::wrap_pi(desired_heading - _vehicle_yaw); const Vector2f desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error)); diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index b72478d746e2..501d1f2eaf66 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -72,7 +72,7 @@ void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state) // Calculate heading error - const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + const float desired_heading = _pure_pursuit.updatePurePursuit(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, desired_velocity_magnitude); const float heading_error = matrix::wrap_pi(desired_heading - yaw); @@ -163,7 +163,7 @@ void RoverMecanumGuidance::updateWaypoints() _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); } else { - _curr_wp = Vector2d(0, 0); + _curr_wp = _curr_pos; } if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)