Merge branch 'main' of github.com:tritonuas/obcpp

configs/config.json

@@ -23,6 +23,7 @@
             "allowed_to_skip_waypoints": false
         },
         "coverage": {
+            "coverage_altitude_m": 30.0,
             "optimize": true,
             "vertical": false,
             "one_way": false

include/pathing/static.hpp

@@ -143,14 +143,14 @@ class RRT {
                        int total_options = TOTAL_OPTIONS_FOR_GOAL_CONNECTION);
 
     /**
-     * Does the logistical work when found one waypoint to another 
+     * Does the logistical work when found one waypoint to another
      *  - adds the node to the tree
      *  - finds the path
      *      - adds altitude to the path
-     * 
+     *
      * @param goal_node  ==> node to add to the tree
      * @param current_goal_index ==> index of the goal that we are trying to
-    */
+     */
     void addNodeToTree(RRTNode *goal_node, int current_goal_index);
 
     /**
@@ -188,7 +188,10 @@ class AirdropSearch {
     AirdropSearch(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone,
                   std::vector<Polygon> obstacles = {},
                   AirdropSearchConfig config = {
-                      .optimize = false, .vertical = false, .one_way = false});
+                      .coverage_altitude_m = 30.0,
+                      .optimize = false,
+                      .vertical = false,
+                      .one_way = false});
 
     /**
      * Generates a path of parallel lines to cover a given area
@@ -200,6 +203,26 @@ class AirdropSearch {
      */
     std::vector<XYZCoord> run() const;
 
+    /**
+     *   The algorithm run if not optimizing the path legnth
+     */
+    std::vector<XYZCoord> coverageDefault() const;
+
+    /**
+     *   The algorithm run if optimizing path length
+     */
+    std::vector<XYZCoord> coverageOptimal() const;
+
+    /**
+     * From a list of dubins paths and waypoints, generate a path
+     *
+     * @param dubins_options  ==> list of dubins options to connect the waypoints
+     * @param waypoints       ==> list of waypoints to connect (always 1 more element than
+     * dubins_options)
+     */
+    std::vector<XYZCoord> generatePath(const std::vector<RRTOption> &dubins_options,
+                                       const std::vector<RRTPoint> &waypoints) const;
+
  private:
     const double scan_radius;    // how far each side of the plane we intend to look (half dist
                                  // between search lines)

include/utilities/constants.hpp

@@ -42,6 +42,7 @@ const int NUM_AIRDROP_BOTTLES = 5;
 
 const char MISSION_CONFIG_PATH[] = "./mission-config.json";
 const double TAKEOFF_ALTITUDE_M = 30.0;
+const double COVERAGE_ALTITUDE_M = 30.0;
 
 const matplot::color FLIGHT_BOUND_COLOR = matplot::color::red;
 const matplot::color AIRDROP_BOUND_COLOR = matplot::color::magenta;

include/utilities/datatypes.hpp

@@ -107,6 +107,7 @@ struct RRTConfig {
 };
 
 struct AirdropSearchConfig {
+    double coverage_altitude_m;
     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

src/pathing/static.cpp

@@ -274,25 +274,25 @@ AirdropSearch::AirdropSearch(const RRTPoint &start, double scan_radius, Polygon
       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 config.optimize ? coverageOptimal() : coverageDefault();
+}
 
-        return path;
+std::vector<XYZCoord> AirdropSearch::coverageDefault() const {
+    // generates the endpoints for the lines (including headings)
+    std::vector<RRTPoint> waypoints =
+        airspace.getAirdropWaypoints(scan_radius, config.one_way, config.vertical);
+    waypoints.emplace(waypoints.begin(), start);
+
+    // generates the path connecting the q
+    std::vector<RRTOption> dubins_options;
+    for (int i = 0; i < waypoints.size() - 1; i++) {
+        dubins_options.push_back(dubins.bestOption(waypoints[i], waypoints[i + 1]));
     }
 
-    // if optimizing, we store dubins options and then compare lengths
+    return generatePath(dubins_options, waypoints);
+}
 
+std::vector<XYZCoord> AirdropSearch::coverageOptimal() const {
     /*
      * The order of paths
      * [0] - alt, vertical
@@ -316,46 +316,63 @@ std::vector<XYZCoord> AirdropSearch::run() const {
 
         // generates the path connecting the waypoints to each other
         std::vector<RRTOption> current_dubins_path;
-        RRTPoint current = start;
-        for (const RRTPoint &waypoint : waypoints) {
-            RRTOption dubins_path = dubins.bestOption(current, waypoint);
+
+        for (int i = 0; i < waypoints.size() - 1; i++) {
+            RRTOption dubins_path = dubins.bestOption(waypoints[i], waypoints[i + 1]);
             lengths[i] += dubins_path.length;
             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;
+    int best_path_idx = 0;
     double shortest_length = lengths[0];
     for (int i = 1; i < lengths.size(); i++) {
         if (lengths[i] < shortest_length) {
             shortest_length = lengths[i];
-            shortest_path_index = i;
+            best_path_idx = i;
         }
     }
 
-    // actually makes the path
-    std::vector<XYZCoord> path;
-    RRTPoint current = start;
-
     // 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);
+        scan_radius, configs[best_path_idx].first, configs[best_path_idx].second);
+
+    waypoints.emplace(waypoints.begin(), start);
+
+    return generatePath(dubins_paths[best_path_idx], waypoints);
+}
+
+std::vector<XYZCoord> AirdropSearch::generatePath(const std::vector<RRTOption> &dubins_options,
+                                                  const std::vector<RRTPoint> &waypoints) const {
+    std::vector<XYZCoord> path;
+
+    double height = waypoints[0].coord.z;
+    double height_difference = config.coverage_altitude_m - waypoints[0].coord.z;
+
+    std::vector<XYZCoord> path_coordinates = dubins.generatePoints(
+        waypoints[0], waypoints[1], dubins_options[0].dubins_path, dubins_options[0].has_straight);
+
+    double height_increment = height_difference / path_coordinates.size();
+
+    for (XYZCoord &coord : path_coordinates) {
+        coord.z = height;
+        height += height_increment;
+    }
+
+    path.insert(path.end(), path_coordinates.begin() + 1, path_coordinates.end());
+
+    for (int i = 1; i < dubins_options.size(); i++) {
+        path_coordinates =
+            dubins.generatePoints(waypoints[i], waypoints[i + 1], dubins_options[i].dubins_path,
+                                  dubins_options[i].has_straight);
+
+        for (XYZCoord &coord : path_coordinates) {
+            coord.z = config.coverage_altitude_m;
+        }
+
         path.insert(path.end(), path_coordinates.begin() + 1, path_coordinates.end());
     }
 

src/utilities/obc_config.cpp

@@ -31,6 +31,8 @@ OBCConfig::OBCConfig(int argc, char* argv[]) {
         this->rrt_config.allowed_to_skip_waypoints =
             configs["pathing"]["rrt"]["allowed_to_skip_waypoints"];
 
+        this->coverage_pathing_config.coverage_altitude_m =
+            configs["pathing"]["coverage"]["coverage_altitude_m"];
         this->coverage_pathing_config.optimize = configs["pathing"]["coverage"]["optimize"];
         this->coverage_pathing_config.vertical = configs["pathing"]["coverage"]["vertical"];
         std::cout << configs["pathing"]["coverage"]["one_way"] << std::endl;
@@ -96,6 +98,7 @@ void OBCConfig::makeDefault() {
     this->rrt_config.point_fetch_method = NEAREST;
     this->rrt_config.allowed_to_skip_waypoints = false;
 
+    this->coverage_pathing_config.coverage_altitude_m = COVERAGE_ALTITUDE_M;
     this->coverage_pathing_config.optimize = true;
     this->coverage_pathing_config.vertical = false;
     this->coverage_pathing_config.one_way = false;

tests/integration/CMakeLists.txt

@@ -176,37 +176,37 @@ target_add_loguru(pathing_integration)
 target_include_directories(pathing_integration PRIVATE ${ImageMagick_INCLUDE_DIRS})
 target_link_libraries(pathing_integration 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)
+add_executable(coverage_pathing "coverage_pathing.cpp")
+target_link_libraries(coverage_pathing PRIVATE obcpp_lib)
+target_include_directories(coverage_pathing PRIVATE ${INCLUDE_DIRECTORY})
+target_add_protobuf(coverage_pathing)
+target_add_torch(coverage_pathing)
+target_add_torchvision(coverage_pathing)
+target_add_json(coverage_pathing)
+target_add_opencv(coverage_pathing)
+target_add_httplib(coverage_pathing)
+target_add_mavsdk(coverage_pathing)
+target_add_matplot(coverage_pathing)
+target_add_loguru(coverage_pathing)
 
-target_include_directories(airdrop_pathing PRIVATE ${ImageMagick_INCLUDE_DIRS})
-target_link_libraries(airdrop_pathing PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES})
+target_include_directories(coverage_pathing PRIVATE ${ImageMagick_INCLUDE_DIRS})
+target_link_libraries(coverage_pathing PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES})
 
-add_executable(airdrop_pathing_2 "airdrop_pathing_2.cpp")
-target_link_libraries(airdrop_pathing_2 PRIVATE obcpp_lib)
-target_include_directories(airdrop_pathing_2 PRIVATE ${INCLUDE_DIRECTORY})
-target_add_protobuf(airdrop_pathing_2)
-target_add_torch(airdrop_pathing_2)
-target_add_torchvision(airdrop_pathing_2)
-target_add_json(airdrop_pathing_2)
-target_add_opencv(airdrop_pathing_2)
-target_add_httplib(airdrop_pathing_2)
-target_add_mavsdk(airdrop_pathing_2)
-target_add_matplot(airdrop_pathing_2)
-target_add_loguru(airdrop_pathing_2)
+add_executable(coverage_pathing_2 "coverage_pathing_2.cpp")
+target_link_libraries(coverage_pathing_2 PRIVATE obcpp_lib)
+target_include_directories(coverage_pathing_2 PRIVATE ${INCLUDE_DIRECTORY})
+target_add_protobuf(coverage_pathing_2)
+target_add_torch(coverage_pathing_2)
+target_add_torchvision(coverage_pathing_2)
+target_add_json(coverage_pathing_2)
+target_add_opencv(coverage_pathing_2)
+target_add_httplib(coverage_pathing_2)
+target_add_mavsdk(coverage_pathing_2)
+target_add_matplot(coverage_pathing_2)
+target_add_loguru(coverage_pathing_2)
 
-target_include_directories(airdrop_pathing_2 PRIVATE ${ImageMagick_INCLUDE_DIRS})
-target_link_libraries(airdrop_pathing_2 PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES})
+target_include_directories(coverage_pathing_2 PRIVATE ${ImageMagick_INCLUDE_DIRS})
+target_link_libraries(coverage_pathing_2 PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES})
 
 add_executable(airdrop_packets "airdrop_packets.cpp")
 target_link_libraries(airdrop_packets PRIVATE obcpp_lib)

tests/integration/airdrop_pathing.cpp → tests/integration/coverage_pathing.cpp

@@ -235,9 +235,9 @@ const static char* mission_json_2020 = R"(
  * FILE OUTPUT LOCATIONS
  *  |- build
  *    |- pathing_output
- *      |- test_airdrop_pathing.jpg
- *      |- test_airdrop_pathing.gif (if enabled)
- *    |- airdop_search_coords.txt
+ *      |- test_coverage_pathing.jpg
+ *      |- test_coverage_pathing.gif (if enabled)
+ *    |- coverage_coords.txt
  *
  *  This rough integration test is to test the airdrop search pathing algorithm
  */
@@ -253,7 +253,7 @@ int main() {
 
     // files to put path_coordinates to
     std::ofstream file;
-    file.open("airdop_search_coords.txt");
+    file.open("coverage_coords.txt");
 
     RRTPoint start = RRTPoint(state->mission_params.getWaypoints()[0], 0);
 
@@ -268,7 +268,7 @@ int main() {
                         state->mission_params.getAirdropBoundary(), {});
 
     plotter.addFinalPolyline(path);
-    plotter.output("test_airdrop_pathing", PathOutputType::STATIC);
+    plotter.output("test_coverage_pathing", PathOutputType::STATIC);
     file.close();
     return 0;
 }

tests/integration/airdrop_pathing_2.cpp → tests/integration/coverage_pathing_2.cpp

@@ -227,14 +227,15 @@ const static char* mission_json_2024 = R"(
  * FILE OUTPUT LOCATIONS
  *  |-- build
  *      |-- pathing_output
- *          |-- test_airdrop_pathing.jpg
- *          |-- test_airdrop_pathing.gif (if enabled)
- *      |-- airdop_search_coords.txt
+ *          |-- test_coverage_pathing.jpg
+ *          |-- test_coverage_pathing.gif (if enabled)
+ *      |-- coverage_coords_2.txt
  *
  *  This rough integration test is to test the airdrop search pathing algorithm
  */
 int main() {
     std::cout << "Messing with Airdrop Zone Search Pathing Part 2" << std::endl;
+
     // First upload a mission so that we generate a path
     // this is roughly the mission from 2020
     DECLARE_HANDLER_PARAMS(state, req, resp);
@@ -245,25 +246,33 @@ int main() {
 
     // files to put path_coordinates to
     std::ofstream file;
-    file.open("airdop_search_coords_2.txt");
+    file.open("coverage_coords_2.txt");
 
     RRTPoint start = RRTPoint(state->mission_params.getWaypoints()[0], 0);
+    start.coord.z = 100;
 
-    AirdropSearch search(start, 9, state->mission_params.getFlightBoundary(),
-                         state->mission_params.getAirdropBoundary());
+    AirdropSearch search(
+        start, 9, state->mission_params.getFlightBoundary(),
+        state->mission_params.getAirdropBoundary(), {},
+        AirdropSearchConfig{.coverage_altitude_m = 30.0, .optimize = false, .vertical = false, .one_way = false});
 
-    Environment env(state->mission_params.getFlightBoundary(), state->mission_params.getAirdropBoundary(), {}, {});
+    Environment env(state->mission_params.getFlightBoundary(),
+                    state->mission_params.getAirdropBoundary(), {}, {});
 
     Polygon scaled = env.scale(0.75, state->mission_params.getFlightBoundary());
 
     std::vector<XYZCoord> path = search.run();
 
+    for (const XYZCoord& coord : path) {
+        file << coord.z << std::endl;
+    }
+
     // plot the path
     std::cout << "Start Plotting" << std::endl;
     PathingPlot plotter("pathing_output", state->mission_params.getFlightBoundary(), scaled, {});
 
     plotter.addFinalPolyline(path);
-    plotter.output("test_airdrop_pathing_2", PathOutputType::STATIC);
+    plotter.output("test_coverage_pathing_2", PathOutputType::STATIC);
     file.close();
     return 0;
 }