aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_OAPathPlanner: path planning around obstacles

Browse Source 
implements both bendy ruler and dijkstra's algorithms

includes many changes after peer review including:
removed unused airspeed and turn-rate arguments
renamed constants to start with OA_BENDYRULER_
removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled
AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit
vehicle's heading is used instead of ground-course when speed is below 0.2m/s
static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly
replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX
use calc_margin_from_xxx function's return values
calc-margin-from-proximity-sensors fails if copy-locations fails
remove debug
remove unused singletons

and other changes including:
integrate fence rename to get_boundary_points
adapt to fence return point and duplicate final point from not being returned
create bendyrule and dijkstra objects on init
add pre-arm-check to ensure they've been created successfully
Dijkstra's detects polygon fence changes
minor fix to Dijkstra's return path
allow update_visgraph to accept 0,0 as extra point

and these fixes from peer reviews:
remove unused methods
add note about dangers of operator[]
reduce memory consuption of ShortPathNode
remove unused methods including print_all_nodes and print_shortest_path
replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX
update method gets check that ekf origin has been set
fix indexing bug in inner fence creation
fix whitespace error
find-node-from-id uses switch statement instead of 3 IF statements
update_visgraph comments added to clarify usage of extra_position argument
find-closest-node-idx always uses node_index instead of int16_t
static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255
some looping style changes
AP_OABendyRuler: fix to ground course calculation
AP_OAPathPlanner: pre_arm_check writes to caller's buffer
remove unnecessary hal and stdio.h includes
rename fence_ptr local variable to fence
add sanity check to OADijkstra's set_fence_margin
add sanity check to OABendyRuler's set_config
BendyRuler's test_bearing local constants changed to be float constants
BendyRuler's ahrs_home made local constant
BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead)
BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt
AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination
AP_OADijkstra: fixes after peer review
remove unnecesary initialisations
replace safe_sqrt(sq()) with length()
remove unnecessary block
comment fixes
AP_OAPathPlanner: add logging of final an intermediate location
This commit is contained in:
Randy Mackay 2019-05-10 14:59:31 +09:00
parent c7b1ad3443
commit af80070ed9
8 changed files with 1397 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

286
libraries/AC_Avoidance/AP_OABendyRuler.cpp Normal file
View File

