diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp index 65e94eb098b..fd3765691d1 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp @@ -96,12 +96,11 @@ TEST_F(GeofenceAvoidancePlannerTest, StartEqualsDestination) FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION); _planner.update_vertices(fake); - _planner.update_start(point, fake); _planner.update_destination(point, fake); - PlannedPath path = _planner.planPath(); + const int num_waypoints = _planner.set_start_and_plan_path_to_destination(point, fake); - ASSERT_EQ(path.num_points, 0); + ASSERT_EQ(num_waypoints, 0); } TEST_F(GeofenceAvoidancePlannerTest, DirectPathNoFence) @@ -112,18 +111,17 @@ TEST_F(GeofenceAvoidancePlannerTest, DirectPathNoFence) FakeGeofence fake(nullptr, 0, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION); _planner.update_vertices(fake); - _planner.update_start(start, fake); _planner.update_destination(destination, fake); - PlannedPath path = _planner.planPath(); + const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake); - ASSERT_EQ(path.num_points, 0); + ASSERT_EQ(num_waypoints, 0); } TEST_F(GeofenceAvoidancePlannerTest, PathAroundExclusionZone) { // in this test there is an exclusion zone between start and destination. The shortest path - // is chosen to be via vertex nr 2 and vertex nr 3, so the path should have two points + // is chosen to be via vertex nr 1 and vertex nr 2, so the path should have two waypoints using namespace matrix; Vector2 start(47.3559582, 8.5192064); Vector2 destination(47.3546153, 8.5193195); @@ -137,19 +135,23 @@ TEST_F(GeofenceAvoidancePlannerTest, PathAroundExclusionZone) FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION); _planner.update_vertices(fake); - _planner.update_start(start, fake); _planner.update_destination(destination, fake); - PlannedPath path = _planner.planPath(); + const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake); - // points[0] is the start itself (always emitted); optimal detour is via vertex 1 and 2 - ASSERT_EQ(path.num_points, 3); - EXPECT_NEAR(path.points[0](0), start(0), 1e-3); - EXPECT_NEAR(path.points[0](1), start(1), 1e-3); - EXPECT_NEAR(path.points[1](0), vertices[1](0), 1e-3); - EXPECT_NEAR(path.points[1](1), vertices[1](1), 1e-3); - EXPECT_NEAR(path.points[2](0), vertices[2](0), 1e-3); - EXPECT_NEAR(path.points[2](1), vertices[2](1), 1e-3); + // start is the current vehicle position, so it is not part of the returned path. + // path = vertex 1 + vertex 2 (2 points) + ASSERT_EQ(num_waypoints, 2); + + // index 0 is the first waypoint after start + const Vector2d first_wp = _planner.get_point_at_index(0); + EXPECT_NEAR(first_wp(0), vertices[1](0), 1e-3); + EXPECT_NEAR(first_wp(1), vertices[1](1), 1e-3); + + // index num_waypoints - 1 is the last waypoint before destination + const Vector2d last_wp = _planner.get_point_at_index(num_waypoints - 1); + EXPECT_NEAR(last_wp(0), vertices[2](0), 1e-3); + EXPECT_NEAR(last_wp(1), vertices[2](1), 1e-3); } TEST_F(GeofenceAvoidancePlannerTest, PathInsideInclusionZone) @@ -170,17 +172,19 @@ TEST_F(GeofenceAvoidancePlannerTest, PathInsideInclusionZone) FakeGeofence fake(vertices, 6, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION); _planner.update_vertices(fake); - _planner.update_start(start, fake); _planner.update_destination(destination, fake); - PlannedPath path = _planner.planPath(); + const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake); - ASSERT_EQ(path.num_points, 2); - ASSERT_NEAR(path.points[0](0), start(0), 1e-3); - ASSERT_NEAR(path.points[0](1), start(1), 1e-3); - ASSERT_NEAR(path.points[1](0), vertices[5](0), 1e-3); - ASSERT_NEAR(path.points[1](1), vertices[5](1), 1e-3); + // start is the current vehicle position, so it is not part of the returned path. + // path = vertex 5 (1 point) + ASSERT_EQ(num_waypoints, 1); + + const Vector2d only_wp = _planner.get_point_at_index(0); + EXPECT_NEAR(only_wp(0), vertices[5](0), 1e-3); + EXPECT_NEAR(only_wp(1), vertices[5](1), 1e-3); } + TEST_F(GeofenceAvoidancePlannerTest, DestinationOutsideInclusion) { using namespace matrix; @@ -199,13 +203,13 @@ TEST_F(GeofenceAvoidancePlannerTest, DestinationOutsideInclusion) FakeGeofence fake(vertices, 6, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION); _planner.update_vertices(fake); - _planner.update_start(start, fake); _planner.update_destination(destination, fake); - PlannedPath path = _planner.planPath(); + const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake); - ASSERT_EQ(path.num_points, 0); + ASSERT_EQ(num_waypoints, 0); } + TEST_F(GeofenceAvoidancePlannerTest, DestinationInsideExclusion) { using namespace matrix; @@ -223,13 +227,13 @@ TEST_F(GeofenceAvoidancePlannerTest, DestinationInsideExclusion) FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION); _planner.update_vertices(fake); - _planner.update_start(start, fake); _planner.update_destination(destination, fake); - PlannedPath path = _planner.planPath(); + const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake); - ASSERT_EQ(path.num_points, 0); + ASSERT_EQ(num_waypoints, 0); } + TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion) { using namespace matrix; @@ -238,7 +242,6 @@ TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion) Vector2 destination(47.3559582, 8.5192064); - static const Vector2 vertices[] = { {47.3552420, 8.5192293}, {47.3555843, 8.5201174}, @@ -248,13 +251,45 @@ TEST_F(GeofenceAvoidancePlannerTest, StartInsideExclusion) FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION); _planner.update_vertices(fake); - _planner.update_start(start, fake); _planner.update_destination(destination, fake); - PlannedPath path = _planner.planPath(); + const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake); - ASSERT_EQ(path.num_points, 0); + ASSERT_EQ(num_waypoints, 0); } + +TEST_F(GeofenceAvoidancePlannerTest, StartInsideGeofence) +{ + using namespace matrix; + // Start and destination are both in valid airspace (north of the exclusion zone), so the + // destination is directly reachable from the start without any detour. + Vector2 start(47.3559582, 8.5192064); + Vector2 destination(47.3560100, 8.5192300); + + static const Vector2 vertices[] = { + {47.3552420, 8.5192293}, + {47.3555843, 8.5201174}, + {47.3551382, 8.5209143}, + {47.3550828, 8.5171901}, + }; + + FakeGeofence fake(vertices, 4, NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION); + _planner.update_vertices(fake); + _planner.update_destination(destination, fake); + + // Vehicle was outside the fence at plan time, so the start is used as an anchor the + // vehicle must fly back to. Even though the destination is directly visible from there, + // the path still has to include the anchor as the first (and only) waypoint. + const int num_waypoints = + _planner.set_start_and_plan_path_to_destination(start, fake, /*start_is_current_position=*/false); + + ASSERT_EQ(num_waypoints, 1); + + const Vector2d anchor_wp = _planner.get_point_at_index(0); + EXPECT_NEAR(anchor_wp(0), start(0), 1e-3); + EXPECT_NEAR(anchor_wp(1), start(1), 1e-3); +} + TEST_F(GeofenceAvoidancePlannerTest, NanStartOrDestination) { using namespace matrix; @@ -266,15 +301,14 @@ TEST_F(GeofenceAvoidancePlannerTest, NanStartOrDestination) _planner.update_vertices(fake); auto plan = [&](const Vector2 &s, const Vector2 &d) { - _planner.update_start(s, fake); _planner.update_destination(d, fake); - return _planner.planPath(); + return _planner.set_start_and_plan_path_to_destination(s, fake); }; - EXPECT_EQ(plan(nan_lat, valid).num_points, 0); - EXPECT_EQ(plan(nan_lon, valid).num_points, 0); - EXPECT_EQ(plan(valid, nan_lat).num_points, 0); - EXPECT_EQ(plan(valid, nan_lon).num_points, 0); + EXPECT_EQ(plan(nan_lat, valid), 0); + EXPECT_EQ(plan(nan_lon, valid), 0); + EXPECT_EQ(plan(valid, nan_lat), 0); + EXPECT_EQ(plan(valid, nan_lon), 0); } TEST_F(GeofenceAvoidancePlannerTest, LatLonOutOfBounds) @@ -290,19 +324,18 @@ TEST_F(GeofenceAvoidancePlannerTest, LatLonOutOfBounds) _planner.update_vertices(fake); auto plan = [&](const Vector2 &s, const Vector2 &d) { - _planner.update_start(s, fake); _planner.update_destination(d, fake); - return _planner.planPath(); + return _planner.set_start_and_plan_path_to_destination(s, fake); }; - EXPECT_EQ(plan(lat_too_high, valid).num_points, 0); - EXPECT_EQ(plan(lat_too_low, valid).num_points, 0); - EXPECT_EQ(plan(lon_too_high, valid).num_points, 0); - EXPECT_EQ(plan(lon_too_low, valid).num_points, 0); - EXPECT_EQ(plan(valid, lat_too_high).num_points, 0); - EXPECT_EQ(plan(valid, lat_too_low).num_points, 0); - EXPECT_EQ(plan(valid, lon_too_high).num_points, 0); - EXPECT_EQ(plan(valid, lon_too_low).num_points, 0); + EXPECT_EQ(plan(lat_too_high, valid), 0); + EXPECT_EQ(plan(lat_too_low, valid), 0); + EXPECT_EQ(plan(lon_too_high, valid), 0); + EXPECT_EQ(plan(lon_too_low, valid), 0); + EXPECT_EQ(plan(valid, lat_too_high), 0); + EXPECT_EQ(plan(valid, lat_too_low), 0); + EXPECT_EQ(plan(valid, lon_too_high), 0); + EXPECT_EQ(plan(valid, lon_too_low), 0); } TEST_F(GeofenceAvoidancePlannerTest, DuplicateNeighborVertex) @@ -326,11 +359,10 @@ TEST_F(GeofenceAvoidancePlannerTest, DuplicateNeighborVertex) FakeGeofence fake(vertices, 7, NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION); _planner.update_vertices(fake); - _planner.update_start(start, fake); _planner.update_destination(destination, fake); - PlannedPath path = _planner.planPath(); + const int num_waypoints = _planner.set_start_and_plan_path_to_destination(start, fake); // THEN the pathplanner should fail - ASSERT_EQ(path.num_points, 0); + ASSERT_EQ(num_waypoints, 0); } diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp index d05bd88cfaf..dd1b6559b83 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp @@ -34,12 +34,12 @@ #include "geofence_avoidance_planner.h" #include #include +#include GeofenceAvoidancePlanner::~GeofenceAvoidancePlanner() { perf_free(_setup_perf); perf_free(_setup_distances_perf); - perf_free(_update_start_perf); perf_free(_update_destination_perf); perf_free(_plan_path_perf); } @@ -55,104 +55,22 @@ static matrix::Vector2f get_vertex_local_position(int poly_index, int vertex_idx return local; } -const PlannedPath &GeofenceAvoidancePlanner::planPath() +void GeofenceAvoidancePlanner::planPath() { - // The navigator task stack is only ~2 KB, so we keep the result as a member - // and return it by reference to avoid overflowing the stack on RTL entry. - _planned_path.num_points = 0; - _planned_path.current_index = 0; - - if (!_polygons_healthy || !_start_healthy || !_destination_healthy) { + if (!_polygons_healthy || !_destination_healthy) { // we are not in a state where we can reliably plan a path, return an empty one - return _planned_path; + return; } perf_begin(_plan_path_perf); - reset_graph_state(); - // reset to the start location - int graph_index = 0; + dijkstra::solveBackward(_num_nodes, _num_nodes - 1, _distances, true, _best_distance, + _next_node_buffer, _visited_buffer); - while (true) { - // update the best distances to all neighbouring nodes - float min_dist = INFINITY; - int next_graph_index = -1; - - for (int i = 0; i < _num_nodes; i++) { - if (i == graph_index) { - continue; - } - - const float distance_to_neighbor = _distances[geofence_utils::symmetricPairIndex(graph_index, i, _num_nodes)]; - - if (distance_to_neighbor < INFINITY) { - const float total_distance_to_neighbor = distance_to_neighbor + _graph_nodes[graph_index].best_distance; - - if (total_distance_to_neighbor < _graph_nodes[i].best_distance) { - _graph_nodes[i].best_distance = total_distance_to_neighbor; - _graph_nodes[i].previous_index = graph_index; - } - } - - if (!_graph_nodes[i].visited && _graph_nodes[i].best_distance < min_dist) { - min_dist = _graph_nodes[i].best_distance; - next_graph_index = i; - } - } - - graph_index = next_graph_index; - - if (graph_index < 0) { - // we are stuck, destination does not seem to be reachable - perf_end(_plan_path_perf); - return _planned_path; - } - - _graph_nodes[graph_index].visited = true; - - if (_graph_nodes[graph_index].type == Node::DESTINATION) { - break; - } - - } - - // backtrack from destination to start and count the nodes along the way - int count = 0; - int idx = graph_index; - - while (idx != 0) { - if (idx < 0) { - // can happen if the destination is not reachable - perf_end(_plan_path_perf); - return _planned_path; - } - - count++; - idx = _graph_nodes[idx].previous_index; - } - - _planned_path.num_points = count; - - // Walk backwards and fill points in reverse, converting local to lat/lon. - // The last iteration lands on the start node, which becomes points[0]. - MapProjection ref{_reference(0), _reference(1)}; - idx = _graph_nodes[graph_index].previous_index; // skip destination - - for (int i = _planned_path.num_points - 1; i >= 0; i--) { - if (idx < 0) { - // this should never happen, but just in case, we return an empty path - _planned_path.num_points = 0; - break; - } - - ref.reproject(_graph_nodes[idx].position(0), _graph_nodes[idx].position(1), - _planned_path.points[i](0), _planned_path.points[i](1)); - idx = _graph_nodes[idx].previous_index; - } perf_end(_plan_path_perf); - return _planned_path; + return; } bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, float margin) @@ -174,7 +92,7 @@ bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, floa } } - if (num_vertices == 0 || num_vertices > kMaxNodes - 2) { // -2 to reserve start and destination slots + if (num_vertices == 0 || num_vertices > kMaxNodes - 1) { // -1 to reserve the destination slot _polygons_healthy = false; return false; } @@ -191,22 +109,21 @@ bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, floa update_distances_between_vertices(geofence); - // invalidate start and end to make sure they are refreshed - _start_healthy = false; + // invalidate destination to force a refresh on the next update_destination() _destination_healthy = false; perf_end(_setup_perf); + + planPath(); return true; } bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination( GeofenceInterface &geofence, float margin) { - // local frame is anchored at _reference (set by update_vertices); this can be computed - // once up-front; start and destination nodes are filled in later when they are known const int num_polygons = geofence.getNumPolygons(); - int node_index = 1; // reserve index 0 for the start + int node_index = 0; for (int poly_index = 0; poly_index < num_polygons; poly_index++) { @@ -229,11 +146,7 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination( } for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) { - _graph_nodes[node_index].type = Node::Type::VERTEX; - _graph_nodes[node_index].position = local_out[vertex_idx]; - _graph_nodes[node_index].best_distance = FLT_MAX; - _graph_nodes[node_index].previous_index = -1; - _graph_nodes[node_index].visited = false; + _positions[node_index] = local_out[vertex_idx]; node_index++; } @@ -256,18 +169,14 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination( for (int i = 0; i < kCircleApproxVertices; i++) { const float angle = i * delta_angle; - _graph_nodes[node_index].type = Node::Type::VERTEX; - _graph_nodes[node_index].position = center + matrix::Vector2f{adjusted_radius * cosf(angle), adjusted_radius * sinf(angle)}; - _graph_nodes[node_index].best_distance = FLT_MAX; - _graph_nodes[node_index].previous_index = -1; - _graph_nodes[node_index].visited = false; + _positions[node_index] = center + matrix::Vector2f{adjusted_radius * cosf(angle), adjusted_radius * sinf(angle)}; node_index++; } } } - // node_index now points at the reserved destination slot; total count includes it - _num_vertices = node_index - 1; + // node_index equals the polygon vertex count; the destination is reserved one slot beyond + _num_vertices = node_index; _num_nodes = node_index + 1; // +1 for the destination return true; @@ -276,20 +185,17 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination( void GeofenceAvoidancePlanner::update_distances_between_vertices(GeofenceInterface &geofence) { perf_begin(_setup_distances_perf); - // vertices occupy indices 1 .. _num_vertices; index 0 and the last slot hold the - // start and destination, which are handled by update_start / update_destination - const int last_vertex = _num_vertices; // loop through all possible vertex to vertex combinations and store the distance between them - for (int i = 1; i <= last_vertex; i++) { - for (int j = i + 1; j <= last_vertex; j++) { + for (int i = 0; i < _num_vertices; i++) { + for (int j = i + 1; j < _num_vertices; j++) { const size_t idx = geofence_utils::symmetricPairIndex(i, j, _num_nodes); - const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[i].position, - _graph_nodes[j].position, _reference); + const bool clear = !geofence.checkIfLineViolatesAnyFence(_positions[i], + _positions[j], _reference); if (clear) { - _distances[idx] = (_graph_nodes[i].position - _graph_nodes[j].position).norm(); + _distances[idx] = (_positions[i] - _positions[j]).norm(); } else { _distances[idx] = INFINITY; @@ -300,56 +206,6 @@ void GeofenceAvoidancePlanner::update_distances_between_vertices(GeofenceInterfa perf_end(_setup_distances_perf); } -void GeofenceAvoidancePlanner::update_start(const matrix::Vector2d &start, GeofenceInterface &geofence) -{ - if (!start.isAllFinite() || !lat_lon_within_bounds(start) || !_polygons_healthy) { - _start_healthy = false; - return; - } - - perf_begin(_update_start_perf); - - MapProjection ref{_reference(0), _reference(1)}; - matrix::Vector2f start_local; - ref.project(start(0), start(1), start_local(0), start_local(1)); - - _graph_nodes[0].position = start_local; - - // distances from start to each vertex - for (int graph_idx = 1; graph_idx <= _num_vertices; graph_idx++) { - const size_t dist_idx = geofence_utils::symmetricPairIndex(0, graph_idx, _num_nodes); - - const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[0].position, - _graph_nodes[graph_idx].position, _reference); - - if (clear) { - _distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[graph_idx].position).norm(); - - } else { - _distances[dist_idx] = INFINITY; - } - } - - // refresh start <-> destination edge if destination is already known - if (_destination_healthy) { - const int dest_idx = _num_nodes - 1; - const size_t dist_idx = geofence_utils::symmetricPairIndex(0, dest_idx, _num_nodes); - - const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[0].position, - _graph_nodes[dest_idx].position, _reference); - - if (clear) { - _distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[dest_idx].position).norm(); - - } else { - _distances[dist_idx] = INFINITY; - } - } - - _start_healthy = true; - perf_end(_update_start_perf); -} - void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destination, GeofenceInterface &geofence) { if (!destination.isAllFinite() || !lat_lon_within_bounds(destination) || !_polygons_healthy) { @@ -363,38 +219,23 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin const int dest_idx = _num_nodes - 1; - if (_destination_healthy && (dest_local - _graph_nodes[dest_idx].position).norm() < FLT_EPSILON) { + if (_destination_healthy && (dest_local - _positions[dest_idx]).norm() < FLT_EPSILON) { // no change in destination, skip the update return; } perf_begin(_update_destination_perf); - _graph_nodes[dest_idx].position = dest_local; + _positions[dest_idx] = dest_local; // distances from each vertex to destination - for (int graph_idx = 1; graph_idx <= _num_vertices; graph_idx++) { + for (int graph_idx = 0; graph_idx < _num_vertices; graph_idx++) { const size_t dist_idx = geofence_utils::symmetricPairIndex(graph_idx, dest_idx, _num_nodes); - const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[graph_idx].position, - _graph_nodes[dest_idx].position, _reference); + const bool clear = !geofence.checkIfLineViolatesAnyFence(_positions[graph_idx], + _positions[dest_idx], _reference); if (clear) { - _distances[dist_idx] = (_graph_nodes[graph_idx].position - _graph_nodes[dest_idx].position).norm(); - - } else { - _distances[dist_idx] = INFINITY; - } - } - - // refresh start <-> destination edge if start is already known - if (_start_healthy) { - const size_t dist_idx = geofence_utils::symmetricPairIndex(0, dest_idx, _num_nodes); - - const bool clear = !geofence.checkIfLineViolatesAnyFence(_graph_nodes[0].position, - _graph_nodes[dest_idx].position, _reference); - - if (clear) { - _distances[dist_idx] = (_graph_nodes[0].position - _graph_nodes[dest_idx].position).norm(); + _distances[dist_idx] = (_positions[graph_idx] - _positions[dest_idx]).norm(); } else { _distances[dist_idx] = INFINITY; @@ -403,6 +244,8 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin _destination_healthy = true; perf_end(_update_destination_perf); + + planPath(); } bool GeofenceAvoidancePlanner::lat_lon_within_bounds(const matrix::Vector2 &lat_lon) @@ -414,21 +257,105 @@ bool GeofenceAvoidancePlanner::lat_lon_within_bounds(const matrix::Vector2= total_points) { + return nan_point; + } + + if (anchor_offset == 1 && index == 0) { + matrix::Vector2d start_global; + MapProjection ref{_reference(0), _reference(1)}; + ref.reproject(_start_local(0), _start_local(1), start_global(0), start_global(1)); + return start_global; + } + + if (_best_starting_index < 0) { + return nan_point; + } + + int next = _best_starting_index; + + for (int i = 0; i < index - anchor_offset; i++) { + next = _next_node_buffer[next]; + } + + if (next < 0 || next >= _num_nodes) { + return nan_point; + } + + matrix::Vector2d global; + MapProjection ref{_reference(0), _reference(1)}; + ref.reproject(_positions[next](0), _positions[next](1), global(0), global(1)); + return global; } diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h index a3517024086..8a66d0025ee 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h @@ -119,37 +119,52 @@ public: GeofenceAvoidancePlanner() = default; ~GeofenceAvoidancePlanner(); - const PlannedPath &planPath(); - bool update_vertices(GeofenceInterface &geofence, float margin = 10.0f); - void update_start(const matrix::Vector2d &start, GeofenceInterface &geofence); + int set_start_and_plan_path_to_destination(matrix::Vector2d start, GeofenceInterface &geofence, + bool start_is_current_position = true); + + matrix::Vector2d get_point_at_index(int index) const; + + // True if the start passed to the last plan call was the vehicle's current position. False means + // planning was anchored to a stored point (e.g. last valid in-fence position when the vehicle is + // outside the fence), so the leg from current position to point 0 is unaccounted for in the path. + bool start_is_current_position() const { return _start_is_current_position; } + + bool update_vertices(GeofenceInterface &geofence, float margin = 50.0f); + void update_destination(const matrix::Vector2d &destination, GeofenceInterface &geofence); private: static constexpr int num_distances_in_graph = (kMaxNodes) * (kMaxNodes - 1) / 2; - Node _graph_nodes[kMaxNodes]; + float _best_distance[kMaxNodes]; float _distances[num_distances_in_graph]; - PlannedPath _planned_path{}; + int _next_node_buffer[kMaxNodes]; + bool _visited_buffer[kMaxNodes]; + + int _best_starting_index{-1}; + int _num_path_points{0}; + + matrix::Vector2f _start_local; + + matrix::Vector2f _positions[kMaxNodes]; int _num_nodes{0}; int _num_vertices{0}; matrix::Vector2 _reference; // lat/lon anchor of the local frame bool _polygons_healthy{false}; - bool _start_healthy{false}; bool _destination_healthy{false}; + bool _start_is_current_position{true}; perf_counter_t _setup_perf{perf_alloc(PC_ELAPSED, "rtl_planner: setup")}; perf_counter_t _setup_distances_perf{perf_alloc(PC_ELAPSED, "rtl_planner: setup distances")}; - perf_counter_t _update_start_perf{perf_alloc(PC_ELAPSED, "rtl_planner: update start")}; perf_counter_t _update_destination_perf{perf_alloc(PC_ELAPSED, "rtl_planner: update destination")}; perf_counter_t _plan_path_perf{perf_alloc(PC_ELAPSED, "rtl_planner: plan path")}; bool update_graph_nodes_without_start_and_destination(GeofenceInterface &geofence, float margin); void update_distances_between_vertices(GeofenceInterface &geofence); - - void reset_graph_state(); + void planPath(); bool lat_lon_within_bounds(const matrix::Vector2 &lat_lon); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 87e8600b14c..4088e9be4ca 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -144,9 +144,14 @@ public: bool isHomeRequired(); - void updateStartForRTLPathPlanner(const matrix::Vector2 &start) + int set_start_and_start_path_to_destination(const matrix::Vector2 &start, bool start_is_current_position = true) { - _avoidance_planner.update_start(start, *this); + return _avoidance_planner.set_start_and_plan_path_to_destination(start, *this, start_is_current_position); + } + + bool plannerStartIsCurrentPosition() const + { + return _avoidance_planner.start_is_current_position(); } void updateDestinationForRTLPathPlanner(const matrix::Vector2 &destination) @@ -154,7 +159,11 @@ public: _avoidance_planner.update_destination(destination, *this); } - const PlannedPath &planPath() { return _avoidance_planner.planPath(); } + const matrix::Vector2d get_point_at_index(int index) const + { + return _avoidance_planner.get_point_at_index(index); + + } /** * print Geofence status to the console diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f45f6b4df5e..22b3bda17a4 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -312,9 +312,9 @@ public: void trigger_hagl_failsafe(uint8_t nav_state); - void updateStartOfRTLPathPlanner(const matrix::Vector2 &start) + int set_start_and_plan_path_to_destination(const matrix::Vector2 &start, bool start_is_current_position = true) { - _geofence.updateStartForRTLPathPlanner(start); + return _geofence.set_start_and_start_path_to_destination(start, start_is_current_position); } void updateDestinationOfRTLPathPlanner(const matrix::Vector2 &destination) @@ -322,7 +322,15 @@ public: _geofence.updateDestinationForRTLPathPlanner(destination); } - const PlannedPath &planPath() { return _geofence.planPath(); } + matrix::Vector2d get_point_at_index(int index) const + { + return _geofence.get_point_at_index(index); + } + + bool geofencePlannerStartIsCurrentPosition() const + { + return _geofence.plannerStartIsCurrentPosition(); + } /** * Returns the last position that was confirmed to be inside all geofences. @@ -331,6 +339,8 @@ public: */ matrix::Vector2 getRtlPlanningStart(); + bool was_last_position_outside_fence() const { return _last_position_was_outside_fence; } + private: int _local_pos_sub{-1}; @@ -386,6 +396,7 @@ private: hrt_abstime _last_geofence_check{0}; matrix::Vector2d _last_valid_position_in_fence{static_cast(NAN), static_cast(NAN)}; + bool _last_position_was_outside_fence{false}; bool _navigator_status_updated{false}; hrt_abstime _last_navigator_status_publication{0}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 469033b14bc..820215f8a42 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1040,6 +1040,10 @@ void Navigator::geofence_breach_check() // latch the current position as a valid RTL planner start whenever no fence is violated if (!_geofence_result.geofence_max_dist_triggered && !_geofence_result.geofence_custom_fence_triggered) { _last_valid_position_in_fence = matrix::Vector2 {current_latitude, current_longitude}; + _last_position_was_outside_fence = false; + + } else { + _last_position_was_outside_fence = true; } _geofence_result.timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 0356cf2296e..d828e978161 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -186,8 +186,12 @@ void RTL::on_inactive() && hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s; if (global_position_recently_updated) { - _navigator->updateStartOfRTLPathPlanner(_navigator->getRtlPlanningStart()); - _rtl_direct.updatePlannedPath(); + if (_navigator->was_last_position_outside_fence()) { + _navigator->set_start_and_plan_path_to_destination(_navigator->getRtlPlanningStart(), false); + + } else { + _navigator->set_start_and_plan_path_to_destination(matrix::Vector2d(_global_pos_sub.get().lat, _global_pos_sub.get().lon), true); + } } publishRemainingTimeEstimate(); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index aadf90c5168..8821baf3ff6 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -72,12 +72,11 @@ void RtlDirect::on_activation() { _global_pos_sub.update(); _vehicle_status_sub.update(); + const bool start_is_current_position = !_navigator->was_last_position_outside_fence(); + _num_waypoints_for_geofence_avoidance = + _navigator->set_start_and_plan_path_to_destination(_navigator->getRtlPlanningStart(), start_is_current_position); - _navigator->updateStartOfRTLPathPlanner(_navigator->getRtlPlanningStart()); - - // destination will only update if the location is different or the previous location was invalid - _navigator->updateDestinationOfRTLPathPlanner(matrix::Vector2d{_land_approach.lat, _land_approach.lon}); - _geofence_aware_return_path = _navigator->planPath(); + _current_geofence_avoidance_index = 0; parameters_update(); @@ -96,7 +95,7 @@ void RtlDirect::on_activation() (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); // send out message only for the first point - if (_geofence_aware_return_path.num_points > 0) { + if (_num_waypoints_for_geofence_avoidance > 0) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: avoiding geofence\t"); events::send(events::ID("rtl_avoiding_geofence"), events::Log::Info, "RTL: avoiding geofence"); } @@ -166,7 +165,7 @@ void RtlDirect::_updateRtlState() switch (_rtl_state) { case RTLState::CLIMBING: - if (_geofence_aware_return_path.hasNextPoint()) { + if (_current_geofence_avoidance_index < _num_waypoints_for_geofence_avoidance) { new_state = RTLState::AVOID_GEOFENCE; } else { @@ -176,7 +175,7 @@ void RtlDirect::_updateRtlState() break; case RTLState::AVOID_GEOFENCE: - if (_geofence_aware_return_path.hasNextPoint()) { + if (_current_geofence_avoidance_index < _num_waypoints_for_geofence_avoidance) { new_state = RTLState::AVOID_GEOFENCE; } else { @@ -264,7 +263,7 @@ void RtlDirect::set_rtl_item() case RTLState::AVOID_GEOFENCE: { - matrix::Vector2d point = _geofence_aware_return_path.getAndPopCurrentPoint(); + matrix::Vector2d point = _navigator->get_point_at_index(_current_geofence_avoidance_index++); if (point.isAllNan()) { // should never happen @@ -457,7 +456,7 @@ 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()) { + } else if (_current_geofence_avoidance_index < _num_waypoints_for_geofence_avoidance) { activation_state = RTLState::AVOID_GEOFENCE; } else { @@ -502,17 +501,32 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() // FALLTHROUGH case RTLState::AVOID_GEOFENCE: { - const int num_points = _geofence_aware_return_path.num_points; - for (int i = 0; i < num_points - 1; ++i) { - matrix::Vector2d start = _geofence_aware_return_path.getPoint(i); - matrix::Vector2d end = _geofence_aware_return_path.getPoint(i + 1); + // If the path was planned from a stored anchor (vehicle was outside the fence at plan time), + // the leg from the current position back to point 0 is unaccounted for in the path itself. + // Add it while the vehicle has not yet reached point 0 (index <= 1: not started, or heading there). + if (!_navigator->geofencePlannerStartIsCurrentPosition() && _current_geofence_avoidance_index <= 1) { + const matrix::Vector2d current_pos{_global_pos_sub.get().lat, _global_pos_sub.get().lon}; + const matrix::Vector2d first_waypoint = _navigator->get_point_at_index(0); + + matrix::Vector2f direction{}; + get_vector_to_next_waypoint(current_pos(0), current_pos(1), first_waypoint(0), first_waypoint(1), + &direction(0), &direction(1)); + const float dist = get_distance_to_next_waypoint(current_pos(0), current_pos(1), + first_waypoint(0), first_waypoint(1)); + + _rtl_time_estimator.addDistance(dist, direction, 0.f); + } + + // accumulate distances between the path waypoints, which have not been reached yet + for (int i = _current_geofence_avoidance_index; i < _num_waypoints_for_geofence_avoidance - 1; ++i) { + const matrix::Vector2d start = _navigator->get_point_at_index(i); + const matrix::Vector2d end = _navigator->get_point_at_index(i + 1); matrix::Vector2f direction{}; get_vector_to_next_waypoint(start(0), start(1), end(0), end(1), &direction(0), &direction(1)); - float dist{get_distance_to_next_waypoint(start(0), start(1), - end(0), end(1))}; + const float dist = get_distance_to_next_waypoint(start(0), start(1), end(0), end(1)); _rtl_time_estimator.addDistance(dist, direction, 0.f); } @@ -524,8 +538,8 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() float dist{0.f}; - if (_geofence_aware_return_path.num_points > 0) { - const matrix::Vector2d last_point = _geofence_aware_return_path.getPoint(_geofence_aware_return_path.num_points - 1); + if (_num_waypoints_for_geofence_avoidance > 0) { + const matrix::Vector2d last_point = _navigator->get_point_at_index(_num_waypoints_for_geofence_avoidance); get_vector_to_next_waypoint(last_point(0), last_point(1), land_approach.lat, land_approach.lon, &direction(0), &direction(1)); dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon); @@ -691,8 +705,3 @@ void RtlDirect::publish_rtl_direct_navigator_mission_item() _navigator_mission_item_pub.publish(navigator_mission_item); } - -void RtlDirect::updatePlannedPath() -{ - _geofence_aware_return_path = _navigator->planPath(); -} diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index fc28cf78fb6..2f1c2800f82 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -112,8 +112,6 @@ public: bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);}; - void updatePlannedPath(); - private: /** * @brief Return to launch state machine. @@ -172,9 +170,11 @@ 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 + int _num_waypoints_for_geofence_avoidance{0}; ///< number of waypoints to be used for geofence avoidance in RTL + int _current_geofence_avoidance_index{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_rtl_descend_alt, (ParamFloat) _param_rtl_land_delay,