integrated geofence aware path planner into rtl direct navigation mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/CMakeLists.txt

@@ -50,6 +50,7 @@ add_subdirectory(dataman_client EXCLUDE_FROM_ALL)
 add_subdirectory(drivers EXCLUDE_FROM_ALL)
 add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL)
 add_subdirectory(geo EXCLUDE_FROM_ALL)
+add_subdirectory(geofence EXCLUDE_FROM_ALL)
 add_subdirectory(gnss EXCLUDE_FROM_ALL)
 add_subdirectory(heatshrink EXCLUDE_FROM_ALL)
 add_subdirectory(hysteresis EXCLUDE_FROM_ALL)

src/modules/navigator/CMakeLists.txt

@@ -76,8 +76,10 @@ px4_add_module(
 	DEPENDS
 		dataman_client
 		geo
+		geofence_utils
 		adsb
 		geofence_breach_avoidance
+		geofence_avoidance_planner
 		motion_planning
 		mission_feasibility_checker
 		rtl_time_estimator

src/modules/navigator/navigation.h

@@ -200,6 +200,15 @@ struct mission_stats_entry_s {
 	uint8_t padding[1];
 };
 
+struct PolygonInfo {
+	uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region)
+	uint16_t dataman_index;
+	union {
+		uint16_t vertex_count;
+		float circle_radius;
+	};
+};
+
 /**
  * Geofence vertex point.
  * Corresponds to the DM_KEY_FENCE_POINTS_0 dataman item

src/modules/navigator/navigator.h

@@ -312,6 +312,12 @@ public:
 
 	void trigger_hagl_failsafe(uint8_t nav_state);
 
+	PlannedPath planPathToDestination(const matrix::Vector2<double> &start, const matrix::Vector2<double> &destination,
+					  float margin)
+	{
+		return _geofence.planPathToDestination(start, destination, margin);
+	}
+
 private:
 
 	int _local_pos_sub{-1};

src/modules/navigator/rtl.cpp

@@ -182,6 +182,7 @@ void RTL::on_inactive()
 	if ((now - _destination_check_time) > 2_s) {
 		_destination_check_time = now;
 		setRtlTypeAndDestination();
+		updateRtlPath();
 		publishRemainingTimeEstimate();
 	}
 
@@ -218,6 +219,25 @@ void RTL::publishRemainingTimeEstimate()
 	_rtl_time_estimate_pub.publish(estimated_time);
 }
 
+void RTL::updateRtlPath()
+{
+
+	const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0
+			&& hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s;
+
+	if (_navigator->home_global_position_valid() && global_position_recently_updated) {
+		switch (_rtl_type) {
+		case RtlType::RTL_DIRECT:
+			_rtl_direct.updateRtlPath();
+			break;
+
+
+		default:
+			break;
+		}
+	}
+}
+
 void RTL::on_activation()
 {
 	_global_pos_sub.update();

src/modules/navigator/rtl.h

@@ -126,6 +126,8 @@ private:
 	 */
 	void publishRemainingTimeEstimate();
 