@ -0,0 +1,286 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_OABendyRuler.h"
#include <AC_Fence/AC_Fence.h>
const int16_t OA_BENDYRULER_BEARING_INC = 5; // check every 5 degrees around vehicle
const float OA_BENDYRULER_LOOKAHEAD_STEP2_RATIO = 1.0f; // step2's lookahead length as a ratio of step1's lookahead length
const float OA_BENDYRULER_LOOKAHEAD_STEP2_MIN = 2.0f; // step2 checks at least this many meters past step1's location
const float OA_BENDYRULER_LOOKAHEAD_PAST_DEST = 2.0f; // lookahead length will be at least this many meters past the destination
const float OA_BENDYRULER_LOW_SPEED_SQUARED = (0.2f * 0.2f); // when ground course is below this speed squared, vehicle's heading will be used
// run background task to find best path and update avoidance_results
// returns true and updates origin_new and destination_new if a best path has been found
bool AP_OABendyRuler::update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new)
{
// bendy ruler always sets origin to current_loc
origin_new = current_loc;
// calculate bearing and distance to final destination
const float bearing_to_dest = current_loc.get_bearing_to(destination) * 0.01f;
const float distance_to_dest = current_loc.get_distance(destination);
// lookahead distance is adjusted dynamically based on avoidance results
_current_lookahead = constrain_float(_current_lookahead, _lookahead * 0.5f, _lookahead);
// calculate lookahead dist and time for step1. distance can be slightly longer than
// the distance to the destination to allow room to dodge after reaching the destination
const float lookahead_step1_dist = MIN(_current_lookahead, distance_to_dest + OA_BENDYRULER_LOOKAHEAD_PAST_DEST);
// calculate lookahead dist for step2
const float lookahead_step2_dist = _current_lookahead * OA_BENDYRULER_LOOKAHEAD_STEP2_RATIO;
// get ground course
float ground_course_deg;
if (ground_speed_vec.length_squared() < OA_BENDYRULER_LOW_SPEED_SQUARED) {
// with zero ground speed use vehicle's heading
ground_course_deg = AP::ahrs().yaw_sensor * 0.01f;
} else {
ground_course_deg = degrees(ground_speed_vec.angle());
}
// check OA_BEARING_INC definition allows checking in all directions
static_assert(360 % OA_BENDYRULER_BEARING_INC == 0, "check 360 is a multiple of OA_BEARING_INC");
// search in OA_BENDYRULER_BEARING_INC degree increments around the vehicle alternating left
// and right. For each direction check if vehicle would avoid all obstacles
float best_bearing = bearing_to_dest;
bool have_best_bearing = false;
float best_margin = -FLT_MAX;
float best_margin_bearing = best_bearing;
for (uint8_t i = 0; i <= (170 / OA_BENDYRULER_BEARING_INC); i++) {
for (uint8_t bdir = 0; bdir <= 1; bdir++) {
// skip duplicate check of bearing straight towards destination
if ((i==0) && (bdir > 0)) {
continue;
}
// bearing that we are probing
const float bearing_delta = i * OA_BENDYRULER_BEARING_INC * (bdir == 0 ? -1.0f : 1.0f);
const float bearing_test = wrap_180(bearing_to_dest + bearing_delta);
// ToDo: add effective groundspeed calculations using airspeed
// ToDo: add prediction of vehicle's position change as part of turn to desired heading
// test location is projected from current location at test bearing
Location test_loc = current_loc;
test_loc.offset_bearing(bearing_test, lookahead_step1_dist);
// calculate margin from fence for this scenario
float margin = calc_avoidance_margin(current_loc, test_loc);
if (margin > best_margin) {
best_margin_bearing = bearing_test;
best_margin = margin;
}
if (margin > _margin_max) {
// this bearing avoids obstacles out to the lookahead_step1_dist
// now check in there is a clear path in three directions towards the destination
if (!have_best_bearing) {
best_bearing = bearing_test;
have_best_bearing = true;
} else if (fabsf(wrap_180(ground_course_deg - bearing_test)) <
fabsf(wrap_180(ground_course_deg - best_bearing))) {
// replace bearing with one that is closer to our current ground course
best_bearing = bearing_test;
}
// perform second stage test in three directions looking for obstacles
const float test_bearings[] { 0.0f, 45.0f, -45.0f };
const float bearing_to_dest2 = test_loc.get_bearing_to(destination) * 0.01f;
float distance2 = constrain_float(lookahead_step2_dist, OA_BENDYRULER_LOOKAHEAD_STEP2_MIN, test_loc.get_distance(destination));
for (uint8_t j = 0; j < ARRAY_SIZE(test_bearings); j++) {
float bearing_test2 = wrap_180(bearing_to_dest2 + test_bearings[j]);
Location test_loc2 = test_loc;
test_loc2.offset_bearing(bearing_test2, distance2);
// calculate minimum margin to fence and obstacles for this scenario
float margin2 = calc_avoidance_margin(test_loc, test_loc2);
if (margin2 > _margin_max) {
// all good, now project in the chosen direction by the full distance
destination_new = current_loc;
destination_new.offset_bearing(bearing_test, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
// if the chosen direction is directly towards the destination turn off avoidance
return (i != 0 || j != 0);
}
}
}
}
}
float chosen_bearing;
if (have_best_bearing) {
// none of the directions tested were OK for 2-step checks. Choose the direction
// that was best for the first step
chosen_bearing = best_bearing;
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.05f);
} else {
// none of the possible paths had a positive margin. Choose
// the one with the highest margin
chosen_bearing = best_margin_bearing;
_current_lookahead = MAX(_lookahead * 0.5f, _current_lookahead * 0.9f);
}
// calculate new target based on best effort
destination_new = current_loc;
destination_new.offset_bearing(chosen_bearing, distance_to_dest);
return true;
}
// calculate minimum distance between a segment and any obstacle
float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end)
{
float circular_fence_margin;
if (!calc_margin_from_circular_fence(start, end, circular_fence_margin)) {
circular_fence_margin = FLT_MAX;
}
float polygon_fence_margin;
if (!calc_margin_from_polygon_fence(start, end, polygon_fence_margin)) {
polygon_fence_margin = FLT_MAX;
}
float proximity_margin;
if (!calc_margin_from_proximity_sensors(start, end, proximity_margin)) {
proximity_margin = FLT_MAX;
}
// return smallest margin from any obstacle
return MIN(MIN(circular_fence_margin, polygon_fence_margin), proximity_margin);
}
// calculate minimum distance between a path and the circular fence (centered on home)
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin)
{
// exit immediately if polygon fence is not enabled
const AC_Fence *fence = AC_Fence::get_singleton();
if (fence == nullptr) {
return false;
}
if ((fence->get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) == 0) {
return false;
}
// calculate start and end point's distance from home
const Location &ahrs_home = AP::ahrs().get_home();
const float start_dist_sq = ahrs_home.get_distance_NE(start).length_squared();
const float end_dist_sq = ahrs_home.get_distance_NE(end).length_squared();
// get circular fence radius
const float fence_radius = fence->get_radius();
// margin is fence radius minus the longer of start or end distance
margin = fence_radius - sqrtf(MAX(start_dist_sq, end_dist_sq));
return true;
}
// calculate minimum distance between a path and the polygon fence
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_polygon_fence(const Location &start, const Location &end, float &margin)
{
// exit immediately if polygon fence is not enabled
const AC_Fence *fence = AC_Fence::get_singleton();
if (fence == nullptr) {
return false;
}
if (((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) || !fence->is_polygon_valid()) {
return false;
}
// get polygon boundary
uint16_t num_points;
const Vector2f* boundary = fence->get_boundary_points(num_points);
if (num_points < 3) {
// this should have already been checked by is_polygon_valid() but just in case
return false;
}
// convert start and end to offsets from EKF origin
Vector2f start_NE, end_NE;
if (!start.get_vector_xy_from_origin_NE(start_NE) || !end.get_vector_xy_from_origin_NE(end_NE)) {
return false;
}
// calculate min distance (in meters) from line to polygon
margin = Polygon_closest_distance_line(boundary, num_points, start_NE, end_NE) * 0.01f;
return true;
}
// calculate minimum distance between a path and proximity sensor obstacles
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_proximity_sensors(const Location &start, const Location &end, float &margin)
{
AP_Proximity* prx = AP_Proximity::get_singleton();
if (prx == nullptr) {
return false;
}
// retrieve obstacles from proximity library
if (!prx->copy_locations(_prx_locs, ARRAY_SIZE(_prx_locs), _prx_loc_count)) {
return false;
}
// exit if no obstacles
if (_prx_loc_count == 0) {
return false;
}
// convert start and end to offsets (in cm) from EKF origin
Vector2f start_NE, end_NE;
if (!start.get_vector_xy_from_origin_NE(start_NE) || !end.get_vector_xy_from_origin_NE(end_NE)) {
return false;
}
// check each obstacle's distance from segment
bool valid_loc_found = false;
float smallest_margin = FLT_MAX;
uint32_t now = AP_HAL::millis();
for (uint16_t i=0; i<_prx_loc_count; i++) {
// check time of objects is still valid
if ((now - _prx_locs[i].last_update_ms) > PROXIMITY_LOCATION_TIMEOUT_MS) {
continue;
}
valid_loc_found = true;
// convert obstacle's location to offset (in cm) from EKF origin
Vector2f point;
if (!_prx_locs[i].loc.get_vector_xy_from_origin_NE(point)) {
continue;
}
// margin is distance between line segment and obstacle minus obstacle's radius
const float m = Vector2f::closest_distance_between_line_and_point(start_NE, end_NE, point) * 0.01f - _prx_locs[i].radius_m;
if (m < smallest_margin) {
smallest_margin = m;
}
}
// if no valid obstacles found then they've all expired so clear buffer
if (!valid_loc_found) {
_prx_loc_count = 0;
}
// return smallest margin
if (smallest_margin < FLT_MAX) {
margin = smallest_margin;
return true;
}
return false;
}

52
libraries/AC_Avoidance/AP_OABendyRuler.h Normal file
View File

@ -0,0 +1,52 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AP_HAL/AP_HAL.h>
/*
* BendyRuler avoidance algorithm for avoiding the polygon and circular fence and dynamic objects detected by the proximity sensor
*/
class AP_OABendyRuler {
public:
AP_OABendyRuler() {}
/* Do not allow copies */
AP_OABendyRuler(const AP_OABendyRuler &other) = delete;
AP_OABendyRuler &operator=(const AP_OABendyRuler&) = delete;
// send configuration info stored in front end parameters
void set_config(float lookahead, float margin_max) { _lookahead = MAX(lookahead, 1.0f); _margin_max = MAX(margin_max, 0.0f); }
// run background task to find best path and update avoidance_results
bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new);
private:
// calculate minimum distance between a path and any obstacle
float calc_avoidance_margin(const Location &start, const Location &end);
// calculate minimum distance between a path and the circular fence (centered on home)
// on success returns true and updates margin
bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin);
// calculate minimum distance between a path and the polygon fence
// on success returns true and updates margin
bool calc_margin_from_polygon_fence(const Location &start, const Location &end, float &margin);
// calculate minimum distance between a path and proximity sensor obstacles
// on success returns true and updates margin
bool calc_margin_from_proximity_sensors(const Location &start, const Location &end, float &margin);
// configuration parameters
float _lookahead; // object avoidance will look this many meters ahead of vehicle
float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle
// internal variables used by background thread
float _current_lookahead; // distance (in meters) ahead of the vehicle we are looking for obstacles
AP_Proximity::Proximity_Location _prx_locs[PROXIMITY_MAX_DIRECTION]; // buffer of locations from proximity library
uint16_t _prx_loc_count;
};

