Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AC_Avoidance_config.h"
       4             : 
       5             : #if AP_OAPATHPLANNER_DIJKSTRA_ENABLED
       6             : 
       7             : #include <AP_Common/AP_Common.h>
       8             : #include <AP_Common/Location.h>
       9             : #include <AP_Math/AP_Math.h>
      10             : #include "AP_OAVisGraph.h"
      11             : #include <AP_Logger/AP_Logger_config.h>
      12             : 
      13             : /*
      14             :  * Dijkstra's algorithm for path planning around polygon fence
      15             :  */
      16             : 
      17             : class AP_OADijkstra {
      18             : public:
      19             : 
      20             :     AP_OADijkstra(AP_Int16 &options);
      21             : 
      22             :     CLASS_NO_COPY(AP_OADijkstra);  /* Do not allow copies */
      23             : 
      24             :     // set fence margin (in meters) used when creating "safe positions" within the polygon fence
      25         270 :     void set_fence_margin(float margin) { _polyfence_margin = MAX(margin, 0.0f); }
      26             : 
      27             :     // trigger Dijkstra's to recalculate shortest path based on current location 
      28           0 :     void recalculate_path() { _shortest_path_ok = false; }
      29             : 
      30             :     // update return status enum
      31             :     enum AP_OADijkstra_State : uint8_t {
      32             :         DIJKSTRA_STATE_NOT_REQUIRED = 0,
      33             :         DIJKSTRA_STATE_ERROR,
      34             :         DIJKSTRA_STATE_SUCCESS
      35             :     };
      36             : 
      37             :     // calculate a destination to avoid the polygon fence
      38             :     // returns DIJKSTRA_STATE_SUCCESS and populates origin_new, destination_new and next_destination_new if avoidance is required
      39             :     // next_destination_new will be non-zero if there is a next destination
      40             :     // dest_to_next_dest_clear will be set to true if the path from (the input) destination to (input) next_destination is clear
      41             :     AP_OADijkstra_State update(const Location &current_loc,
      42             :                                const Location &destination,
      43             :                                const Location &next_destination,
      44             :                                Location& origin_new,
      45             :                                Location& destination_new,
      46             :                                Location& next_destination_new,
      47             :                                bool& dest_to_next_dest_clear);
      48             : 
      49             : private:
      50             : 
      51             :     // returns true if at least one inclusion or exclusion zone is enabled
      52             :     bool some_fences_enabled() const;
      53             : 
      54             :     enum class AP_OADijkstra_Error : uint8_t {
      55             :         DIJKSTRA_ERROR_NONE = 0,
      56             :         DIJKSTRA_ERROR_OUT_OF_MEMORY,
      57             :         DIJKSTRA_ERROR_OVERLAPPING_POLYGON_POINTS,
      58             :         DIJKSTRA_ERROR_FAILED_TO_BUILD_INNER_POLYGON,
      59             :         DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES,
      60             :         DIJKSTRA_ERROR_FENCE_DISABLED,
      61             :         DIJKSTRA_ERROR_TOO_MANY_FENCE_POINTS,
      62             :         DIJKSTRA_ERROR_NO_POSITION_ESTIMATE,
      63             :         DIJKSTRA_ERROR_COULD_NOT_FIND_PATH
      64             :     } _error_id;
      65             : 
      66             :     // return error message for a given error id
      67             :     const char* get_error_msg(AP_OADijkstra_Error error_id) const;
      68             : 
      69             :     // report error to ground station
      70             :     void report_error(AP_OADijkstra_Error error_id);
      71             : 
      72             :     //
      73             :     // inclusion polygon methods
      74             :     //
      75             : 
      76             :     // check if inclusion polygons have been updated since create_inclusion_polygon_with_margin was run
      77             :     // returns true if changed
      78             :     bool check_inclusion_polygon_updated() const;
      79             : 
      80             :     // create polygons inside the existing inclusion polygons
      81             :     // returns true on success.  returns false on failure and err_id is updated
      82             :     bool create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id);
      83             : 
      84             :     //
      85             :     // exclusion polygon methods
      86             :     //
      87             : 
      88             :     // check if exclusion polygons have been updated since create_exclusion_polygon_with_margin was run
      89             :     // returns true if changed
      90             :     bool check_exclusion_polygon_updated() const;
      91             : 
      92             :     // create polygons around existing exclusion polygons
      93             :     // returns true on success.  returns false on failure and err_id is updated
      94             :     bool create_exclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id);
      95             : 
      96             :     //
      97             :     // exclusion circle methods
      98             :     //
      99             : 
     100             :     // check if exclusion circles have been updated since create_exclusion_circle_with_margin was run
     101             :     // returns true if changed
     102             :     bool check_exclusion_circle_updated() const;
     103             : 
     104             :     // create polygons around existing exclusion circles
     105             :     // returns true on success.  returns false on failure and err_id is updated
     106             :     bool create_exclusion_circle_with_margin(float margin_cm, AP_OADijkstra_Error &err_id);
     107             : 
     108             :     //
     109             :     // other methods
     110             :     //
     111             : 
     112             :     // returns total number of points across all fence types
     113             :     uint16_t total_numpoints() const;
     114             : 
     115             :     // get a single point across the total list of points from all fence types
     116             :     // also returns the type of point
     117             :     bool get_point(uint16_t index, Vector2f& point) const;
     118             : 
     119             :     // returns true if line segment intersects polygon or circular fence
     120             :     bool intersects_fence(const Vector2f &seg_start, const Vector2f &seg_end) const;
     121             : 
     122             :     // create visibility graph for all fence (with margin) points
     123             :     // returns true on success.  returns false on failure and err_id is updated
     124             :     bool create_fence_visgraph(AP_OADijkstra_Error &err_id);
     125             : 
     126             :     // calculate shortest path from origin to destination
     127             :     // returns true on success.  returns false on failure and err_id is updated
     128             :     // requires create_polygon_fence_with_margin and create_polygon_fence_visgraph to have been run
     129             :     // resulting path is stored in _shortest_path array as vector offsets from EKF origin
     130             :     bool calc_shortest_path(const Location &origin, const Location &destination, AP_OADijkstra_Error &err_id);
     131             : 
     132             :     // shortest path state variables
     133             :     bool _inclusion_polygon_with_margin_ok;
     134             :     bool _exclusion_polygon_with_margin_ok;
     135             :     bool _exclusion_circle_with_margin_ok;
     136             :     bool _polyfence_visgraph_ok;
     137             :     bool _shortest_path_ok;
     138             : 
     139             :     Location _destination_prev;     // destination of previous iterations (used to determine if path should be re-calculated)
     140             :     Location _next_destination_prev;// next_destination of previous iterations (used to determine if path should be re-calculated)
     141             :     uint8_t _path_idx_returned;     // index into _path array which gives location vehicle should be currently moving towards
     142             :     bool _dest_to_next_dest_clear;  // true if path from dest to next_dest is clear (i.e. does not intersects a fence)
     143             : 
     144             :     // inclusion polygon (with margin) related variables
     145             :     float _polyfence_margin = 10;           // margin around polygon defaults to 10m but is overriden with set_fence_margin
     146             :     AP_ExpandingArray<Vector2f> _inclusion_polygon_pts; // array of nodes corresponding to inclusion polygon points plus a margin
     147             :     uint8_t _inclusion_polygon_numpoints;   // number of points held in above array
     148             :     uint32_t _inclusion_polygon_update_ms;  // system time of boundary update from AC_Fence (used to detect changes to polygon fence)
     149             : 
     150             :     // exclusion polygon related variables
     151             :     AP_ExpandingArray<Vector2f> _exclusion_polygon_pts; // array of nodes corresponding to exclusion polygon points plus a margin
     152             :     uint8_t _exclusion_polygon_numpoints;   // number of points held in above array
     153             :     uint32_t _exclusion_polygon_update_ms;  // system time exclusion polygon was updated (used to detect changes)
     154             : 
     155             :     // exclusion circle related variables
     156             :     AP_ExpandingArray<Vector2f> _exclusion_circle_pts; // array of nodes surrounding exclusion circles plus a margin
     157             :     uint8_t _exclusion_circle_numpoints;    // number of points held in above array
     158             :     uint32_t _exclusion_circle_update_ms;   // system time exclusion circles were updated (used to detect changes)
     159             : 
     160             :     // visibility graphs
     161             :     AP_OAVisGraph _fence_visgraph;          // holds distances between all inclusion/exclusion fence points (with margin)
     162             :     AP_OAVisGraph _source_visgraph;         // holds distances from source point to all other nodes
     163             :     AP_OAVisGraph _destination_visgraph;    // holds distances from the destination to all other nodes
     164             : 
     165             :     // updates visibility graph for a given position which is an offset (in cm) from the ekf origin
     166             :     // to add an additional position (i.e. the destination) set add_extra_position = true and provide the position in the extra_position argument
     167             :     // requires create_polygon_fence_with_margin to have been run
     168             :     // returns true on success
     169             :     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));
     170             : 
     171             :     typedef uint8_t node_index;         // indices into short path data
     172             :     struct ShortPathNode {
     173             :         AP_OAVisGraph::OAItemID id;     // unique id for node (combination of type and id number)
     174             :         bool visited;                   // true if all this node's neighbour's distances have been updated
     175             :         node_index distance_from_idx;   // index into _short_path_data from where distance was updated (or 255 if not set)
     176             :         float distance_cm;              // distance from source (number is tentative until this node is the current node and/or visited = true)
     177             :     };
     178             :     AP_ExpandingArray<ShortPathNode> _short_path_data;
     179             :     node_index _short_path_data_numpoints;  // number of elements in _short_path_data array
     180             : 
     181             :     // update total distance for all nodes visible from current node
     182             :     // curr_node_idx is an index into the _short_path_data array
     183             :     void update_visible_node_distances(node_index curr_node_idx);
     184             : 
     185             :     // find a node's index into _short_path_data array from it's id (i.e. id type and id number)
     186             :     // returns true if successful and node_idx is updated
     187             :     bool find_node_from_id(const AP_OAVisGraph::OAItemID &id, node_index &node_idx) const;
     188             : 
     189             :     // find index of node with lowest tentative distance (ignore visited nodes)
     190             :     // returns true if successful and node_idx argument is updated
     191             :     bool find_closest_node_idx(node_index &node_idx) const;
     192             : 
     193             :     // final path variables and functions
     194             :     AP_ExpandingArray<AP_OAVisGraph::OAItemID> _path;   // ids of points on return path in reverse order (i.e. destination is first element)
     195             :     uint8_t _path_numpoints;                            // number of points on return path
     196             :     Vector2f _path_source;                              // source point used in shortest path calculations (offset in cm from EKF origin)
     197             :     Vector2f _path_destination;                         // destination position used in shortest path calculations (offset in cm from EKF origin)
     198             : 
     199             :     // return number of points on path
     200         767 :     uint8_t get_shortest_path_numpoints() const { return _path_numpoints; }
     201             : 
     202             :     // return point from final path as an offset (in cm) from the ekf origin
     203             :     bool get_shortest_path_point(uint8_t point_num, Vector2f& pos) const;
     204             : 
     205             :     // find the position of a node as an offset (in cm) from the ekf origin
     206             :     // returns true if successful and pos is updated
     207             :     bool convert_node_to_point(const AP_OAVisGraph::OAItemID& id, Vector2f& pos) const;
     208             : 
     209             :     AP_OADijkstra_Error _error_last_id;                 // last error id sent to GCS
     210             :     uint32_t _error_last_report_ms;                     // last time an error message was sent to GCS
     211             : 
     212             : #if HAL_LOGGING_ENABLED
     213             :     // Logging functions
     214             :     void Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const;
     215             :     void Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const;
     216             : #else
     217             :     void Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const {}
     218             :     void Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const {}
     219             : #endif
     220             :     uint8_t _log_num_points;
     221             :     uint8_t _log_visgraph_version;
     222             : 
     223             :     // reference to AP_OAPathPlanner options param
     224             :     AP_Int16 &_options;
     225             : };
     226             : 
     227             : #endif  // AP_OAPATHPLANNER_DIJKSTRA_ENABLED