Feat/pathing airdrop (#142)

* Infrastructure + Naive Search Method (not bounds safe)

* Add integration test, change Environment to reflect new information

* Unit Testing (not finished, but 'should' work), refactor header-->source

* fix broken include because of file rename

---------

Co-authored-by: Tyler Lentz <[email protected]>

include/pathing/environment.hpp

@@ -21,8 +21,8 @@
  */
 class Environment {
  public:
-    Environment(const Polygon& valid_region, const std::vector<XYZCoord>& goals,
-                const std::vector<Polygon>& obstacles);
+    Environment(const Polygon& valid_region, const Polygon& airdrop_zone,
+                const std::vector<XYZCoord>& goals, const std::vector<Polygon>& obstacles);
 
     /**
      * Check if a point is in the valid region
@@ -123,16 +123,6 @@ class Environment {
      */
     bool isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) const;
 
-    /**
-     * Find the bounds of the valid region (i.e. the max/min x and y values).
-     *
-     * ASSUMES valid_region has already been created
-     *
-     * @return a pair of pairs, where the first pair is min/max x values and the second is the
-     * min/max y values
-     */
-    std::pair<std::pair<double, double>, std::pair<double, double>> findBounds() const;
-
     /**
      * Determines whether a line segment intersects the polygon
      *
@@ -172,8 +162,44 @@ class Environment {
      */
     bool intersect(XYZCoord p1, XYZCoord q1, XYZCoord p2, XYZCoord q2) const;
 
+    /**
+     * Returns endpoints (of vertical lines) on airdrop_zone for coverage pathing
+     * 
+     * TODO - UNIT TESTS
+     *
+     * @return the endpoints on the airdrop zone
+     */
+    std::vector<RRTPoint> getAirdropEndpoints(int scan_radius) const;
+
+    /**
+     * Fills an intersection if one exists between an edge of the polygon and the VERTICAL ray
+     *
+     * @param p1 the first point of the edge
+     * @param p2 the second point of the edge
+     * @param rayStart the start of the ray
+     * @param rayEnd the end of the ray
+     * @param intersection the intersection point (WILL BE FILLED IF INTERSECTION EXISTS)
+     * @return true if an intersection exists, false otherwise
+     */
+    bool rayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, const XYZCoord& rayStart,
+                           const XYZCoord& rayEnd, XYZCoord& intersection) const;
+
+    /**
+     * Finds all intersections between a VERTICAL ray and a polygon, and returns them as a lit
+     * 
+     * TODO - UNIT TESTS
+     *
+     * @param polygon the polygon to check intersections
+     * @param rayStart the start of the ray
+     * @param rayEnd the end of the ray
+     * @return a list of intersections
+     */
+    std::vector<XYZCoord> findIntersections(const Polygon& polygon, const XYZCoord& rayStart,
+                                            const XYZCoord& rayEnd) const;
+
  private:
     const Polygon valid_region;            // boundary of the valid map
+    const Polygon airdrop_zone;            // boundary of the airdrop zone (subset of valid_region)
     const std::vector<XYZCoord> goals;     // goal point
     const std::vector<Polygon> obstacles;  // obstacles in the map
 
@@ -186,6 +212,17 @@ class Environment {
                  // is (min x, max x),
                  // second pair is
                  // (min y, max y)
+
+    /**
+     * Find the bounds of the valid region (i.e. the max/min x and y values).
+     *
+     * ASSUMES valid_region has already been created
+     *
+     * @return a pair of pairs, where the first pair is min/max x values and the second is the
+     * min/max y values
+     */
+    std::pair<std::pair<double, double>, std::pair<double, double>> findBounds(
+        const Polygon& bounds) const;
 };
 
 #endif  // INCLUDE_PATHING_ENVIRONMENT_HPP_

include/pathing/static.hpp

@@ -161,6 +161,35 @@ class RRT {
     void optimizeTree(RRTNode *sample);
 };
 
