From f8bebd9e41e913f4045de8be060f52a4dab5df27 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Thu, 18 Jul 2024 13:34:56 +0200 Subject: [PATCH] ackermann: implement pure pursuit lib --- msg/RoverAckermannGuidanceStatus.msg | 1 - src/modules/rover_ackermann/CMakeLists.txt | 1 + .../RoverAckermannGuidance.cpp | 90 +++---------------- .../RoverAckermannGuidance.hpp | 13 +-- 4 files changed, 14 insertions(+), 91 deletions(-) diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg index 69dec26942..a06d1290bf 100644 --- a/msg/RoverAckermannGuidanceStatus.msg +++ b/msg/RoverAckermannGuidanceStatus.msg @@ -4,6 +4,5 @@ float32 desired_speed # [m/s] Rover desired ground speed float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller float32 heading_error # [deg] Heading error of the pure pursuit controller float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions -float32 crosstrack_error # [m] Shortest distance from the vehicle to the path # TOPICS rover_ackermann_guidance_status diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 805e20b73f..f02a91c0b6 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -43,6 +43,7 @@ px4_add_module( RoverAckermannGuidance px4_work_queue SlewRate + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index 19cdf5cf1a..bd77e31eea 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -209,21 +209,24 @@ void RoverAckermannGuidance::updateWaypoints() _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); // Global waypoint coordinates - if (position_setpoint_triplet.current.valid) { + 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 = Vector2d(0, 0); } - if (position_setpoint_triplet.previous.valid) { + 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) { + 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 { @@ -280,45 +283,16 @@ float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &curr_pos_local, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base, const float &desired_speed, const float &vehicle_yaw) { - // Calculate crosstrack error - const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local; - - if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen) - return 0.f; - } - - const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local; - const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) / - prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized(); - const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local; - - // Calculate desired heading towards lookahead point - float desired_heading{0.f}; - float lookahead_distance = math::constrain(lookahead_gain * desired_speed, - lookahead_min, lookahead_max); - - if (crosstrack_error.longerThan(lookahead_distance)) { - if (crosstrack_error.norm() < lookahead_max) { - lookahead_distance = crosstrack_error.norm(); // Scale lookahead radius - desired_heading = calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, lookahead_distance); - - } else { // Excessively large crosstrack error - desired_heading = calcDesiredHeading(curr_wp_local, curr_pos_local, curr_pos_local, lookahead_distance); - } - - } else { // Crosstrack error smaller than lookahead - desired_heading = calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, lookahead_distance); - } - // Calculate desired steering to reach lookahead point + const float lookahead_distance = math::constrain(lookahead_gain * desired_speed, + lookahead_min, lookahead_max); + const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, + lookahead_distance); const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); - // For logging _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); - _rover_ackermann_guidance_status.crosstrack_error = crosstrack_error.norm(); - // Calculate desired steering if (math::abs_t(heading_error) <= M_PI_2_F) { return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); @@ -330,47 +304,5 @@ float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local, return atanf(2 * wheel_base * (-1.0f + sinf(heading_error + M_PI_2_F)) / lookahead_distance); } -} - -float RoverAckermannGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, - const Vector2f &curr_pos_local, - const float &lookahead_distance) -{ - // Setup variables - const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0)); - const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local( - 0) - prev_wp_local(0)); - const float a = -line_segment_slope; - const float c = -line_segment_rover_offset; - const float r = lookahead_distance; - const float x0 = -a * c / (a * a + 1.0f); - const float y0 = -c / (a * a + 1.0f); - - // Calculate intersection points - if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist - return 0.f; - - } else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists - return atan2f(y0, x0); - - } else { // Two intersetion points exist - const float d = r * r - c * c / (a * a + 1.0f); - const float mult = sqrt(d / (a * a + 1.0f)); - const float ax = x0 + mult; - const float bx = x0 - mult; - const float ay = y0 - a * mult; - const float by = y0 + a * mult; - const Vector2f point1(ax, ay); - const Vector2f point2(bx, by); - const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1; - const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2; - - // Return intersection point closer to current waypoint - if (distance1.norm_squared() < distance2.norm_squared()) { - return atan2f(ay, ax); - - } else { - return atan2f(by, bx); - } - } + } diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index a618f830e5..2674f67ca4 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -35,6 +35,7 @@ // PX4 includes #include +#include // uORB includes #include @@ -121,17 +122,6 @@ public: const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base, const float &desired_speed, const float &vehicle_yaw); - /** - * @brief Return desired heading to the intersection point between a circle with a radius of - * lookahead_distance around the vehicle and a line segment from the previous to the current waypoint. - * @param curr_wp_local Current waypoint in local frame. - * @param prev_wp_local Previous waypoint in local frame. - * @param curr_pos_local Current position of the vehicle in local frame. - * @param lookahead_distance Radius of circle around vehicle. - */ - float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local, - const float &lookahead_distance); - protected: /** * @brief Update the parameters of the module. @@ -154,6 +144,7 @@ private: MapProjection _global_local_proj_ref{}; // Transform global to local coordinates. + PurePursuit _pure_pursuit; // Pure pursuit library // Rover variables Vector2d _curr_pos{};