Path Optimization

include/pathing/environment.hpp

@@ -168,15 +168,22 @@ class Environment {
      *  - if horizonal, endpoints will always be left right, top down
      *
      * TODO - UNIT TESTS
-     *
+     * @param scan_radius the radius of search, lines will be 2 * scan_radius apart
+     * @param vertical whether or not to scan vertically
      * @return the endpoints on the airdrop zone
      */
     std::vector<XYZCoord> getAirdropEndpoints(int scan_radius, bool vertical) const;
 
     /**
-     * Returns waypoints
-     *
+     * Returns waypoints for airdrop coverage pathing
+     * - if vertical, waypoints will always be top down, left right
+     * - if horizonal, waypoints will always be left right, top down
      *
+     * @param scan_radius the radius of search, lines will be 2 * scan_radius apart
+     * @param one_way whether or not to return waypoints in one direction (if not, returns in
+     * alterating directions)
+     * @param vertical whether or not to scan vertically
+     * @return the waypoints on the airdrop zone
      */
     std::vector<RRTPoint> getAirdropWaypoints(int scan_radius, bool one_way = false,
                                               bool vertical = false) const;
@@ -220,11 +227,15 @@ class Environment {
      */
     std::vector<XYZCoord> findIntersections(const Polygon& polygon, const XYZCoord& rayStart,
                                             const XYZCoord& rayEnd, bool vertical) const;
-    
+
     /**
-    *
-    */
-    Polygon scale (double scale, const Polygon& source_polygon) const;
+     * Returns a new polygon that is scaled by a given factor
+     *
+     * @param scale the factor to scale the polygon by
+     * @param source_polygon the polygon to scale
+     * @return the scaled polygon
+     */
+    Polygon scale(double scale, const Polygon& source_polygon) const;
 
  private:
     const Polygon valid_region;            // boundary of the valid map

include/pathing/static.hpp

@@ -173,7 +173,9 @@ class RRT {
 class AirdropSearch {
  public:
     AirdropSearch(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone,
-                  std::vector<Polygon> obstacles = {});
+                  std::vector<Polygon> obstacles = {},
+                  AirdropSearchConfig config = {
+                      .optimize = false, .vertical = false, .one_way = false});
 
     /**
      * Generates a path of parallel lines to cover a given area
@@ -188,6 +190,7 @@ class AirdropSearch {
     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
+    const AirdropSearchConfig config;
 };
 
 std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state);

include/utilities/datatypes.hpp

@@ -103,4 +103,10 @@ struct RRTConfig {
                                      // RRT iteration
 };
 
+struct AirdropSearchConfig {
+    bool optimize; // whether to ignore the config below and run all ways.
+    bool vertical; // if true, will search in vertical lines
+    bool one_way; // if true, path returned will only be in 1 direction
+};
+
 #endif  // INCLUDE_UTILITIES_DATATYPES_HPP_

src/pathing/static.cpp

@@ -275,23 +275,100 @@ std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state) {
 }
 
 AirdropSearch::AirdropSearch(const RRTPoint &start, double scan_radius, Polygon bounds,
-                             Polygon airdrop_zone, std::vector<Polygon> obstacles)
+                             Polygon airdrop_zone, std::vector<Polygon> obstacles,
+                             AirdropSearchConfig config)
     : start(start),
       scan_radius(scan_radius),
       airspace(Environment(bounds, airdrop_zone, {}, obstacles)),
-      dubins(Dubins(TURNING_RADIUS, POINT_SEPARATION)) {}
+      dubins(Dubins(TURNING_RADIUS, POINT_SEPARATION)),
+      config(config) {}
 
 std::vector<XYZCoord> AirdropSearch::run() const {
+    if (!config.optimize) {
+        // generates the endpoints for the lines (including headings)
+        std::vector<RRTPoint> waypoints =
+            airspace.getAirdropWaypoints(scan_radius, config.one_way, config.vertical);
+
+        // generates the path connecting the q
+        std::vector<XYZCoord> path;
+        RRTPoint current = start;
+        for (const RRTPoint & 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;
+    }
+
+    // if optimizing, we store dubins options and then compare lengths
+
+    /*
+     * The order of paths
+     * [0] - alt, vertical
+     * [1] - alt, horizontal
+     * [2] - one_way, vertical
+     * [3] - one_way, horizontal
+     */
+
+    std::vector<std::pair<bool, bool>> configs = {
+        {false, true}, {false, false}, {true, true}, {true, false}};
+
+    std::vector<std::vector<RRTOption>> dubins_paths;
+
     // generates the endpoints for the lines (including headings)
-    std::vector<RRTPoint> waypoints = airspace.getAirdropWaypoints(scan_radius);
+    for (auto &config : configs) {
+        std::vector<RRTPoint> waypoints =
+            airspace.getAirdropWaypoints(scan_radius, config.first, config.second);
+
+        // generates the path connecting the q
+        std::vector<RRTOption> current_dubins_path;
+        RRTPoint current = start;
+        for (const RRTPoint& waypoint : waypoints) {
+            RRTOption dubins_path = dubins.bestOption(current, waypoint);
+            current_dubins_path.push_back(dubins_path);
+            current = waypoint;
+        }
+
+        dubins_paths.push_back(current_dubins_path);
+    }
+
+    // finds the shortest path
+    int shortest_path_index = 0;
+    double shortest_length = std::numeric_limits<double>::max();
+    for (int i = 0; i < dubins_paths.size(); i++) {
+        double length = 0;
+        for (auto &option : dubins_paths[i]) {
+            length += option.length;
+        }
 
-    // generates the path connecting the q
+        if (length < shortest_length) {
+            shortest_length = length;
+            shortest_path_index = i;
+        }
+    }
+
+    // actually makes the path
     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;
+
+    // gets the path
+    std::vector<RRTPoint> waypoints = airspace.getAirdropWaypoints(
+        scan_radius, configs[shortest_path_index].first, configs[shortest_path_index].second);
+
+    // initial path to the region is a special case, so we deal with it individually
+    std::vector<XYZCoord> init_path = dubins
+                       .generatePoints(current, waypoints[0],
+                                       dubins_paths[shortest_path_index][0].dubins_path,
+                                       dubins_paths[shortest_path_index][0].has_straight);
+    path.insert(path.end(), init_path.begin(), init_path.end());
+
+    // go through all the waypoints
+    for (int i = 0; i < dubins_paths[shortest_path_index].size() - 1; i++) {
+        std::vector<XYZCoord> path_coordinates = dubins.generatePoints(
+            waypoints[i], waypoints[i + 1], dubins_paths[shortest_path_index][i + 1].dubins_path,
+            dubins_paths[shortest_path_index][i + 1].has_straight);
+        path.insert(path.end(), path_coordinates.begin() + 1, path_coordinates.end());
     }
 
     return path;

tests/integration/airdrop_pathing.cpp

@@ -258,7 +258,7 @@ int main() {
     RRTPoint start = RRTPoint(state->config.getWaypoints()[0], 0);
 
     AirdropSearch search(start, 20, state->config.getFlightBoundary(),
-                         state->config.getAirdropBoundary());
+                         state->config.getAirdropBoundary(), {}, {true, true, true});
 
     std::vector<XYZCoord> path = search.run();
 

tests/integration/airdrop_pathing_2.cpp

@@ -12,8 +12,8 @@
 #include "network/gcs_routes.hpp"
 #include "pathing/plotting.hpp"
 #include "pathing/static.hpp"
-#include "ticks/mission_prep.hpp"
 #include "ticks/mav_upload.hpp"
+#include "ticks/mission_prep.hpp"
 #include "ticks/path_gen.hpp"
 #include "ticks/tick.hpp"
 #include "utilities/constants.hpp"
@@ -25,8 +25,6 @@
     httplib::Request REQ;                                                   \
     httplib::Response RESP
 
-
-
 const static char* mission_json_2024 = R"(
 {
     "BottleAssignments": [
@@ -254,17 +252,15 @@ int main() {
     AirdropSearch search(start, 9, state->config.getFlightBoundary(),
                          state->config.getAirdropBoundary());
 
-    Environment env(state->config.getFlightBoundary(), state->config.getAirdropBoundary(),
-                    {}, {});
+    Environment env(state->config.getFlightBoundary(), state->config.getAirdropBoundary(), {}, {});
 
     Polygon scaled = env.scale(0.75, state->config.getFlightBoundary());
 
     std::vector<XYZCoord> path = search.run();
 
     // plot the path
     std::cout << "Start Plotting" << std::endl;
-    PathingPlot plotter("pathing_output", state->config.getFlightBoundary(),
-                        scaled, {});
+    PathingPlot plotter("pathing_output", state->config.getFlightBoundary(), scaled, {});
 
     plotter.addFinalPolyline(path);
     plotter.output("test_airdrop_pathing_2", PathOutputType::STATIC);