+/**
+ * Class that performs Coverage-Path_Planning (CPP) over a given polygon
+ *
+ * Basically draws vertical lines, and the connects them with Dubins paths
+ *
+ * Limitations
+ * - Cannot path through non-convex shapes
+ * - Does not check if path is inbounds or not
+ */
+class AirdropSearch {
+ public:
+    AirdropSearch(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone,
+                  std::vector<Polygon> obstacles = {});
+
+    /**
+     * Generates a path of parallel lines to cover a given area
+     *
+     * @return  ==> list of 2-vectors describing the path through the aridrop_zone
+     */
+    std::vector<XYZCoord> run() const;
+
+ private:
+    const double scan_radius;    // how far each side of the plane we intend to look (half dist
+                                 // between search lines)
+    const RRTPoint start;        // start location (doesn't have to be near polygon)
+    const Environment airspace;  // information aobut the airspace
+    const Dubins dubins;         // dubins object to generate paths
+};
+
 std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state);
 
 #endif  // INCLUDE_PATHING_STATIC_HPP_

include/utilities/constants.hpp

@@ -15,24 +15,26 @@ const double TWO_PI = 2 * M_PI;
 const double HALF_PI = M_PI / 2;
 
 // FROM OBC PYTHON
-const double TURNING_RADIUS = 30;
-const double POINT_SEPARATION = 10;
+const double TURNING_RADIUS = 30.0;
+const double POINT_SEPARATION = 10.0;
 
 // RRT CONSTANTS
-const int ITERATIONS_PER_WAYPOINT = 200;
-const double SEARCH_RADIUS = 1000;
-const double REWIRE_RADIUS = 200;
+const int ITERATIONS_PER_WAYPOINT = 200;  // number of times RRT is ran per waypoint
+const double SEARCH_RADIUS =
+    1000.0;  // DOES NOTHING, limits how far off the tree the new node can be
+const double REWIRE_RADIUS = 200.0;  // ONLY FOR RRT-STAR, max radius from new node to rewire
 
 // RRT HELPER CONSTANTS
-const double EPOCH_TEST_MARGIN = 0.97;
-const int ENV_PATH_VALIDATION_STEP_SIZE = 5;
-const int K_RANDOM_NODES = 100;
-const int K_CLOESEST_NODES = 50;
+const double EPOCH_TEST_MARGIN = 0.97;        // at what margin of improvement does it stop
+const int ENV_PATH_VALIDATION_STEP_SIZE = 5;  // how many points to skip when validating path
+const int NUM_EPOCHS = 5;                     // number of times to evaulate the path length
+const int K_RANDOM_NODES = 100;               // how many nodes to generate for the tree
+const int K_CLOESEST_NODES = 50;  // how many nodes to look at when finding the closest node
 const int TOTAL_OPTIONS_FOR_GOAL_CONNECTION =
     2048;  // TODO - MUST SCALE WITH ITERATIONS OR ELSE CANT FIND GOAL
 
-const int TRIES_FOR_RANDOM_POINT = 64;  // for generating random points
-const int MAX_DUBINS_OPTIONS_TO_PARSE = 16;
+const int TRIES_FOR_RANDOM_POINT = 64;       // for generating random points
+const int MAX_DUBINS_OPTIONS_TO_PARSE = 16;  // how many routes to check when connecting two nodes
 
 const int DEFAULT_GCS_PORT = 5010;
 

src/pathing/environment.cpp

@@ -10,12 +10,13 @@
 #include "utilities/datatypes.hpp"
 #include "utilities/rng.hpp"
 
-Environment::Environment(const Polygon& valid_region, const std::vector<XYZCoord>& goals,
-                         const std::vector<Polygon>& obstacles)
+Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone,
+                         const std::vector<XYZCoord>& goals, const std::vector<Polygon>& obstacles)
     : valid_region(valid_region),
+      airdrop_zone(airdrop_zone),
       goals(goals),
       goals_found(0),
