Add bezier trajectory tracking in obstacle avoidance

src/lib/flight_tasks/tasks/Utility/CMakeLists.txt

@@ -41,7 +41,7 @@ px4_add_library(FlightTaskUtility
 	ManualVelocitySmoothingZ.cpp
 )
 
-target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis)
+target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier)
 target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS FlightTaskUtility)

src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp

@@ -36,6 +36,7 @@
  */
 
 #include "ObstacleAvoidance.hpp"
+#include "bezier/BezierN.hpp"
 
 using namespace matrix;
 using namespace time_literals;
@@ -61,13 +62,21 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 {
 	_sub_vehicle_status.update();
 	_sub_vehicle_trajectory_waypoint.update();
+	_sub_vehicle_trajectory_bezier.update();
 
-	const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp)
+	const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get();
-					    > TRAJECTORY_STREAM_TIMEOUT_US;
+	const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get();
-	const bool avoidance_point_valid =
-		_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
 
-	_avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid, hrt_absolute_time());
+	const bool avoidance_data_timeout =
+		hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US &&
+		hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(bezier_msg.control_points[bezier_msg.bezier_order -
+							1].delta * 1e6f);
+
+	const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
+	const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0;
+
+	_avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid
+			&& !avoidance_bezier_valid, hrt_absolute_time());
 
 	const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state());
 
@@ -97,18 +106,63 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	}
 
 	if (avoidance_point_valid) {
-		pos_sp = Vector3f(_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
+		const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0];
-		vel_sp = Vector3f(_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
+		pos_sp = Vector3f(point0.position);
+		vel_sp = Vector3f(point0.velocity);
 
 		if (!_ext_yaw_active) {
 			// inject yaw setpoints only if weathervane isn't active
-			yaw_sp =  _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
+			yaw_sp =  point0.yaw;
-			yaw_speed_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
+			yaw_speed_sp = point0.yaw_speed;
-		}
 		}
 
+	} else if (avoidance_bezier_valid) {
+
+		float yaw = NAN, yaw_speed = NAN;
+		_generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed);
+
+		if (!_ext_yaw_active) {
+			// inject yaw setpoints only if weathervane isn't active
+			yaw_sp =  yaw;
+			yaw_speed_sp = yaw_speed;
+		}
+	}
 }
 
+void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity,
+		float &yaw, float &yaw_velocity)
+{
+	const auto &msg =  _sub_vehicle_trajectory_bezier.get();
+	int bezier_order = msg.bezier_order;
+	matrix::Vector3f bezier_points[bezier_order];
+	float bezier_yaws[bezier_order];
+
+	for (int i = 0; i < bezier_order; i++) {
+		bezier_points[i] = Vector3f(msg.control_points[i].position);
+		bezier_yaws[i] = msg.control_points[i].yaw;
+	}
+
+	const float duration_s = msg.control_points[bezier_order - 1].delta;
+	const hrt_abstime now = hrt_absolute_time();
+	const hrt_abstime start = msg.timestamp;
+	const hrt_abstime end = start + hrt_abstime(duration_s * 1e6f);
+
+	float T = NAN;
+
+	if (bezier::calculateT(start, end, now, T) &&
+	    bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) &&
+	    bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity)
+	   ) {
+		// translate velocities into real velocities
+		yaw_velocity *= duration_s;
+		velocity *= duration_s;
+
+	} else {
+		PX4_WARN("Obstacle Avoidance system failed, bad trajectory");
+	}
+}
+
+
 void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw,
 		const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed,
 		const bool ext_yaw_active, const int wp_type)

src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp

@@ -52,6 +52,7 @@
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_trajectory_bezier.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/position_setpoint.h>
 
@@ -116,6 +117,7 @@ public:
 
 protected:
 
+	uORB::SubscriptionData<vehicle_trajectory_bezier_s> _sub_vehicle_trajectory_bezier{ORB_ID(vehicle_trajectory_bezier)}; /**< vehicle trajectory waypoint subscription */
 	uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */
 	uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
 
@@ -140,6 +142,7 @@ protected:
 	 * Publishes vehicle command.
 	 */
 	void _publishVehicleCmdDoLoiter();
+	void _generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, float &yaw, float &yaw_velocity);
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad    /**< Acceptance radius for multicopter altitude */

src/lib/flight_tasks/tasks/Utility/ObstacleAvoidanceTest.cpp

@@ -101,6 +101,42 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
 	EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp));
 }
 
+TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier)
+{
+	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming
+	// from offboard
+	TestObstacleAvoidance oa;
+
+	vehicle_trajectory_bezier_s message {};
+	message.timestamp = hrt_absolute_time();
+	message.bezier_order = 1;
+	message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f;
+	message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f;
+	message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f;
+	message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f;
+	message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = 0.5f;
+
+	// GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message
+	uORB::Publication<vehicle_trajectory_bezier_s> vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
+	vehicle_trajectory_bezier_pub.publish(message);
+
+	vehicle_status_s vehicle_status{};
+	vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+	uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
+	vehicle_status_pub.publish(vehicle_status);
+
+	// WHEN: we inject the vehicle_trajectory_waypoint in the interface
+	oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
+
+	// THEN: the setpoints should be injected
+	EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0], pos_sp(0));
+	EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1], pos_sp(1));
+	EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2], pos_sp(2));
+	EXPECT_FLOAT_EQ(vel_sp.norm(), 0);
+	EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw, yaw_sp);
+	EXPECT_FLOAT_EQ(yaw_speed_sp, 0);
+}
+
 TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy)
 {
 	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message