Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AC_Avoidance_config.h"
       4             : 
       5             : #if AP_OAPATHPLANNER_ENABLED
       6             : 
       7             : #include <AP_Common/AP_Common.h>
       8             : #include <AP_Common/AP_ExpandingArray.h>
       9             : 
      10             : /*
      11             :  * Visibility graph used by Dijkstra's algorithm for path planning around fence, stay-out zones and moving obstacles
      12             :  */
      13             : class AP_OAVisGraph {
      14             : public:
      15             :     AP_OAVisGraph();
      16             : 
      17             :     CLASS_NO_COPY(AP_OAVisGraph);  /* Do not allow copies */
      18             : 
      19             :     // types of items held in graph
      20             :     enum OAType : uint8_t {
      21             :         OATYPE_SOURCE = 0,
      22             :         OATYPE_DESTINATION,
      23             :         OATYPE_INTERMEDIATE_POINT,
      24             :     };
      25             : 
      26             :     // support up to 255 items of each type
      27             :     typedef uint8_t oaid_num;
      28             : 
      29             :     // id for uniquely identifying objects held in visibility graphs and paths
      30             :     class OAItemID {
      31             :     public:
      32             :         OAType id_type;
      33             :         oaid_num id_num;
      34        1722 :         bool operator ==(const OAItemID &i) const { return ((id_type == i.id_type) && (id_num == i.id_num)); }
      35             :     };
      36             : 
      37             :     struct VisGraphItem {
      38             :         OAItemID id1;       // first item's id
      39             :         OAItemID id2;       // second item's id
      40             :         float distance_cm;  // distance between the items
      41             :     };
      42             : 
      43             :     // clear all elements from graph
      44           5 :     void clear() { _num_items = 0; }
      45             : 
      46             :     // get number of items in visibility graph table
      47         924 :     uint16_t num_items() const { return _num_items; }
      48             : 
      49             :     // add item to visiblity graph, returns true on success, false if graph is full
      50             :     bool add_item(const OAItemID &id1, const OAItemID &id2, float distance_cm);
      51             : 
      52             :     // allow accessing graph as an array, 0 indexed
      53             :     // Note: no protection against out-of-bounds accesses so use with num_items()
      54         832 :     const VisGraphItem& operator[](uint16_t i) const { return _items[i]; }
      55             : 
      56             : private:
      57             : 
      58             :     AP_ExpandingArray<VisGraphItem> _items;
      59             :     uint16_t _num_items;
      60             : };
      61             : 
      62             : #endif  // AP_OAPATHPLANNER_ENABLED