+	void updateRtlPath();
+
 	/**
 	 * @brief Find RTL destination.
 	 *

src/modules/navigator/rtl_direct.cpp

@@ -146,13 +146,30 @@ void RtlDirect::_updateRtlState()
 {
 	// RTL_LAND_DELAY > 0 -> wait seconds, < 0 wait indefinitely
 	const bool wait_at_rtl_descend_alt = fabsf(_param_rtl_land_delay.get()) > FLT_EPSILON;
-	const bool is_multicopter = (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+	const bool is_multicopter = (_vehicle_status_sub.get().vehicle_type ==
+				     vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
 
 	RTLState new_state{RTLState::IDLE};
 
 	switch (_rtl_state) {
 	case RTLState::CLIMBING:
-		new_state = RTLState::MOVE_TO_LOITER;
+		if (_geofence_aware_return_path.hasNextPoint()) {
+			new_state = RTLState::MOVE_TO_INTERMEDIATE_POINT;
+
+		} else {
+			new_state = RTLState::MOVE_TO_LOITER;
+		}
+
+		break;
+
+	case RTLState::MOVE_TO_INTERMEDIATE_POINT:
+		if (_geofence_aware_return_path.hasNextPoint()) {
+			new_state = RTLState::MOVE_TO_INTERMEDIATE_POINT;
+
+		} else {
+			new_state = RTLState::MOVE_TO_LOITER;
+		}
+
 		break;
 
 	case RTLState::MOVE_TO_LOITER:
@@ -171,7 +188,8 @@ void RtlDirect::_updateRtlState()
 
 	case RTLState::LOITER_HOLD:
 		if (_vehicle_status_sub.get().is_vtol
-		    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		    && _vehicle_status_sub.get().vehicle_type ==
+		    vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 			new_state = RTLState::MOVE_TO_LAND;
 
 		} else {
@@ -205,7 +223,6 @@ void RtlDirect::_updateRtlState()
 	_rtl_state = new_state;
 }
 
-
 void RtlDirect::set_rtl_item()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -231,10 +248,25 @@ void RtlDirect::set_rtl_item()
 			break;
 		}
 
-	case RTLState::MOVE_TO_LOITER: {
+	case RTLState::MOVE_TO_INTERMEDIATE_POINT: {
+
+			const matrix::Vector2d point = _geofence_aware_return_path.getNextPoint();
 			PositionYawSetpoint pos_yaw_sp {
-				.lat = _land_approach.lat,
-				.lon = _land_approach.lon,
+				.lat = point(0),
+				.lon = point(1),
+				.alt = _rtl_alt,
+			};
+
+			setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);
+			break;
+		}
+
+	case RTLState::MOVE_TO_LOITER: {
+
+			const loiter_point_s current_destination = _land_approach;
+			PositionYawSetpoint pos_yaw_sp {
+				.lat = current_destination.lat,
+				.lon = current_destination.lon,
 				.alt = _rtl_alt,
 			};
 
@@ -242,7 +274,7 @@ void RtlDirect::set_rtl_item()
 			// can be displayed on groundstation and the WP is accepted once within loiter radius
 			if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 				pos_yaw_sp.yaw = NAN;
-				setLoiterHoldMissionItem(_mission_item, pos_yaw_sp, 0.f, _land_approach.loiter_radius_m);
+				setLoiterHoldMissionItem(_mission_item, pos_yaw_sp, 0.f, current_destination.loiter_radius_m);
 
 			} else {
 				// already set final yaw if close to destination and weather vane is disabled
@@ -404,6 +436,9 @@ RtlDirect::RTLState RtlDirect::getActivationState()
 	} else if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) {
 		activation_state = RTLState::CLIMBING;
 
+	} else if (_geofence_aware_return_path.hasNextPoint()) {
+		activation_state = RTLState::MOVE_TO_INTERMEDIATE_POINT;
+
 	} else {
 		activation_state = RTLState::MOVE_TO_LOITER;
 	}
@@ -444,12 +479,26 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 				}
 			}
 
+		// FALLTHROUGH
+		case RTLState::MOVE_TO_INTERMEDIATE_POINT: {
+				matrix::Vector2f direction{};
+				matrix::Vector2d intermediate_point = _geofence_aware_return_path.getCurrentPoint();
+				get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, intermediate_point(0),
+							    intermediate_point(1), &direction(0), &direction(1));
+				float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, intermediate_point(0), intermediate_point(1))};
+
+				_rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f);
+			}
+
 		// FALLTHROUGH
 		case RTLState::MOVE_TO_LOITER: {
 				matrix::Vector2f direction{};
-				get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat,
+				matrix::Vector2d intermediate_point = _geofence_aware_return_path.getCurrentPoint();
+				const matrix::Vector2d start_position = intermediate_point.isAllFinite() ? matrix::Vector2d(intermediate_point(0), intermediate_point(1))
+									: matrix::Vector2d(_global_pos_sub.get().lat, _global_pos_sub.get().lon);
+				get_vector_to_next_waypoint(start_position(0), start_position(1), land_approach.lat,
 							    land_approach.lon, &direction(0), &direction(1));
-				float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon)};
+				float move_to_land_dist{get_distance_to_next_waypoint(start_position(0), start_position(1), land_approach.lat, land_approach.lon)};
 
 				if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 					move_to_land_dist = max(0.f, move_to_land_dist - land_approach.loiter_radius_m);
@@ -604,3 +653,10 @@ void RtlDirect::publish_rtl_direct_navigator_mission_item()
 
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }
+
+void RtlDirect::updateRtlPath()
+{
+	const matrix::Vector2d current_position{_global_pos_sub.get().lat, _global_pos_sub.get().lon};
+	_geofence_aware_return_path = _navigator->planPathToDestination(current_position, matrix::Vector2d(_destination.lat, _destination.lon),
+				      2.0f * _navigator->get_acceptance_radius());
+}

src/modules/navigator/rtl_direct.h

@@ -55,6 +55,7 @@
 #include <uORB/topics/wind.h>
 
 #include <lib/rtl/rtl_time_estimator.h>
+#include <navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h>
 #include "mission_block.h"
 #include "navigation.h"
 #include "safe_point_land.hpp"
@@ -111,6 +112,8 @@ public:
 
 	bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);};
 
+	void updateRtlPath();
+
 private:
 	/**
 	 * @brief Return to launch state machine.
@@ -118,6 +121,7 @@ private:
 	 */
 	enum class RTLState {
 		CLIMBING,
+		MOVE_TO_INTERMEDIATE_POINT,
 		MOVE_TO_LOITER,
 		LOITER_DOWN,
 		LOITER_HOLD,
@@ -168,7 +172,7 @@ private:
 
 	PositionYawSetpoint _destination{(double)NAN, (double)NAN, NAN, NAN}; ///< the RTL position to fly to
 	loiter_point_s _land_approach;
-
+	PlannedPath _geofence_aware_return_path;
 	float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should transit to the destination
 
 	DEFINE_PARAMETERS(