506
libraries/AC_Avoidance/AP_OADijkstra.cpp Normal file
View File

@ -0,0 +1,506 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_OADijkstra.h"
#include <AC_Fence/AC_Fence.h>
#define OA_DIJKSTRA_POLYGON_VISGRAPH_PTS (OA_DIJKSTRA_POLYGON_FENCE_PTS * OA_DIJKSTRA_POLYGON_FENCE_PTS / 2)
#define OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX 255 // index use to indicate we do not have a tentative short path for a node
/// Constructor
AP_OADijkstra::AP_OADijkstra() :
_polyfence_visgraph(OA_DIJKSTRA_POLYGON_VISGRAPH_PTS),
_source_visgraph(OA_DIJKSTRA_POLYGON_FENCE_PTS),
_destination_visgraph(OA_DIJKSTRA_POLYGON_FENCE_PTS)
{
}
// calculate a destination to avoid the polygon fence
// returns true if avoidance is required and updates origin_new and destination_new
bool AP_OADijkstra::update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new)
{
// require ekf origin to have been set
struct Location ekf_origin {};
if (!AP::ahrs().get_origin(ekf_origin)) {
return false;
}
// check for fence updates
if (check_polygon_fence_updated()) {
_polyfence_with_margin_ok = false;
}
// create inner polygon fence
if (!_polyfence_with_margin_ok) {
_polyfence_with_margin_ok = create_polygon_fence_with_margin(_polyfence_margin * 100.0f);
if (!_polyfence_with_margin_ok) {
_polyfence_visgraph_ok = false;
_shortest_path_ok = false;
return false;
}
}
// create visgraph for inner polygon fence
if (!_polyfence_visgraph_ok) {
_polyfence_visgraph_ok = create_polygon_fence_visgraph();
if (!_polyfence_visgraph_ok) {
_shortest_path_ok = false;
return false;
}
}
// rebuild path if destination has changed
if (!destination.same_latlon_as(_destination_prev)) {
_destination_prev = destination;
_shortest_path_ok = false;
}
// calculate shortest path from current_loc to destination
if (!_shortest_path_ok) {
_shortest_path_ok = calc_shortest_path(current_loc, destination);
if (!_shortest_path_ok) {
return false;
}
// start from 2nd point on path (first is the original origin)
_path_idx_returned = 1;
}
// path has been created, return latest point
Vector2f dest_pos;
if (get_shortest_path_point(_path_idx_returned, dest_pos)) {
// for the first point return origin as current_loc
Vector2f origin_pos;
if ((_path_idx_returned > 0) && get_shortest_path_point(_path_idx_returned-1, origin_pos)) {
// convert offset from ekf origin to Location
Location temp_loc(Vector3f(origin_pos.x, origin_pos.y, 0.0f));
origin_new = temp_loc;
} else {
// for first point use current loc as origin
origin_new = current_loc;
}
// convert offset from ekf origin to Location
Location temp_loc(Vector3f(dest_pos.x, dest_pos.y, 0.0f));
destination_new = destination;
destination_new.lat = temp_loc.lat;
destination_new.lng = temp_loc.lng;
// check if we should advance to next point for next iteration
const bool near_oa_wp = current_loc.get_distance(destination_new) <= 2.0f;
const bool past_oa_wp = current_loc.past_interval_finish_line(origin_new, destination_new);
if (near_oa_wp || past_oa_wp) {
_path_idx_returned++;
}
return true;
}
return false;
}
// check if polygon fence has been updated since we created the inner fence. returns true if changed
bool AP_OADijkstra::check_polygon_fence_updated() const
{
// exit immediately if polygon fence is not enabled
const AC_Fence *fence = AC_Fence::get_singleton();
if (fence == nullptr) {
return false;
}
return (_polyfence_update_ms != fence->get_boundary_update_ms());
}
// create a smaller polygon fence within the existing polygon fence
// returns true on success
bool AP_OADijkstra::create_polygon_fence_with_margin(float margin_cm)
{
// exit immediately if polygon fence is not enabled
const AC_Fence *fence = AC_Fence::get_singleton();
if (fence == nullptr) {
return false;
}
// get polygon boundary
uint16_t num_points;
const Vector2f* boundary = fence->get_boundary_points(num_points);
if ((boundary == nullptr) || (num_points < 3)) {
return false;
}
// fail if fence is too large
if (num_points > OA_DIJKSTRA_POLYGON_FENCE_PTS) {
return false;
}
// for each point on polygon fence
// Note: boundary is "unclosed" meaning the last point is *not* the same as the first
for (uint8_t i=0; i<num_points; i++) {
// find points before and after current point (relative to current point)
const uint8_t before_idx = (i == 0) ? num_points-1 : i-1;
const uint8_t after_idx = (i == num_points-1) ? 0 : i+1;
Vector2f before_pt = boundary[before_idx] - boundary[i];
Vector2f after_pt = boundary[after_idx] - boundary[i];
// if points are overlapping fail
if (before_pt.is_zero() || after_pt.is_zero() || (before_pt == after_pt)) {
return false;
}
// scale points to be unit vectors
before_pt.normalize();
after_pt.normalize();
// calculate intermediate point and scale to margin
Vector2f intermediate_pt = (after_pt + before_pt) * 0.5f;
float intermediate_len = intermediate_pt.length();
intermediate_pt *= (margin_cm / intermediate_len);
// find final point which is inside the original polygon
_polyfence_pts[i] = boundary[i] + intermediate_pt;
if (Polygon_outside(_polyfence_pts[i], boundary, num_points)) {
_polyfence_pts[i] = boundary[i] - intermediate_pt;
if (Polygon_outside(_polyfence_pts[i], boundary, num_points)) {
// could not find a point on either side that was within the fence so fail
// this can happen if fence lines are closer than margin_cm
return false;
}
}
}
// update number of fence points
_polyfence_numpoints = num_points;
// record fence update time so we don't process this exact fence again
_polyfence_update_ms = fence->get_boundary_update_ms();
return true;
}
// create a visibility graph of the polygon fence
// returns true on success
// requires create_polygon_fence_with_margin to have been run
bool AP_OADijkstra::create_polygon_fence_visgraph()
{
// exit immediately if no polygon fence (with margin)
if (_polyfence_numpoints == 0) {
return false;
}
// exit immediately if polygon fence is not enabled
const AC_Fence *fence = AC_Fence::get_singleton();
if (fence == nullptr) {
return false;
}
// get polygon boundary
uint16_t num_points;
const Vector2f* boundary = fence->get_boundary_points(num_points);
if ((boundary == nullptr) || (num_points < 3)) {
return false;
}
// clear polygon visibility graph
_polyfence_visgraph.clear();
// calculate distance from each polygon fence point to all other points
for (uint8_t i=0; i<_polyfence_numpoints-1; i++) {
const Vector2f &start1 = _polyfence_pts[i];
for (uint8_t j=i+1; j<_polyfence_numpoints-1; j++) {
const Vector2f &end1 = _polyfence_pts[j];
Vector2f intersection;
// ToDo: calculation below could be sped up by removing unused intersection and
// skipping segments we know are parallel to our fence-with-margin segments
if (!Polygon_intersects(boundary, num_points, start1, end1, intersection)) {
// line segment does not intersect with original fence so add to visgraph
_polyfence_visgraph.add_item({AP_OAVisGraph::OATYPE_FENCE_POINT, i},
{AP_OAVisGraph::OATYPE_FENCE_POINT, j},
(_polyfence_pts[i] - _polyfence_pts[j]).length());
}
// ToDo: store infinity when there is no clear path between points to allow faster search later
}
}
return true;
}
// updates visibility graph for a given position which is an offset (in cm) from the ekf origin
// to add an additional position (i.e. the destination) set add_extra_position = true and provide the position in the extra_position argument
// requires create_polygon_fence_with_margin to have been run
// returns true on success
bool AP_OADijkstra::update_visgraph(AP_OAVisGraph& visgraph, const AP_OAVisGraph::OAItemID& oaid, const Vector2f &position, bool add_extra_position, Vector2f extra_position)
{
// exit immediately if no polygon fence (with margin)
if (_polyfence_numpoints == 0) {
return false;
}
// exit immediately if polygon fence is not enabled
const AC_Fence *fence = AC_Fence::get_singleton();
if (fence == nullptr) {
return false;
}
// get polygon boundary
uint16_t num_points;
const Vector2f* boundary = fence->get_boundary_points(num_points);
if ((boundary == nullptr) || (num_points < 3)) {
return false;
}
// clear visibility graph
visgraph.clear();
// calculate distance from position to all fence points
for (uint8_t i=0; i<_polyfence_numpoints-1; i++) {
Vector2f intersection;
if (!Polygon_intersects(boundary, num_points, position, _polyfence_pts[i], intersection)) {
// line segment does not intersect with original fence so add to visgraph
visgraph.add_item(oaid, {AP_OAVisGraph::OATYPE_FENCE_POINT, i}, (position - _polyfence_pts[i]).length());
}
// ToDo: store infinity when there is no clear path between points to allow faster search later
}
// add extra point to visibility graph if it doesn't intersect with polygon fence
if (add_extra_position) {
Vector2f intersection;
if (!Polygon_intersects(boundary, num_points, position, extra_position, intersection)) {
visgraph.add_item(oaid, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, (position - extra_position).length());
}
}
return true;
}
// update total distance for all nodes visible from current node
// curr_node_idx is an index into the _short_path_data array
void AP_OADijkstra::update_visible_node_distances(node_index curr_node_idx)
{
// sanity check
if (curr_node_idx > _short_path_data_numpoints) {
return;
}
// get current node for convenience
const ShortPathNode &curr_node = _short_path_data[curr_node_idx];
// for each visibility graph
const AP_OAVisGraph* visgraphs[] = {&_polyfence_visgraph, &_destination_visgraph};
for (uint8_t v=0; v<ARRAY_SIZE(visgraphs); v++) {
// skip if empty
const AP_OAVisGraph &curr_visgraph = *visgraphs[v];
if (curr_visgraph.num_items() == 0) {
continue;
}
// search visibility graph for items visible from current_node
for (uint8_t i=0; i<curr_visgraph.num_items(); i++) {
const AP_OAVisGraph::VisGraphItem &item = curr_visgraph[i];
// match if current node's id matches either of the id's in the graph (i.e. either end of the vector)
if ((curr_node.id == item.id1) || (curr_node.id == item.id2)) {
AP_OAVisGraph::OAItemID matching_id = (curr_node.id == item.id1) ? item.id2 : item.id1;
// find item's id in node array
node_index item_node_idx;
if (find_node_from_id(matching_id, item_node_idx)) {
// if current node's distance + distance to item is less than item's current distance, update item's distance
const float dist_to_item_via_current_node = _short_path_data[curr_node_idx].distance_cm + item.distance_cm;
if (dist_to_item_via_current_node < _short_path_data[item_node_idx].distance_cm) {
// update item's distance and set "distance_from_idx" to current node's index
_short_path_data[item_node_idx].distance_cm = dist_to_item_via_current_node;
_short_path_data[item_node_idx].distance_from_idx = curr_node_idx;
}
}
}
}
}
}
// find a node's index into _short_path_data array from it's id (i.e. id type and id number)
// returns true if successful and node_idx is updated
bool AP_OADijkstra::find_node_from_id(const AP_OAVisGraph::OAItemID &id, node_index &node_idx) const
{
switch (id.id_type) {
case AP_OAVisGraph::OATYPE_SOURCE:
// source node is always the first node
if (_short_path_data_numpoints > 0) {
node_idx = 0;
return true;
}
break;
case AP_OAVisGraph::OATYPE_DESTINATION:
// destination is always the 2nd node
if (_short_path_data_numpoints > 1) {
node_idx = 1;
return true;
}
break;
case AP_OAVisGraph::OATYPE_FENCE_POINT:
// must be a fence node which start from 3rd node
if (_short_path_data_numpoints > id.id_num + 2) {
node_idx = id.id_num + 2;
return true;
}
break;
}
// could not find node
return false;
}
// find index of node with lowest tentative distance (ignore visited nodes)
// returns true if successful and node_idx argument is updated
bool AP_OADijkstra::find_closest_node_idx(node_index &node_idx) const
{
node_index lowest_idx = 0;
float lowest_dist = FLT_MAX;
// scan through all nodes looking for closest
for (node_index i=0; i<_short_path_data_numpoints; i++) {
const ShortPathNode &node = _short_path_data[i];
if (!node.visited && (node.distance_cm < lowest_dist)) {
lowest_idx = i;
lowest_dist = node.distance_cm;
}
}
if (lowest_dist < FLT_MAX) {
node_idx = lowest_idx;
return true;
}
return false;
}
// calculate shortest path from origin to destination
// returns true on success
// requires create_polygon_fence_with_margin and create_polygon_fence_visgraph to have been run
// resulting path is stored in _shortest_path array as vector offsets from EKF origin
bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &destination)
{
// convert origin and destination to offsets from EKF origin
Vector2f origin_NE, destination_NE;
if (!origin.get_vector_xy_from_origin_NE(origin_NE) || !destination.get_vector_xy_from_origin_NE(destination_NE)) {
return false;
}
// create origin and destination visgraphs of polygon points
update_visgraph(_source_visgraph, {AP_OAVisGraph::OATYPE_SOURCE, 0}, origin_NE, true, destination_NE);
update_visgraph(_destination_visgraph, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, destination_NE);
// check OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX is defined correct
static_assert(OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX > OA_DIJKSTRA_POLYGON_FENCE_PTS, "check OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX > OA_DIJKSTRA_POLYGON_FENCE_PTS");
// add origin and destination (node_type, id, visited, distance_from_idx, distance_cm) to short_path_data array
_short_path_data[0] = {{AP_OAVisGraph::OATYPE_SOURCE, 0}, false, 0, 0};
_short_path_data[1] = {{AP_OAVisGraph::OATYPE_DESTINATION, 0}, false, OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX, FLT_MAX};
_short_path_data_numpoints = 2;
// add fence points to short_path_data array (node_type, id, visited, distance_from_idx, distance_cm)
// skip last fence point because it is the same as the first
for (uint8_t i=0; i<_polyfence_numpoints-1; i++) {
_short_path_data[_short_path_data_numpoints++] = {{AP_OAVisGraph::OATYPE_FENCE_POINT, i}, false, OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX, FLT_MAX};
}
// start algorithm from source point
node_index current_node_idx = 0;
// update nodes visible from source point
for (uint8_t i=0; i<_source_visgraph.num_items(); i++) {
node_index node_idx;
if (find_node_from_id(_source_visgraph[i].id2, node_idx)) {
_short_path_data[node_idx].distance_cm = _source_visgraph[i].distance_cm;
_short_path_data[node_idx].distance_from_idx = current_node_idx;
} else {
return false;
}
}
// mark source node as visited
_short_path_data[current_node_idx].visited = true;
// move current_node_idx to node with lowest distance
while (find_closest_node_idx(current_node_idx)) {
// update distances to all neighbours of current node
update_visible_node_distances(current_node_idx);
// mark current node as visited
_short_path_data[current_node_idx].visited = true;
}
// extract path starting from destination
bool success = false;
node_index nidx;
if (!find_node_from_id({AP_OAVisGraph::OATYPE_DESTINATION,0}, nidx)) {
return false;
}
_path_numpoints = 0;
while (true) {
// fail if out of space or newest node has invalid distance or from index
if ((_path_numpoints >= OA_DIJKSTRA_POLYGON_SHORTPATH_PTS) ||
(_short_path_data[nidx].distance_from_idx == OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) ||
(_short_path_data[nidx].distance_cm >= FLT_MAX)) {
break;
} else {
// add node's id to path array
_path[_path_numpoints] = _short_path_data[nidx].id;
_path_numpoints++;
// we are done if node is the source
if (_short_path_data[nidx].id.id_type == AP_OAVisGraph::OATYPE_SOURCE) {
success = true;
break;
} else {
// follow node's "distance_from_idx" to previous node on path
nidx = _short_path_data[nidx].distance_from_idx;
}
}
}
// update source and destination for by get_shortest_path_point
if (success) {
_path_source = origin_NE;
_path_destination = destination_NE;
}
return success;
}
// return point from final path as an offset (in cm) from the ekf origin
bool AP_OADijkstra::get_shortest_path_point(uint8_t point_num, Vector2f& pos)
{
if ((_path_numpoints == 0) || (point_num >= _path_numpoints)) {
return false;
}
// get id from path
AP_OAVisGraph::OAItemID id = _path[_path_numpoints - point_num - 1];
// convert id to a position offset from EKF origin
switch (id.id_type) {
case AP_OAVisGraph::OATYPE_SOURCE:
pos = _path_source;
return true;
case AP_OAVisGraph::OATYPE_DESTINATION:
pos = _path_destination;
return true;
case AP_OAVisGraph::OATYPE_FENCE_POINT:
// sanity check polygon fence has the point
if (id.id_num < _polyfence_numpoints) {
pos = _polyfence_pts[id.id_num];
return true;
}
return false;
}
// we should never reach here but just in case
return false;
}

