Feat/precompute pathing

configs/dev.json

@@ -18,6 +18,7 @@
     },
     "pathing": {
         "laps": 2,
+        "upload_distance_buffer_m": 50.0,
         "dubins": {
             "turning_radius": 30.0,
             "point_separation": 20.0

configs/jetson.json

@@ -18,6 +18,7 @@
     },
     "pathing": {
         "laps": 10,
+        "upload_distance_buffer_m": 50.0,
         "dubins": {
             "turning_radius": 5.0,
             "point_separation": 20.0

include/core/mission_state.hpp

@@ -48,6 +48,9 @@ class MissionState {
     void setInitPath(const MissionPath& init_path);
     MissionPath getInitPath();
 
+    void setNextWaypointPath(const MissionPath& next_waypoint_path);
+    MissionPath getNextWaypointPath();
+
     void setCoveragePath(const MissionPath& coverage_path);
     MissionPath getCoveragePath();
 
@@ -130,6 +133,8 @@ class MissionState {
 
     std::mutex init_path_mut;  // for reading/writing the initial path
     MissionPath init_path;
+    std::mutex next_waypoint_path_mut;  // for reading/writing the next waypoint path
+    MissionPath next_waypoint_path;
     std::mutex coverage_path_mut;  // for reading/writing the coverage path
     MissionPath coverage_path;
     std::mutex airdrop_path_mut;

include/pathing/static.hpp

@@ -302,11 +302,18 @@ class AirdropApproachPathing {
  */
 RRTPoint getCurrentLoc(std::shared_ptr<MissionState> state);
 
-MissionPath generateInitialPath(std::shared_ptr<MissionState> state);
+std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state);
 
-MissionPath generateSearchPath(std::shared_ptr<MissionState> state);
+std::vector<GPSCoord>
+generateNextWaypointPath(std::shared_ptr<MissionState> state, double start_angle);
 
-MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal);
+std::vector<GPSCoord>
+generateSearchPath(std::shared_ptr<MissionState> state, double start_angle);
+
+double calculateFinalAngle(const std::vector<GPSCoord>& path, std::shared_ptr<MissionState> state);
+
+std::vector<GPSCoord>
+generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal);
 
 std::pair<double, double> estimateAreaCoveredAndPathLength(const std::vector<XYZCoord> &goals,
                                                            const Environment &airspace);

include/ticks/path_gen.hpp

@@ -27,8 +27,7 @@ class PathGenTick : public Tick {
     void init() override;
     Tick* tick() override;
  private:
-    std::future<MissionPath> init_path;
-    std::future<MissionPath> coverage_path;
+    std::future<void> paths_future;
 
     void startPathGeneration();
 };

include/utilities/obc_config.hpp

@@ -106,6 +106,7 @@ struct AirdropApproachConfig {
 
 struct PathingConfig {
     int laps;
+    double upload_distance_buffer_m;
     DubinsConfig dubins;
     RRTConfig rrt;
     AirdropCoverageConfig coverage;

src/core/mission_state.cpp

@@ -79,6 +79,16 @@ MissionPath MissionState::getInitPath() {
     return this->init_path;
 }
 
+void MissionState::setNextWaypointPath(const MissionPath& next_waypoint_path) {
+    Lock lock(this->next_waypoint_path_mut);
+    this->next_waypoint_path = next_waypoint_path;
+}
+
+MissionPath MissionState::getNextWaypointPath() {
+    Lock lock(this->next_waypoint_path_mut);
+    return this->next_waypoint_path;
+}
+
 void MissionState::setCoveragePath(const MissionPath& coverage_path) {
     Lock lock(this->coverage_path_mut);
     this->coverage_path = coverage_path;

src/network/mavlink.cpp

@@ -186,7 +186,7 @@ MavlinkClient::MavlinkClient(OBCConfig config)
     });
 
     this->telemetry->subscribe_heading([this](mavsdk::Telemetry::Heading heading) {
-        VLOG_F(DEBUG, "Heading: %d", heading.heading_deg);
+        VLOG_F(DEBUG, "Heading: %f deg", heading.heading_deg);
         Lock lock(this->data_mut);
         this->data.heading_deg = heading.heading_deg;
     });

src/pathing/static.cpp

@@ -619,7 +619,7 @@ RRTPoint getCurrentLoc(std::shared_ptr<MissionState> state) {
     return RRTPoint(start_xyz, start_angle);
 }
 
-MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
+std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state) {
     // first waypoint is start
 
     // the other waypoitns is the goals
@@ -653,13 +653,65 @@ MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
         output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint));
     }
 
-    return MissionPath(MissionPath::Type::FORWARD, output_coords);
+    return output_coords;
 }
 
