Pure Pursuit: Handle edge case and add logging message

msg/CMakeLists.txt

@@ -173,6 +173,7 @@ set(msg_files
 	PowerButtonState.msg
 	PowerMonitor.msg
 	PpsCapture.msg
+	PurePursuit.msg
 	PwmInput.msg
 	Px4ioStatus.msg
 	QshellReq.msg
@@ -183,13 +184,10 @@ set(msg_files
 	RcParameterMap.msg
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
-	RoverAckermannGuidanceStatus.msg
 	RoverAckermannSetpoint.msg
 	RoverAckermannStatus.msg
-	RoverDifferentialGuidanceStatus.msg
 	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
-	RoverMecanumGuidanceStatus.msg
 	RoverMecanumSetpoint.msg
 	RoverMecanumStatus.msg
 	Rpm.msg

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

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 (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
+				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);
 }

src/lib/pure_pursuit/PurePursuit.hpp

@@ -35,6 +35,8 @@
 
 #include <matrix/math.hpp>
 #include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/pure_pursuit.h>
 
 using namespace matrix;
 
@@ -84,34 +86,47 @@ 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:
 	/**
 	 * @brief Update the parameters of the module.
 	 */
 	void updateParams() override;
 
+	// uORB Publication
+	uORB::Publication<pure_pursuit_s> _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
 };

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
 }