106
libraries/AC_Avoidance/AP_OADijkstra.h Normal file
View File

@ -0,0 +1,106 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_OAVisGraph.h"
#define OA_DIJKSTRA_POLYGON_FENCE_PTS 20 // algorithm can handle up to this many polygon fence points
#define OA_DIJKSTRA_POLYGON_SHORTPATH_PTS OA_DIJKSTRA_POLYGON_FENCE_PTS+1 // return path can have up to this many destinations
/*
* Dijkstra's algorithm for path planning around polygon fence
*/
class AP_OADijkstra {
public:
AP_OADijkstra();
/* Do not allow copies */
AP_OADijkstra(const AP_OADijkstra &other) = delete;
AP_OADijkstra &operator=(const AP_OADijkstra&) = delete;
// set fence margin (in meters) used when creating "safe positions" within the polygon fence
void set_fence_margin(float margin) { _polyfence_margin = MAX(margin, 0.0f); }
// calculate a destination to avoid the polygon fence
// returns true if avoidance is required and target destination is placed in result_loc
bool update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new);
private:
// check if polygon fence has been updated since we created the inner fence. returns true if changed
bool check_polygon_fence_updated() const;
// create a smaller polygon fence within the existing polygon fence
// returns true on success
bool create_polygon_fence_with_margin(float margin_cm);
// create a visibility graph of the polygon fence
// returns true on success
// requires create_polygon_fence_with_margin to have been run
bool create_polygon_fence_visgraph();
// calculate shortest path from origin to destination
// returns true on success
// requires create_polygon_fence_with_margin and create_polygon_fence_visgraph to have been run
// resulting path is stored in _shortest_path array as vector offsets from EKF origin
bool calc_shortest_path(const Location &origin, const Location &destination);
// shortest path state variables
bool _polyfence_with_margin_ok;
bool _polyfence_visgraph_ok;
bool _shortest_path_ok;
Location _destination_prev; // destination of previous iterations (used to determine if path should be re-calculated)
uint8_t _path_idx_returned; // index into _path array which gives location vehicle should be currently moving towards
// polygon fence (with margin) related variables
float _polyfence_margin = 10;
Vector2f _polyfence_pts[OA_DIJKSTRA_POLYGON_FENCE_PTS];
uint8_t _polyfence_numpoints;
uint32_t _polyfence_update_ms; // system time of boundary update from AC_Fence (used to detect changes to polygon fence)
// visibility graphs
AP_OAVisGraph _polyfence_visgraph;
AP_OAVisGraph _source_visgraph;
AP_OAVisGraph _destination_visgraph;
// updates visibility graph for a given position which is an offset (in cm) from the ekf origin
// to add an additional position (i.e. the destination) set add_extra_position = true and provide the position in the extra_position argument
// requires create_polygon_fence_with_margin to have been run
// returns true on success
bool update_visgraph(AP_OAVisGraph& visgraph, const AP_OAVisGraph::OAItemID& oaid, const Vector2f &position, bool add_extra_position = false, Vector2f extra_position = Vector2f(0,0));
typedef uint8_t node_index; // indices into short path data
struct ShortPathNode {
AP_OAVisGraph::OAItemID id; // unique id for node (combination of type and id number)
bool visited; // true if all this node's neighbour's distances have been updated
node_index distance_from_idx; // index into _short_path_data from where distance was updated (or 255 if not set)
float distance_cm; // distance from source (number is tentative until this node is the current node and/or visited = true)
} _short_path_data[OA_DIJKSTRA_POLYGON_SHORTPATH_PTS];
node_index _short_path_data_numpoints; // number of elements in _short_path_data array
// update total distance for all nodes visible from current node
// curr_node_idx is an index into the _short_path_data array
void update_visible_node_distances(node_index curr_node_idx);
// find a node's index into _short_path_data array from it's id (i.e. id type and id number)
// returns true if successful and node_idx is updated
bool find_node_from_id(const AP_OAVisGraph::OAItemID &id, node_index &node_idx) const;
// find index of node with lowest tentative distance (ignore visited nodes)
// returns true if successful and node_idx argument is updated
bool find_closest_node_idx(node_index &node_idx) const;
// final path variables and functions
AP_OAVisGraph::OAItemID _path[OA_DIJKSTRA_POLYGON_SHORTPATH_PTS]; // ids of points on return path in reverse order (i.e. destination is first element)
uint8_t _path_numpoints; // number of points on return path
Vector2f _path_source; // source point used in shortest path calculations (offset in cm from EKF origin)
Vector2f _path_destination; // destination position used in shortest path calculations (offset in cm from EKF origin)
// return point from final path as an offset (in cm) from the ekf origin
bool get_shortest_path_point(uint8_t point_num, Vector2f& pos);
};

