motion_planning: sanitize inputs to position and velocity smoothing libs

src/lib/motion_planning/PositionSmoothing.hpp

@@ -328,7 +328,7 @@ public:
 	 */
 	inline void setMaxAllowedHorizontalError(float error)
 	{
-		_max_allowed_horizontal_error = error;
+		_max_allowed_horizontal_error = (PX4_ISFINITE(error)) ? math::max(error, kMinHorizontalError) : kDefaultHorizontalError;
 	}
 
 	/**
@@ -410,8 +410,14 @@ public:
 
 
 private:
+	// [m] minimum max horizontal vehicle position error from trajectory allowing time integration along trajectory
+	static constexpr float kMinHorizontalError{0.1f};
+
+	// [m] default max horizontal vehicle position error from trajectory allowing time integration along trajectory
+	static constexpr float kDefaultHorizontalError{2.f};
+
 	/* params, only modified from external */
-	float _max_allowed_horizontal_error{0.f};
+	float _max_allowed_horizontal_error{kDefaultHorizontalError};
 	float _vertical_acceptance_radius{0.f};
 	float _cruise_speed{0.f};
 	float _horizontal_trajectory_gain{0.f};

src/lib/motion_planning/VelocitySmoothing.cpp

@@ -57,6 +57,10 @@ void VelocitySmoothing::reset(float accel, float vel, float pos)
 
 float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max) const
 {
+	if (math::abs_t(j_max) < FLT_EPSILON) {
+		return 0.f;
+	}
+
 	/* Check maximum acceleration, saturate and recompute T1 if needed */
 	float accel_T1 = a0 + j_max * T1;
 	float T1_new = T1;
@@ -73,6 +77,10 @@ float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, flo
 
 float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) const
 {
+	if (math::abs_t(j_max) < FLT_EPSILON) {
+		return 0.f;
+	}
+
 	float delta = 2.f * a0 * a0 + 4.f * j_max * v3;
 
 	if (delta < 0.f) {
@@ -81,6 +89,7 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
 	}
 
 	float sqrt_delta = sqrtf(delta);
+
 	float T1_plus = (-a0 + 0.5f * sqrt_delta) / j_max;
 	float T1_minus = (-a0 - 0.5f * sqrt_delta) / j_max;
 
@@ -103,6 +112,10 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
 
 float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max) const
 {
+	if (math::abs_t(j_max) < FLT_EPSILON) {
+		return 0.f;
+	}
+
 	float a = -j_max;
 	float b = j_max * T123 - a0;
 	float delta = T123 * T123 * j_max * j_max + 2.f * T123 * a0 * j_max - a0 * a0 - 4.f * j_max * v3;
@@ -156,6 +169,10 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3) const
 
 float VelocitySmoothing::computeT3(float T1, float a0, float j_max) const
 {
+	if (math::abs_t(j_max) < FLT_EPSILON) {
+		return 0.f;
+	}
+
 	float T3 = a0 / j_max + T1;
 	return math::max(T3, 0.f);
 }

src/lib/motion_planning/VelocitySmoothing.hpp

@@ -33,6 +33,9 @@
 
 #pragma once
 
+#include <px4_defines.h>
+#include <mathlib/math/Limits.hpp>
+
 struct Trajectory {
 	float j; //< jerk
 	float a; //< acceleration
@@ -91,20 +94,46 @@ public:
 	 * Getters and setters
 	 */
 	float getMaxJerk() const { return _max_jerk; }
-	void setMaxJerk(float max_jerk) { _max_jerk = max_jerk; }
+	void setMaxJerk(float max_jerk) { _max_jerk = PX4_ISFINITE(max_jerk) ? math::max(max_jerk, kMinJerk) : kMinJerk; }
 
 	float getMaxAccel() const { return _max_accel; }
-	void setMaxAccel(float max_accel) { _max_accel = max_accel; }
+	void setMaxAccel(float max_accel) { _max_accel = PX4_ISFINITE(max_accel) ? math::max(max_accel, kMinAccel) : kMinAccel; }
 
 	float getMaxVel() const { return _max_vel; }
-	void setMaxVel(float max_vel) { _max_vel = max_vel; }
+	void setMaxVel(float max_vel) { _max_vel = PX4_ISFINITE(max_vel) ? math::max(max_vel, 0.f) : 0.f; }
 
 	float getCurrentJerk() const { return _state.j; }
-	void setCurrentAcceleration(const float accel) { _state.a = _state_init.a = accel; }
+	void setCurrentAcceleration(const float accel)
+	{
+		if (PX4_ISFINITE(accel)) {
+			_state.a = accel;
+			_state_init.a = accel;
+
+		} else {
+			_state.a = 0.;
+			_state_init.a = 0.;
+		}
+	}
 	float getCurrentAcceleration() const { return _state.a; }
-	void setCurrentVelocity(const float vel) { _state.v = _state_init.v = vel; }
+	void setCurrentVelocity(const float vel)
+	{
+		if (PX4_ISFINITE(vel)) {
+			_state.v = vel;
+			_state_init.v = vel;
+
+		} else {
+			_state.v = 0.f;
+			_state_init.v = 0.f;
+		}
+	}
 	float getCurrentVelocity() const { return _state.v; }
-	void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; }
+	void setCurrentPosition(const float pos)
+	{
+		if (PX4_ISFINITE(pos)) {
+			_state.x = pos;
+			_state_init.x = pos;
+		}
+	}
 	float getCurrentPosition() const { return _state.x; }
 
 	float getVelSp() const { return _vel_sp; }
@@ -123,6 +152,12 @@ public:
 	 */
 	static void timeSynchronization(VelocitySmoothing *traj, int n_traj);
 
+	// [m/s^2] minimum value of the smoother's maximum acceleration
+	static constexpr float kMinAccel{0.1f};
+
+	// [m/s^3] minimum value of the smoother's maximum jerk
+	static constexpr float kMinJerk{0.1f};
+
 private:
 
 	/**