mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
af80070ed9
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
64 lines
1.8 KiB
C++
64 lines
1.8 KiB
C++
#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;
|
|
};
|