240
libraries/AC_Avoidance/AP_OAPathPlanner.cpp Normal file
View File

@ -0,0 +1,240 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_OAPathPlanner.h"
#include <AP_Math/AP_Math.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AC_Fence/AC_Fence.h>
#include <AP_Logger/AP_Logger.h>
#include "AP_OABendyRuler.h"
#include "AP_OADijkstra.h"
extern const AP_HAL::HAL &hal;
// parameter defaults
const float OA_LOOKAHEAD_DEFAULT = 50;
const float OA_MARGIN_MAX_DEFAULT = 10;
const int16_t OA_TIMEOUT_MS = 2000; // avoidance results over 2 seconds old are ignored
const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = {
// @Param: TYPE
// @DisplayName: Object Avoidance Path Planning algorithm to use
// @Description: Enabled/disable path planning around obstacles
// @Values: 0:Disabled,1:BendyRuler,2:Dijkstra
// @User: Standard
AP_GROUPINFO_FLAGS("TYPE", 1, AP_OAPathPlanner, _type, OA_PATHPLAN_DISABLED, AP_PARAM_FLAG_ENABLE),
// @Param: LOOKAHEAD
// @DisplayName: Object Avoidance look ahead distance maximum
// @Description: Object Avoidance will look this many meters ahead of vehicle
// @Units: m
// @Range: 1 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("LOOKAHEAD", 2, AP_OAPathPlanner, _lookahead, OA_LOOKAHEAD_DEFAULT),
// @Param: MARGIN_MAX
// @DisplayName: Object Avoidance wide margin distance
// @Description: Object Avoidance will ignore objects more than this many meters from vehicle
// @Units: m
// @Range: 1 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("MARGIN_MAX", 3, AP_OAPathPlanner, _margin_max, OA_MARGIN_MAX_DEFAULT),
AP_GROUPEND
};
/// Constructor
AP_OAPathPlanner::AP_OAPathPlanner()
{
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
}
// perform any required initialisation
void AP_OAPathPlanner::init()
{
// run background task looking for best alternative destination
switch (_type) {
case OA_PATHPLAN_DISABLED:
// do nothing
break;
case OA_PATHPLAN_BENDYRULER:
if (_oabendyruler == nullptr) {
_oabendyruler = new AP_OABendyRuler();
}
break;
case OA_PATHPLAN_DIJKSTRA:
if (_oadijkstra == nullptr) {
_oadijkstra = new AP_OADijkstra();
}
break;
}
}
// pre-arm checks that algorithms have been initialised successfully
bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
// check if initialisation has succeeded
switch (_type) {
case OA_PATHPLAN_DISABLED:
// do nothing
break;
case OA_PATHPLAN_BENDYRULER:
if (_oabendyruler == nullptr) {
hal.util->snprintf(failure_msg, failure_msg_len, "BendyRuler OA requires reboot");
return false;
}
break;
case OA_PATHPLAN_DIJKSTRA:
if (_oadijkstra == nullptr) {
hal.util->snprintf(failure_msg, failure_msg_len, "Dijkstra OA requires reboot");
return false;
}
break;
}
return true;
}
// provides an alternative target location if path planning around obstacles is required
// returns true and updates result_loc with an intermediate location
bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
const Location &origin,
const Location &destination,
Location &result_origin,
Location &result_destination)
{
// exit immediately if disabled
if (_type == OA_PATHPLAN_DISABLED) {
return false;
}
WITH_SEMAPHORE(_rsem);
if (!_thread_created) {
// create the avoidance thread as low priority. It should soak
// up spare CPU cycles to fill in the avoidance_result structure based
// on requests in avoidance_request
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OAPathPlanner::avoidance_thread, void),
"avoidance",
8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
return false;
}
_thread_created = true;
}
const uint32_t now = AP_HAL::millis();
// place new request for the thread to work on
avoidance_request.current_loc = current_loc;
avoidance_request.origin = origin;
avoidance_request.destination = destination;
avoidance_request.ground_speed_vec = AP::ahrs().groundspeed_vector();
avoidance_request.request_time_ms = now;
// return results from background thread's latest checks
if (destination.lat == avoidance_result.destination.lat &&
destination.lng == avoidance_result.destination.lng &&
now - avoidance_result.result_time_ms < OA_TIMEOUT_MS) {
// we have a result from the thread
result_origin = avoidance_result.origin_new;
result_destination = avoidance_result.destination_new;
// log result
if (avoidance_result.result_time_ms != _logged_time_ms) {
_logged_time_ms = avoidance_result.result_time_ms;
AP::logger().Write_OA(_type, destination, result_destination);
}
return avoidance_result.avoidance_needed;
}
// do not performance avoidance because background thread's results were
// run against a different destination or they are simply not required
return false;
}
// avoidance thread that continually updates the avoidance_result structure based on avoidance_request
void AP_OAPathPlanner::avoidance_thread()
{
while (true) {
// run at 10hz or less
hal.scheduler->delay(100);
Location origin_new;
Location destination_new;
{
WITH_SEMAPHORE(_rsem);
uint32_t now = AP_HAL::millis();
if (now - avoidance_request.request_time_ms > OA_TIMEOUT_MS) {
// this is a very old request, don't process it
continue;
}
// copy request to avoid conflict with main thread
avoidance_request2 = avoidance_request;
// store passed in origin and destination so we can return it if object avoidance is not required
origin_new = avoidance_request.origin;
destination_new = avoidance_request.destination;
}
// run background task looking for best alternative destination
bool res = false;
switch (_type) {
case OA_PATHPLAN_DISABLED:
continue;
case OA_PATHPLAN_BENDYRULER:
if (_oabendyruler == nullptr) {
continue;
}
_oabendyruler->set_config(_lookahead, _margin_max);
res = _oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new);
break;
case OA_PATHPLAN_DIJKSTRA:
if (_oadijkstra == nullptr) {
continue;
}
_oadijkstra->set_fence_margin(_margin_max);
res = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new);
break;
}
{
// give the main thread the avoidance result
WITH_SEMAPHORE(_rsem);
avoidance_result.destination = avoidance_request2.destination;
avoidance_result.origin_new = res ? origin_new : avoidance_result.origin_new;
avoidance_result.destination_new = res ? destination_new : avoidance_result.destination;
avoidance_result.result_time_ms = AP_HAL::millis();
avoidance_result.avoidance_needed = res;
}
}
}
// singleton instance
AP_OAPathPlanner *AP_OAPathPlanner::_singleton;
namespace AP {
AP_OAPathPlanner *ap_oapathplanner()
{
return AP_OAPathPlanner::get_singleton();
}
}

