purePursuit: migrate parameters to library

ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann

@@ -15,9 +15,6 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
 param set-default NAV_ACC_RAD 0.5
 param set-default RA_ACC_RAD_GAIN 2
 param set-default RA_ACC_RAD_MAX 3
-param set-default RA_LOOKAHD_GAIN 1
-param set-default RA_LOOKAHD_MAX 10
-param set-default RA_LOOKAHD_MIN 1
 param set-default RA_MAX_ACCEL 1.5
 param set-default RA_MAX_JERK 15
 param set-default RA_MAX_SPEED 3
@@ -30,6 +27,11 @@ param set-default RA_SPEED_I 0.01
 param set-default RA_SPEED_P 2
 param set-default RA_WHEEL_BASE 0.321
 
+# Pure Pursuit parameters
+param set-default PP_LOOKAHD_GAIN 1
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1
+
 # Simulated sensors
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 0

src/lib/pure_pursuit/CMakeLists.txt

@@ -36,4 +36,5 @@ px4_add_library(pure_pursuit
 	PurePursuit.hpp
 )
 
-px4_add_unit_gtest(SRC PurePursuitTest.cpp LINKLIBS pure_pursuit)
+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)

src/lib/pure_pursuit/PurePursuit.cpp

@@ -34,21 +34,43 @@
 #include "PurePursuit.hpp"
 #include <mathlib/mathlib.h>
 
+
+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");
+	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 lookahead_distance)
+				      const float vehicle_speed)
 {
 	// Check input validity
-	if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || lookahead_distance < FLT_EPSILON
-	    || !PX4_ISFINITE(lookahead_distance) || !prev_wp_ned.isAllFinite()) {
+	if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || vehicle_speed < 0.f
+	    || !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;
 	const Vector2f prev_wp_to_curr_wp = curr_wp_ned - prev_wp_ned;
 
-	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));
@@ -61,11 +83,11 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2
 	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
+	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(),
+	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;

src/lib/pure_pursuit/PurePursuit.hpp

@@ -31,7 +31,10 @@
  *
  ****************************************************************************/
 
+#pragma once
+
 #include <matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
 
 using namespace matrix;
 
@@ -50,6 +53,10 @@ using namespace matrix;
  * 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.
+ * The lookahead distance is defined as v * k.
+ * 	v: Vehicle ground speed [m/s]
+ * 	k: Tuning parameter (Implemented as PP_LOOKAHD_GAIN)
+ * The lookahead distance is further constrained between an upper and lower threshhold (Implemented as PP_LOOKAHD_MAX, PP_LOOKAHD_MIN).
  * 							C
  * 		  				       /
  * 						    __/__
@@ -68,24 +75,52 @@ using namespace matrix;
  * 				⌄
  * 			(+- 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.
+ * 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.
  */
-class PurePursuit
+class PurePursuit : public ModuleParams
 {
 public:
+	PurePursuit(ModuleParams *parent);
+	~PurePursuit() = default;
+
 	/**
 	 * @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.
+	 * 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.
+	 * 	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 lookahead_distance Radius of circle around vehicle [m].
+	 * @param vehicle_speed Vehicle speed [m/s].
+	 * @param RA_LOOKAHD_GAIN Tuning parameter [-]
+	 * @param RA_LOOKAHD_MAX Maximum lookahead distance [m]
+	 * @param RA_LOOKAHD_MIN Minimum lookahead distance [m]
 	 */
 	float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
-				 const float lookahead_distance);
+				 const float vehicle_speed);
+
+	float getLookaheadDistance() {return _lookahead_distance;};
+
+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};
 };

src/lib/pure_pursuit/PurePursuitTest.cpp

@@ -59,16 +59,30 @@
  * 				|
  * 				⌄
  * 			(+- 3.14159 rad)
+ *
+ * NOTE:
+ * 	The tuning parameters for the pure pursuit algorithm are set to the following for all tests:
+ * 	   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
+ * 	the lookahead distance.
+ *
 ******************************************************************/
 
 #include <gtest/gtest.h>
 #include <lib/pure_pursuit/PurePursuit.hpp>
 
 using namespace matrix;
 
-TEST(PurePursuitTest, InvalidLookaheadDistance)
+class PurePursuitTest : public ::testing::Test
+{
+public:
+	PurePursuit pure_pursuit{nullptr};
+};
+
+TEST_F(PurePursuitTest, InvalidSpeed)
 {
-	PurePursuit pure_pursuit;
 	//      V   C
 	//         /
 	//  	  /
@@ -77,20 +91,16 @@ TEST(PurePursuitTest, InvalidLookaheadDistance)
 	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);
+	// 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));
-	EXPECT_FALSE(PX4_ISFINITE(desired_heading3));
 }
 
-TEST(PurePursuitTest, InvalidWaypoints)
+TEST_F(PurePursuitTest, InvalidWaypoints)
 {
-	PurePursuit pure_pursuit;
 	//	V   C
 	//         /
 	//  	  /
@@ -115,9 +125,8 @@ TEST(PurePursuitTest, InvalidWaypoints)
 	EXPECT_FALSE(PX4_ISFINITE(desired_heading3));
 }
 
-TEST(PurePursuitTest, OutOfLookahead)
+TEST_F(PurePursuitTest, OutOfLookahead)
 {
-	PurePursuit pure_pursuit;
 	const float lookahead_distance{5.f};
 	//	V   C
 	//         /
@@ -133,13 +142,12 @@ TEST(PurePursuitTest, OutOfLookahead)
 	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
+	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
 }
 
-TEST(PurePursuitTest, WaypointOverlap)
+TEST_F(PurePursuitTest, WaypointOverlap)
 {
-	PurePursuit pure_pursuit;
 	const float lookahead_distance{5.f};
 	//	    C/P
 	//
@@ -157,13 +165,12 @@ TEST(PurePursuitTest, WaypointOverlap)
 	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
+	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
 }
 
-TEST(PurePursuitTest, CurrAndPrevSameNorthCoordinate)
+TEST_F(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,
@@ -190,5 +197,5 @@ TEST(PurePursuitTest, CurrAndPrevSameNorthCoordinate)
 	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
+	EXPECT_NEAR(desired_heading4, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path
 }

src/lib/pure_pursuit/module.yaml

@@ -0,0 +1,37 @@
+module_name: Pure Pursuit
+
+parameters:
+    - group: Pure Pursuit
+      definitions:
+        PP_LOOKAHD_GAIN:
+          description:
+            short: Tuning parameter for the pure pursuit controller
+            long: Lower value -> More aggressive controller (beware overshoot/oscillations)
+          type: float
+          min: 0.1
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: 1
+
+        PP_LOOKAHD_MIN:
+          description:
+            short: Minimum lookahead distance for the pure pursuit controller
+          type: float
+          unit: m
+          min: 0.1
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: 1
+
+        PP_LOOKAHD_MAX:
+          description:
+            short: Maximum lookahead distance for the pure pursuit controller
+          type: float
+          unit: m
+          min: 0.1
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: 10