Initial Conversion of RRT pathing

tritonuas · Apr 18, 2024 · e079ea1 · e079ea1
1 parent ac4e301
commit e079ea1
Show file tree

Hide file tree

Showing 7 changed files with 88 additions and 47 deletions.
diff --git a/configs/config.json b/configs/config.json
@@ -9,5 +9,18 @@
         "gcs": {
             "port": 5010
         }
+    },
+    "static-pathing": {
+        "rrt": {
+            "iterations_per_waypoint": 200,
+            "rewire_radius": 200.0,
+            "k_closest_nodes": 50,
+            "total_options_for_goal_connection": 2048
+        },
+        "cpp": {
+            "vertical": false,
+            "one-way": true,
+            "optimize": false
+        }
     }
 }
diff --git a/configs/default-config.json b/configs/default-config.json
@@ -9,5 +9,18 @@
         "mavlink": {
             "connect": "tcp://172.17.0.1:5760"
         }
+    },
+    "static-pathing": {
+        "rrt": {
+            "iterations_per_waypoint": 200,
+            "rewire_radius": 200.0,
+            "k_closest_nodes": 50,
+            "total_options_for_goal_connection": 2048
+        },
+        "cpp": {
+            "vertical": false,
+            "one-way": true,
+            "optimize": false
+        }
     }
 }
diff --git a/configs/dev-config.json b/configs/dev-config.json
@@ -9,5 +9,18 @@
         "gcs": {
             "port": 5010
         }
+    },
+    "static-pathing": {
+        "rrt": {
+            "iterations_per_waypoint": 200,
+            "rewire_radius": 200.0,
+            "k_closest_nodes": 50,
+            "total_options_for_goal_connection": 2048
+        },
+        "cpp": {
+            "vertical": false,
+            "one-way": true,
+            "optimize": false
+        }
     }
 }
diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp
@@ -21,10 +21,11 @@
 
 class RRT {
  public:
-    RRT(RRTPoint start, std::vector<XYZCoord> goals, int iterations_per_waypoint,
-        double search_radius, double rewire_radius, Polygon bounds,
+    RRT(RRTPoint start, std::vector<XYZCoord> goals, double search_radius, Polygon bounds,
         std::vector<Polygon> obstacles = {},
-        RRTConfig config = {.optimize = false,
+        RRTConfig config = {.iterations_per_waypoint = ITERATIONS_PER_WAYPOINT,
+                            .rewire_radius = REWIRE_RADIUS,
+                            .optimize = false,
                             .point_fetch_method = POINT_FETCH_METHODS::NONE,
                             .allowed_to_skip_waypoints = false});
 

diff --git a/include/utilities/datatypes.hpp b/include/utilities/datatypes.hpp
@@ -97,7 +97,9 @@ enum POINT_FETCH_METHODS {
 };
 
 struct RRTConfig {
-    bool optimize;  // run RRT* if true
+    int iterations_per_waypoint;  // number of iterations run between two waypoints
+    double rewire_radius;         // maximum distance from sampled point to optimize during RRT*
+    bool optimize;                // run RRT* if true
     POINT_FETCH_METHODS point_fetch_method;
     bool allowed_to_skip_waypoints;  // if true, will skip waypoints if it can not connect after 1
                                      // RRT iteration

diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp
@@ -18,12 +18,11 @@
 #include "utilities/datatypes.hpp"
 #include "utilities/rng.hpp"
 
-RRT::RRT(RRTPoint start, std::vector<XYZCoord> goals, int iterations_per_waypoint,
-         double search_radius, double rewire_radius, Polygon bounds, std::vector<Polygon> obstacles,
-         RRTConfig config)
-    : iterations_per_waypoint(iterations_per_waypoint),
+RRT::RRT(RRTPoint start, std::vector<XYZCoord> goals, double search_radius, Polygon bounds,
+         std::vector<Polygon> obstacles, RRTConfig config)
+    : iterations_per_waypoint(config.iterations_per_waypoint),
       search_radius(search_radius),
-      rewire_radius(rewire_radius),
+      rewire_radius(config.rewire_radius),
       tree(start, Environment(bounds, {}, goals, obstacles),
            Dubins(TURNING_RADIUS, POINT_SEPARATION)),
       config(config) {}
@@ -238,42 +237,6 @@ RRTNode *RRT::parseOptions(const std::vector<std::pair<RRTNode *, RRTOption>> &o
 
 void RRT::optimizeTree(RRTNode *sample) { tree.RRTStar(sample, rewire_radius); }
 
-std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state) {
-    // first waypoint is start
-    RRTPoint start(state->config.getWaypoints().front(), 0);
-
-    // 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",
-              std::to_string(state->config.getWaypoints().size()).c_str());
-        return {};
-    }
-
-    std::vector<XYZCoord> goals;
-
-    // Copy elements (reference) from the second element to the last element of source into
-    // destination all other methods of copying over crash???
-    for (int i = 1; i < state->config.getWaypoints().size(); i++) {
-        goals.emplace_back(state->config.getWaypoints()[i]);
-    }
-
-    RRT rrt(start, goals, ITERATIONS_PER_WAYPOINT, SEARCH_RADIUS, REWIRE_RADIUS,
-            state->config.getFlightBoundary(), {}, RRTConfig{true, POINT_FETCH_METHODS::NONE});
-
-    rrt.run();
-
-    std::vector<XYZCoord> path = rrt.getPointsToGoal();
-    std::vector<GPSCoord> output_coords;
-    int count = 0;
-
-    for (XYZCoord wpt : path) {
-        output_coords.push_back(state->getCartesianConverter().value().toLatLng(wpt));
-    }
-
-    return output_coords;
-}
-
 AirdropSearch::AirdropSearch(const RRTPoint &start, double scan_radius, Polygon bounds,
                              Polygon airdrop_zone, std::vector<Polygon> obstacles,
                              AirdropSearchConfig config)
@@ -371,3 +334,39 @@ std::vector<XYZCoord> AirdropSearch::run() const {
 
     return path;
 }
+
+std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state) {
+    // first waypoint is start
+    RRTPoint start(state->config.getWaypoints().front(), 0);
+
+    // 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",
+              std::to_string(state->config.getWaypoints().size()).c_str());
+        return {};
+    }
+
+    std::vector<XYZCoord> goals;
+
+    // Copy elements (reference) from the second element to the last element of source into
+    // destination all other methods of copying over crash???
+    for (int i = 1; i < state->config.getWaypoints().size(); i++) {
+        goals.emplace_back(state->config.getWaypoints()[i]);
+    }
+
+    RRT rrt(start, goals, SEARCH_RADIUS, state->config.getFlightBoundary(), {},
+            RRTConfig{ITERATIONS_PER_WAYPOINT, REWIRE_RADIUS, true, POINT_FETCH_METHODS::NONE});
+
+    rrt.run();
+
+    std::vector<XYZCoord> path = rrt.getPointsToGoal();
+    std::vector<GPSCoord> output_coords;
+    int count = 0;
+
+    for (XYZCoord wpt : path) {
+        output_coords.push_back(state->getCartesianConverter().value().toLatLng(wpt));
+    }
+
+    return output_coords;
+}
diff --git a/tests/integration/path_planning.cpp b/tests/integration/path_planning.cpp
@@ -279,9 +279,9 @@ int main() {
     int num_iterations = 512;
     double search_radius = 9999;
     double rewire_radius = 256;
-    RRTConfig config = RRTConfig { true, POINT_FETCH_METHODS::NONE, true };
+    RRTConfig config = RRTConfig { num_iterations, rewire_radius, true, POINT_FETCH_METHODS::NONE, true };
 
-    RRT rrt = RRT(start, goals, num_iterations, search_radius, rewire_radius,
+    RRT rrt = RRT(start, goals, search_radius, 
                   state->config.getFlightBoundary(), obstacles, config);
 
     // print out stats