90
libraries/AC_Avoidance/AP_OAPathPlanner.h Normal file
View File

@ -0,0 +1,90 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Param/AP_Param.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_OABendyRuler.h"
#include "AP_OADijkstra.h"
/*
* This class provides path planning around fence, stay-out zones and moving obstacles
*/
class AP_OAPathPlanner {
public:
AP_OAPathPlanner();
/* Do not allow copies */
AP_OAPathPlanner(const AP_OAPathPlanner &other) = delete;
AP_OAPathPlanner &operator=(const AP_OAPathPlanner&) = delete;
// get singleton instance
static AP_OAPathPlanner *get_singleton() {
return _singleton;
}
// perform any required initialisation
void init();
/// returns true if all pre-takeoff checks have completed successfully
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
// provides an alternative target location if path planning around obstacles is required
// returns true and updates result_origin and result_destination with an intermediate path
bool mission_avoidance(const Location &current_loc,
const Location &origin,
const Location &destination,
Location &result_origin,
Location &result_destination) WARN_IF_UNUSED;
// enumerations for _TYPE parameter
enum OAPathPlanTypes {
OA_PATHPLAN_DISABLED = 0,
OA_PATHPLAN_BENDYRULER = 1,
OA_PATHPLAN_DIJKSTRA = 2
};
static const struct AP_Param::GroupInfo var_info[];
private:
// avoidance thread that continually updates the avoidance_result structure based on avoidance_request
void avoidance_thread();
// an avoidance request from the navigation code
struct avoidance_info {
Location current_loc;
Location origin;
Location destination;
Vector2f ground_speed_vec;
uint32_t request_time_ms;
} avoidance_request, avoidance_request2;
// an avoidance result from the avoidance thread
struct {
Location destination; // destination vehicle is trying to get to (also used to verify the result matches a recent request)
Location origin_new; // intermediate origin. The start of line segment that vehicle should follow
Location destination_new; // intermediate destination vehicle should move towards
uint32_t result_time_ms; // system time the result was calculated (used to verify the result is recent)
bool avoidance_needed; // true if the vehicle should move along the path from origin_new to destination_new
} avoidance_result;
// parameters
AP_Int8 _type; // avoidance algorith to be used
AP_Float _lookahead; // object avoidance will look this many meters ahead of vehicle
AP_Float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle
// internal variables used by front end
HAL_Semaphore_Recursive _rsem; // semaphore for multi-thread use of avoidance_request and avoidance_result
bool _thread_created; // true once background thread has been created
AP_OABendyRuler *_oabendyruler; // Bendy Ruler algorithm
AP_OADijkstra *_oadijkstra; // Dijkstra's algorithm
uint32_t _logged_time_ms; // result_time_ms of last result logged (triggers logging of new results)
static AP_OAPathPlanner *_singleton;
};
namespace AP {
AP_OAPathPlanner *ap_oapathplanner();
};