-      bounds(findBounds()),
+      bounds(findBounds(valid_region)),
       obstacles(obstacles) {}
 
 bool Environment::isPointInBounds(const XYZCoord& point) const {
@@ -165,26 +166,6 @@ bool Environment::isLineInBounds(const XYZCoord& start_point, const XYZCoord& en
     return true;
 }
 
-std::pair<std::pair<double, double>, std::pair<double, double>> Environment::findBounds() const {
-    if (valid_region.empty()) {
-        return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0));
-    }
-
-    double min_x = valid_region[0].x;
-    double max_x = valid_region[0].x;
-    double min_y = valid_region[0].y;
-    double max_y = valid_region[0].y;
-
-    for (const XYZCoord& point : valid_region) {
-        min_x = std::min(min_x, point.x);
-        max_x = std::max(max_x, point.x);
-        min_y = std::min(min_y, point.y);
-        max_y = std::max(max_y, point.y);
-    }
-
-    return {{min_x, max_x}, {min_y, max_y}};
-}
-
 bool Environment::doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point,
                                            const Polygon& polygon) const {
     for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) {
@@ -243,3 +224,90 @@ bool Environment::intersect(XYZCoord p1, XYZCoord q1, XYZCoord p2, XYZCoord q2)
 
     return false;  // Doesn't fall in any of the above cases
 }
+
+std::vector<RRTPoint> Environment::getAirdropEndpoints(int scan_radius) const {
+    auto bounds = findBounds(airdrop_zone);
+    auto [x_min, x_max] = bounds.first;
+    auto [y_min, y_max] = bounds.second;
+
+    std::vector<RRTPoint> endpoints;
+    bool fly_down = true;
+    for (double x = x_min + scan_radius; x <= x_max - scan_radius; x += scan_radius * 2) {
+        // finds where this x-coordinate intersects with the airdrop zone (always convex)
+        const XYZCoord top(x, y_max, 0);
+        const XYZCoord bottom(x, y_min, 0);
+        std::vector<XYZCoord> intersections = findIntersections(airdrop_zone, top, bottom);
+
+        // adjacent lines should be in different directions
+        double angle = fly_down ? 3 * M_PI / 2 : HALF_PI;
+        RRTPoint top_vector(intersections[0], angle);
+        RRTPoint bottom_vector(intersections[1], angle);
+
+        if (fly_down) {
+            endpoints.push_back(top_vector);
+            endpoints.push_back(bottom_vector);
+        } else {
+            endpoints.push_back(bottom_vector);
+            endpoints.push_back(top_vector);
+        }
+
+        fly_down = !fly_down;
+    }
+
+    return endpoints;
+}
+
+bool Environment::rayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2,
+                                    const XYZCoord& rayStart, const XYZCoord& rayEnd,
+                                    XYZCoord& intersection) const {
+    // if the x coordinate lines between the edge
+    if ((p2.x <= rayStart.x && p1.x >= rayStart.x) || (p1.x <= rayStart.x && p2.x >= rayStart.x)) {
+        double slope = (p2.y - p1.y) / (p2.x - p1.x);
+        // finds where they intersect using a line y - y' = m(x - x')
+        intersection = XYZCoord(rayStart.x, slope * (rayStart.x - p1.x) + p1.y, 0);
+        return true;
+    }
+    return false;
+}
+
+std::vector<XYZCoord> Environment::findIntersections(const Polygon& polygon,
+                                                     const XYZCoord& rayStart,
+                                                     const XYZCoord& rayEnd) const {
+    // array to be filled
+    std::vector<XYZCoord> intersections;
+    int n = polygon.size();
+
+    for (int i = 0; i < n; ++i) {
+        const XYZCoord& p1 = polygon[i];
+        const XYZCoord& p2 = polygon[(i + 1) % n];
+
+        // temporary variable to be changed if an intersection is found
+        XYZCoord intersection(0, 0, 0);
+        if (rayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection)) {
+            intersections.push_back(intersection);
+        }
+    }
+
+    return intersections;
+}
+
+std::pair<std::pair<double, double>, std::pair<double, double>> Environment::findBounds(
+    const Polygon& region) const {
+    if (region.empty()) {
+        return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0));
+    }
+
+    double min_x = region[0].x;
+    double max_x = region[0].x;
+    double min_y = region[0].y;
+    double max_y = region[0].y;
+
+    for (const XYZCoord& point : region) {
+        min_x = std::min(min_x, point.x);
+        max_x = std::max(max_x, point.x);
+        min_y = std::min(min_y, point.y);
+        max_y = std::max(max_y, point.y);
+    }
+
+    return {{min_x, max_x}, {min_y, max_y}};
+}