-MissionPath generateSearchPath(std::shared_ptr<MissionState> state) {
+std::vector<GPSCoord>
+generateNextWaypointPath(std::shared_ptr<MissionState> state, double start_angle) {
+    if (state->mission_params.getWaypoints().size() < 1) {
+        loguru::set_thread_name("Static Pathing");
+        LOG_F(ERROR, "Not enough waypoints to generate a path, requires >=1, num waypoints: %s",
+              std::to_string(state->mission_params.getWaypoints().size()).c_str());
+        return {};
+    }
+
+    std::vector<XYZCoord> goals = state->mission_params.getWaypoints();
+
+    if (state->config.pathing.rrt.generate_deviations) {
+        Environment mapping_bounds(state->mission_params.getAirdropBoundary(), {}, {}, goals, {});
+        goals = generateRankedNewGoalsList(goals, mapping_bounds)[0];
+    }
+
+    RRTPoint start(goals.back(), start_angle);
+
+    // add buffer to the start point so that we dont loopty loop
+    double buffer_m = state->config.pathing.upload_distance_buffer_m;
+    if (buffer_m > 0.0) {
+        start.coord.x += buffer_m * std::cos(start_angle);
+        start.coord.y += buffer_m * std::sin(start_angle);
+    }
+
+    RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), state->config,
+            {}, {});
+
+    rrt.run();
+
+    std::vector<XYZCoord> path = rrt.getPointsToGoal();
+
+    std::vector<GPSCoord> output_coords;
+    for (const XYZCoord &waypoint : path) {
+        output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint));
+    }
+
+    return output_coords;
+}
+
+double calculateFinalAngle(const std::vector<GPSCoord>& path, std::shared_ptr<MissionState> state) {
+
+    if (path.size() < 2) {
+        return 0.0;
+    }
+
+    XYZCoord pt1 = state->getCartesianConverter()->toXYZ(path[path.size() - 2]);
+    XYZCoord pt2 = state->getCartesianConverter()->toXYZ(path[path.size() - 1]);
+
+    return std::atan2(pt2.y - pt1.y, pt2.x - pt1.x);
+}
+
+std::vector<GPSCoord> generateSearchPath(std::shared_ptr<MissionState> state, double start_angle) {
     std::vector<GPSCoord> gps_coords;
     if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) {
-        RRTPoint start(state->mission_params.getWaypoints().back(), 0);
+        RRTPoint start(state->mission_params.getWaypoints().back(), start_angle);
         double scan_radius = state->config.pathing.coverage.camera_vision_m;
 
         ForwardCoveragePathing pathing(start, scan_radius,
@@ -670,19 +722,19 @@ MissionPath generateSearchPath(std::shared_ptr<MissionState> state) {
             gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord));
         }
 
-        return MissionPath(MissionPath::Type::FORWARD, gps_coords);
+        return gps_coords;
     } else {  // hover
         HoverCoveragePathing pathing(state);
 
         for (const auto &coord : pathing.run()) {
             gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord));
         }
-        return MissionPath(MissionPath::Type::HOVER, gps_coords,
-                           state->config.pathing.coverage.hover.hover_time_s);
+        return gps_coords;
     }
 }
 
-MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal) {
+std::vector<GPSCoord>
+generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal) {
     std::shared_ptr<MavlinkClient> mav = state->getMav();
     /*
         Note: this function was neutered right before we attempted to fly at the 2024 competition
@@ -725,5 +777,5 @@ MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const G
     // gps_path.push_back(goal);
     // gps_path.push_back(goal);
 
-    return MissionPath(MissionPath::Type::FORWARD, gps_path);
+    return gps_path;
 }

src/ticks/airdrop_prep.cpp

@@ -67,7 +67,8 @@ Tick* AirdropPrepTick::tick() {
           target.coordinate().latitude(), target.coordinate().longitude(),
           target.coordinate().altitude());
 
-    state->setAirdropPath(generateAirdropApproach(state, target.coordinate()));
+    state->setAirdropPath(MissionPath(MissionPath::Type::FORWARD,
+                            generateAirdropApproach(state, target.coordinate())));
 
     LOG_F(INFO, "Generated approach path");
 

src/ticks/fly_waypoints.cpp

@@ -81,31 +81,12 @@ Tick* FlyWaypointsTick::tick() {
     }
 
 
-    if (state->getLapsRemaining() > 0) {
-        // regenerate path
-        std::future<MissionPath> init_path;
-        init_path = std::async(std::launch::async, generateInitialPath, this->state);
-        auto init_status = init_path.wait_for(std::chrono::milliseconds(0));
-        int count_ms = 2500;
-        const int wait_time_ms = 100;
-
-        while (init_status != std::future_status::ready && count_ms > 0) {
-            LOG_F(WARNING, "Waiting for path to be generated...");
-            std::this_thread::sleep_for(std::chrono::milliseconds(wait_time_ms));
-            init_status = init_path.wait_for(std::chrono::milliseconds(0));
-            count_ms -= wait_time_ms;
-        }
-
-        if (count_ms <= 0) {
-            LOG_F(ERROR, "Path generation took too long. Trying Again...");
-            return nullptr;
-        }
-
-        state->setInitPath(init_path.get());
+    if (state->getLapsRemaining() > 1) {
+        state->decrementLapsRemaining();
 
         return new MavUploadTick(
             this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)),
-            state->getInitPath(), false);
+            state->getNextWaypointPath(), false);
     }
 
     return new MavUploadTick(

src/ticks/path_gen.cpp

@@ -22,20 +22,44 @@ void PathGenTick::init() {
 }
 
 Tick* PathGenTick::tick() {
-    auto init_status = this->init_path.wait_for(0ms);
-    auto search_status = this->coverage_path.wait_for(0ms);
-    if (init_status == std::future_status::ready &&
-        search_status == std::future_status::ready) {
+    auto status = this->paths_future.wait_for(0ms);
+    if (status == std::future_status::ready) {
         LOG_F(INFO, "Initial and Coverage paths generated");
-        state->setInitPath(this->init_path.get());
-        state->setCoveragePath(this->coverage_path.get());
         return new PathValidateTick(this->state);
     }
 
     return nullptr;
 }
 
 void PathGenTick::startPathGeneration() {
-    this->init_path = std::async(std::launch::async, generateInitialPath, this->state);
-    this->coverage_path = std::async(std::launch::async, generateSearchPath, this->state);
+    this->paths_future = std::async(std::launch::async, [this]() {
+        std::vector<GPSCoord> init_gps = generateInitialPath(this->state);
+
+        double angle1 = calculateFinalAngle(init_gps, this->state);
+
+        std::vector<GPSCoord> next_gps = generateNextWaypointPath(this->state, angle1);
+        init_gps.reserve(init_gps.size() + (state->config.pathing.laps-1) * next_gps.size());
+        for (int i = 0; i < state->config.pathing.laps-1; i++) {
+            init_gps.insert(init_gps.end(), next_gps.begin(), next_gps.end());
+        }
+        double angle2 = calculateFinalAngle(next_gps, this->state);
+
+        int num_waypoints_to_remove =
+            std::ceil(this->state->config.pathing.upload_distance_buffer_m /
+                      this->state->config.pathing.dubins.point_separation);
+         std::vector<GPSCoord> coverage_gps = generateSearchPath(this->state, angle2);
+         coverage_gps.erase(coverage_gps.begin(), coverage_gps.begin() + num_waypoints_to_remove);;
+
+        MissionPath coverage;
+        if (this->state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) {
+            coverage = MissionPath(MissionPath::Type::FORWARD, coverage_gps);
+        } else {
+            coverage = MissionPath(MissionPath::Type::HOVER, coverage_gps,
+                                   this->state->config.pathing.coverage.hover.hover_time_s);
+        }
+        MissionPath waypoint = MissionPath(MissionPath::Type::FORWARD, init_gps);
+        this->state->setInitPath(waypoint);
+        this->state->setNextWaypointPath(waypoint); // TO DO: DELETE NEXT WAYPOINT PATH
+        this->state->setCoveragePath(coverage);
+    });
 }

src/utilities/obc_config.cpp

@@ -60,6 +60,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) {
 
     SET_CONFIG_OPT(pathing, laps);
     SET_CONFIG_OPT(pathing, rrt, iterations_per_waypoint);
+    SET_CONFIG_OPT(pathing, upload_distance_buffer_m);
     SET_CONFIG_OPT(pathing, rrt, rewire_radius);
     SET_CONFIG_OPT(pathing, rrt, optimize);
     SET_CONFIG_OPT(pathing, rrt, generate_deviations);