54
libraries/AC_Avoidance/AP_OAVisGraph.cpp Normal file
View File

@ -0,0 +1,54 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_OAVisGraph.h"
// constructor with size argument
AP_OAVisGraph::AP_OAVisGraph(uint8_t size)
{
init(size);
}
// initialise array to given size
bool AP_OAVisGraph::init(uint8_t size)
{
// only allow init once
if (_items != nullptr) {
return false;
}
// allocate array
_items = (VisGraphItem *)calloc(size, sizeof(VisGraphItem));
if (_items != nullptr) {
_num_items_max = size;
return true;
}
return false;
}
// add item to visiblity graph, returns true on success, false if graph is full
bool AP_OAVisGraph::add_item(const OAItemID &id1, const OAItemID &id2, float distance_cm)
{
// check there is space
if (_num_items >= _num_items_max) {
return false;
}
// add item
_items[_num_items] = {id1, id2, distance_cm};
_num_items++;
return true;
}

63
libraries/AC_Avoidance/AP_OAVisGraph.h Normal file
View File

@ -0,0 +1,63 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
/*
* Visibility graph used by Dijkstra's algorithm for path planning around fence, stay-out zones and moving obstacles
*/
class AP_OAVisGraph {
public:
AP_OAVisGraph();
AP_OAVisGraph(uint8_t size);
/* Do not allow copies */
AP_OAVisGraph(const AP_OAVisGraph &other) = delete;
AP_OAVisGraph &operator=(const AP_OAVisGraph&) = delete;
// types of items held in graph
enum OAType : uint8_t {
OATYPE_SOURCE = 0,
OATYPE_DESTINATION,
OATYPE_FENCE_POINT
};
// support up to 255 items of each type
typedef uint8_t oaid_num;
// id for uniquely identifying objects held in visibility graphs and paths
class OAItemID {
public:
OAType id_type;
oaid_num id_num;
bool operator ==(const OAItemID &i) const { return ((id_type == i.id_type) && (id_num == i.id_num)); }
};
struct VisGraphItem {
OAItemID id1; // first item's id
OAItemID id2; // second item's id
float distance_cm; // distance between the items
};
// initialise array to given size
bool init(uint8_t size);
// clear all elements from graph
void clear() { _num_items = 0; }
// get number of items in visibility graph table
uint8_t num_items() const { return _num_items; }
// add item to visiblity graph, returns true on success, false if graph is full
bool add_item(const OAItemID &id1, const OAItemID &id2, float distance_cm);
// allow accessing graph as an array, 0 indexed
// Note: no protection against out-of-bounds accesses so use with num_items()
const VisGraphItem& operator[](uint8_t i) const { return _items[i]; }
private:
VisGraphItem *_items;
uint8_t _num_items_max;
uint8_t _num_items;
};