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/network/mavlink.hpp

@@ -83,6 +83,7 @@ class MavlinkClient {
     mavsdk::Telemetry::RcStatus get_conn_status();
     bool armAndHover(std::shared_ptr<MissionState> state);
     bool startMission();
+    bool clearMission();
 
     /*
      * Triggers a relay on the ArduPilot
@@ -92,10 +93,9 @@ class MavlinkClient {
      */
     bool triggerRelay(int relay_number, bool state);
 
+    // Safety Functions
     void KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY();
-
-    // rtl
-    void rtl();
+    void returnToLaunch();
 
  private:
     mavsdk::Mavsdk mavsdk;

include/pathing/static.hpp

@@ -302,11 +302,26 @@ class AirdropApproachPathing {
  */
 RRTPoint getCurrentLoc(std::shared_ptr<MissionState> state);
 
-MissionPath generateInitialPath(std::shared_ptr<MissionState> state);
+/**
+ * Calculates the approximate angle of exit out of a waypoint path
+ * 
+ * @param path  ==> the waypoint path
+ * @param state ==> mission state
+ * @return          angle of exit (+x-axis is 0 deg, ccw is positive)
+ */
+double calculateFinalAngle(const MissionPath& path,
+                           const std::optional<CartesianConverter<GPSProtoVec>> cartesianConverter);
+
+std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state);
+
+std::vector<GPSCoord>
+generateNextWaypointPath(std::shared_ptr<MissionState> state, double start_angle);
 
-MissionPath generateSearchPath(std::shared_ptr<MissionState> state);
+std::vector<GPSCoord>
+generateSearchPath(std::shared_ptr<MissionState> state, double start_angle);
 
-MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal);
+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/gcs_routes.cpp

@@ -573,7 +573,7 @@ DEF_GCS_HANDLE(Post, camera, runpipeline) {
 
 DEF_GCS_HANDLE(Post, rtl) {
     LOG_REQUEST("POST", "/rtl");
-    state->getMav()->rtl();
+    state->getMav()->returnToLaunch();
     LOG_RESPONSE(INFO, "RTL activated", OK);
 }
 

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;
     });
@@ -511,12 +511,27 @@ bool MavlinkClient::startMission() {
     return true;
 }
 
+/**
+ * Clears the existng mission
+ */
+bool MavlinkClient::clearMission() {
+    LOG_F(INFO, "Sending clear mission command");
+    auto start_result = this->mission->clear_mission();
+    if (start_result != mavsdk::MissionRaw::Result::Success) {
+        LOG_S(ERROR) << "FAIL: Mission could not be cleared " << start_result;
+        return false;
+    }
+
+    LOG_F(INFO, "Mission Cleared!");
+    return true;
+}
+
 void MavlinkClient::KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY() {
     LOG_F(ERROR, "KILLING THE PLANE: SETTING AFS_TERMINATE TO 1");
     auto result = this->param->set_param_int("AFS_TERMINATE", 1);
     LOG_S(ERROR) << "KILL RESULT: " << result;
 }
 
-void MavlinkClient::rtl() {
+void MavlinkClient::returnToLaunch() {
     this->action->return_to_launch();
 }

src/pathing/static.cpp

@@ -619,7 +619,23 @@ RRTPoint getCurrentLoc(std::shared_ptr<MissionState> state) {
     return RRTPoint(start_xyz, start_angle);
 }
 
-MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
+double calculateFinalAngle(
+    const MissionPath& path,
+    const std::optional<CartesianConverter<GPSProtoVec>> cartesianConverter) {
+
+    const auto& coords = path.get();
+    if (coords.size() < 2) {
+        return 0.0;     // should probably either have angle between now and point
+                        // or use assert for force size >=2
+    }
+
+    XYZCoord pt1 = cartesianConverter->toXYZ(coords[coords.size() - 2]);
+    XYZCoord pt2 = cartesianConverter->toXYZ(coords[coords.size() - 1]);
+
+    return std::atan2(pt2.y - pt1.y, pt2.x - pt1.x);
+}
+
+std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state) {
     // first waypoint is start
 
     // the other waypoitns is the goals
@@ -653,13 +669,53 @@ 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;
+}
+
+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;
 }
 
-MissionPath generateSearchPath(std::shared_ptr<MissionState> state) {
+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 +726,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 +781,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_search.cpp

@@ -23,32 +23,17 @@ std::chrono::milliseconds FlySearchTick::getWait() const {
 }
 
 void FlySearchTick::init() {
+    // TODO: can we delete the following camera lines?
     this->state->getCamera()->startStreaming();
     this->airdrop_boundary = this->state->mission_params.getAirdropBoundary();
     this->last_photo_time = getUnixTime_ms();
 
-    // note: I didn't get around to testing if 1 would be a better value than 0
-    // to see if the mission start can be forced.
-    if (!this->state->getMav()->setMissionItem(1)) {
-        LOG_F(ERROR, "Failed to reset Mission");
-    }
-
     this->mission_started = this->state->getMav()->startMission();
 
-    // I have another one here because idk how startmIssion behaves exactly
-    if (!this->state->getMav()->setMissionItem(1)) {
-        LOG_F(ERROR, "Failed to reset Mission");
-    }
-
     LOG_F(INFO, "Total Waypoint #: %zu", this->state->getMav()->totalWaypoints());
 }
 
 Tick* FlySearchTick::tick() {
-    if (!this->mission_started) {
-        this->mission_started = this->state->getMav()->startMission();
-        return nullptr;
-    }
-
     bool isMissionFinished = state->getMav()->isMissionFinished();
 
     if (isMissionFinished) {

src/ticks/fly_waypoints.cpp

@@ -18,22 +18,9 @@ FlyWaypointsTick::FlyWaypointsTick(std::shared_ptr<MissionState> state, Tick* ne
     : Tick(state, TickID::FlyWaypoints), next_tick(next_tick) {}
 
 void FlyWaypointsTick::init() {
-    // note: I didn't get around to testing if 1 would be a better value than 0
-    // to see if the mission start can be forced.
-    if (!this->state->getMav()->setMissionItem(1)) {
-        LOG_F(ERROR, "Failed to reset Mission");
-    }
-
     this->mission_started = this->state->getMav()->startMission();
-    state->getCamera()->startStreaming();
-    this->last_photo_time = getUnixTime_ms();
-
-    // I have another one here because idk how startmIssion behaves exactly
-    if (!this->state->getMav()->setMissionItem(1)) {
-        LOG_F(ERROR, "Failed to reset Mission");
-    }
-
     state->decrementLapsRemaining();
+
     LOG_F(INFO, "Started FlyWaypointsTick, Laps Remaining: %d", state->getLapsRemaining());
 }
 
@@ -80,32 +67,10 @@ Tick* FlyWaypointsTick::tick() {
         return nullptr;
     }
 
-
     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());
-
         return new MavUploadTick(
             this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)),
-            state->getInitPath(), false);
+            state->getNextWaypointPath(), false);
     }
 
     return new MavUploadTick(

src/ticks/mav_upload.cpp

@@ -24,6 +24,7 @@ std::chrono::milliseconds MavUploadTick::getWait() const {
 }
 
 void MavUploadTick::init() {
+    this->state->getMav()->clearMission();
     this->mav_uploaded = std::async(std::launch::async,
                                     &MavlinkClient::uploadMissionUntilSuccess,
                                     this->state->getMav(),