src/pathing/static.cpp

@@ -24,7 +24,8 @@ RRT::RRT(RRTPoint start, std::vector<XYZCoord> goals, int iterations_per_waypoin
     : iterations_per_waypoint(iterations_per_waypoint),
       search_radius(search_radius),
       rewire_radius(rewire_radius),
-      tree(start, Environment(bounds, goals, obstacles), Dubins(TURNING_RADIUS, POINT_SEPARATION)),
+      tree(start, Environment(bounds, {}, goals, obstacles),
+           Dubins(TURNING_RADIUS, POINT_SEPARATION)),
       config(config) {}
 
 void RRT::run() {
@@ -54,7 +55,7 @@ void RRT::run() {
 std::vector<XYZCoord> RRT::getPointsToGoal() const { return tree.getPathToGoal(); }
 
 bool RRT::RRTIteration(int tries, int current_goal_index) {
-    const int epoch_interval = tries / 5;
+    const int epoch_interval = tries / NUM_EPOCHS;
     int current_epoch = epoch_interval;
 
     RRTNode *goal_node = nullptr;
@@ -244,8 +245,7 @@ std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state) {
     // the other waypoitns is the goals
     if (state->config.getWaypoints().size() < 2) {
         loguru::set_thread_name("Static Pathing");
-        LOG_F(ERROR,
-              "Not enough waypoints to generate a path, required 2+, existing waypoints: %s",
+        LOG_F(ERROR, "Not enough waypoints to generate a path, required 2+, existing waypoints: %s",
               std::to_string(state->config.getWaypoints().size()).c_str());
         return {};
     }
@@ -273,3 +273,26 @@ std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state) {
 
     return output_coords;
 }
+
+AirdropSearch::AirdropSearch(const RRTPoint &start, double scan_radius, Polygon bounds,
+                             Polygon airdrop_zone, std::vector<Polygon> obstacles)
+    : start(start),
+      scan_radius(scan_radius),
+      airspace(Environment(bounds, airdrop_zone, {}, obstacles)),
+      dubins(Dubins(std::min(scan_radius, TURNING_RADIUS), POINT_SEPARATION)) {}
+
+std::vector<XYZCoord> AirdropSearch::run() const {
+    // generates the endpoints for the lines (including headings)
+    std::vector<RRTPoint> waypoints = airspace.getAirdropEndpoints(scan_radius);
+
+    // generates the path connecting the q
+    std::vector<XYZCoord> path;
+    RRTPoint current = start;
+    for (auto &waypoint : waypoints) {
+        std::vector<XYZCoord> dubins_path = dubins.dubinsPath(current, waypoint);
+        path.insert(path.end(), dubins_path.begin() + 1, dubins_path.end());
+        current = waypoint;
+    }
+
+    return path;
+}

tests/integration/CMakeLists.txt

@@ -146,6 +146,22 @@ target_add_loguru(path_planning)
 target_include_directories(path_planning PRIVATE ${ImageMagick_INCLUDE_DIRS})
 target_link_libraries(path_planning PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES})
 
+add_executable(airdrop_pathing "airdrop_pathing.cpp")
+target_link_libraries(airdrop_pathing PRIVATE obcpp_lib)
+target_include_directories(airdrop_pathing PRIVATE ${INCLUDE_DIRECTORY})
+target_add_protobuf(airdrop_pathing)
+target_add_torch(airdrop_pathing)
+target_add_torchvision(airdrop_pathing)
+target_add_json(airdrop_pathing)
+target_add_opencv(airdrop_pathing)
+target_add_httplib(airdrop_pathing)
+target_add_mavsdk(airdrop_pathing)
+target_add_matplot(airdrop_pathing)
+target_add_loguru(airdrop_pathing)
+
+target_include_directories(airdrop_pathing PRIVATE ${ImageMagick_INCLUDE_DIRS})
+target_link_libraries(airdrop_pathing PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES})
+
 # airdrop_sockets
 # add_executable(airdrop_sockets src/network/airdrop_sockets.c tests/integration/airdrop_sockets.c)
 # target_include_directories(airdrop_sockets PRIVATE ${INCLUDE_DIRECTORY})