From 9cdb00bce79f19fb4caf838ae4d34da5cd0eda0f Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 9 Jul 2024 12:21:00 +0200 Subject: [PATCH] add pure pursuit library --- src/lib/CMakeLists.txt | 1 + src/lib/pure_pursuit/CMakeLists.txt | 39 +++++ src/lib/pure_pursuit/PurePursuit.cpp | 75 +++++++++ src/lib/pure_pursuit/PurePursuit.hpp | 91 +++++++++++ src/lib/pure_pursuit/PurePursuitTest.cpp | 194 +++++++++++++++++++++++ 5 files changed, 400 insertions(+) create mode 100644 src/lib/pure_pursuit/CMakeLists.txt create mode 100644 src/lib/pure_pursuit/PurePursuit.cpp create mode 100644 src/lib/pure_pursuit/PurePursuit.hpp create mode 100644 src/lib/pure_pursuit/PurePursuitTest.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 4e20ce106ba8..7a7b5973fd4a 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -64,6 +64,7 @@ add_subdirectory(perf EXCLUDE_FROM_ALL) add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL) add_subdirectory(pid EXCLUDE_FROM_ALL) add_subdirectory(pid_design EXCLUDE_FROM_ALL) +add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) diff --git a/src/lib/pure_pursuit/CMakeLists.txt b/src/lib/pure_pursuit/CMakeLists.txt new file mode 100644 index 000000000000..1547d3dcdb69 --- /dev/null +++ b/src/lib/pure_pursuit/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(pure_pursuit + PurePursuit.cpp + PurePursuit.hpp +) + +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 new file mode 100644 index 000000000000..5313efb393db --- /dev/null +++ b/src/lib/pure_pursuit/PurePursuit.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "PurePursuit.hpp" +#include + +float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, + const float lookahead_distance) +{ + // Check input validity + if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || lookahead_distance < FLT_EPSILON + || !PX4_ISFINITE(lookahead_distance) || !prev_wp_ned.isAllFinite()) { + return NAN; + } + + // Pure pursuit + 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(); + const Vector2f distance_on_line_segment = (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_on_line_segment - + prev_wp_to_curr_pos; // Shortest vector from the current position to the path + + 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)); + +} diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp new file mode 100644 index 000000000000..6e6203296194 --- /dev/null +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +using namespace matrix; + +/** + * @file PurePursuit.hpp + * + * Implementation of pure pursuit guidance logic. + * + * Acknowledgements and References: + * + * This implementation has been built for PX4 based on the idea from [1] (not including any code). + * + * [1] Coulter, R. C. (1992). Implementation of the Pure Pursuit Path Tracking Algorithm + * (Techreport CMU-RI-TR-92-01). + * + * Pure pursuit is a path following algorithm that uses the intersection between the path and + * a circle (the radius of which is referred to as lookahead distance) around the vehicle as + * the target point for the vehicle. + * C + * / + * __/__ + * / / \ + * / / \ + * | / V | + * \/ / + * /\ _____ / + * N (0 rad) / + * ^ P + * | + * | D + * (-1.5708 rad) <----- ⨂ -----> E (1.5708 rad) + * | + * | + * ⌄ + * (+- 3.14159 rad) + * + * Input: Current/prev waypoint and the vehicle position in NED frame as well as the lookahead distance. + * Output: Calculates the intersection points and returns the heading towards the point that is closer to the current waypoint. + */ +class PurePursuit +{ +public: + /** + * @brief Return heading towards the intersection point between a circle with a radius of + * lookahead_distance 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. + * 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 lookahead_distance Radius of circle around vehicle [m]. + */ + float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, + const float lookahead_distance); +}; diff --git a/src/lib/pure_pursuit/PurePursuitTest.cpp b/src/lib/pure_pursuit/PurePursuitTest.cpp new file mode 100644 index 000000000000..6d1ad1caae67 --- /dev/null +++ b/src/lib/pure_pursuit/PurePursuitTest.cpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/****************************************************************** + * Test code for the Pure Pursuit algorithm + * Run this test only using "make tests TESTFILTER=PurePursuit" + * + * Graphic interpretation: + * Legend: + * C: Current waypoint + * P: Previous waypoint + * V: Vehicle + * |: Line segment + * Orientation: + * C + * / + * __/__ + * / / \ + * / / \ + * | / V | + * \/ / + * /\ _____ / + * N (0 rad) / + * ^ P + * | + * | D + * (-1.5708 rad) <----- ⨂ -----> E (1.5708 rad) + * | + * | + * ⌄ + * (+- 3.14159 rad) +******************************************************************/ + +#include +#include + +using namespace matrix; + +TEST(PurePursuitTest, InvalidLookaheadDistance) +{ + PurePursuit pure_pursuit; + // V C + // / + // / + // / + // P + 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); + // Zero lookahead + const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, 0.f); + // Negative lookahead + const float desired_heading2 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, -1.f); + // NaN lookahead + const float desired_heading3 = 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)); + EXPECT_FALSE(PX4_ISFINITE(desired_heading3)); +} + +TEST(PurePursuitTest, InvalidWaypoints) +{ + PurePursuit pure_pursuit; + // V C + // / + // / + // / + // P + 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); + // Curr WP is NAN + const float desired_heading2 = pure_pursuit.calcDesiredHeading(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)); +} + +TEST(PurePursuitTest, OutOfLookahead) +{ + PurePursuit 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); + // 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 current waypoint + EXPECT_NEAR(desired_heading2, M_PI_F, FLT_EPSILON); // Fallback: Bearing to current waypoint +} + +TEST(PurePursuitTest, WaypointOverlap) +{ + PurePursuit 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); + // 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 current waypoint + EXPECT_NEAR(desired_heading2, -(M_PI_4_F + M_PI_2_F), FLT_EPSILON); // Fallback: Bearing to current waypoint +} + +TEST(PurePursuitTest, CurrAndPrevSameNorthCoordinate) +{ + PurePursuit 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); + + // 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); + // 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); + // 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); + + 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 current waypoint +}