Implement Refueling + Tick restructuring

configs/dev.json

@@ -13,7 +13,8 @@
         }
     },
     "takeoff": {
-        "altitude_m": 30.0
+        "altitude_m": 30.0,
+        "payload_size": 2
     },
     "pathing": {
         "dubins": {
@@ -25,7 +26,8 @@
             "rewire_radius": 256.0,
             "optimize": true,
             "point_fetch_method": "nearest",
-            "allowed_to_skip_waypoints": false
+            "allowed_to_skip_waypoints": false,
+            "generate_deviations": false
         },
         "coverage": {
             "altitude_m": 30.0,

include/core/mission_parameters.hpp

@@ -30,6 +30,7 @@ class MissionParameters {
 
     Polygon getFlightBoundary();
     Polygon getAirdropBoundary();
+    Polygon getMappingBoundary();
     Polyline getWaypoints();
     // CHANGE: Return type is now std::vector<Airdrop>
     std::vector<Airdrop> getAirdrops();
@@ -39,7 +40,7 @@ class MissionParameters {
     // Important to use this instead of the singular getters
     // to avoid race conditions
     // CHANGE: Tuple element type is now std::vector<Airdrop>
-    std::tuple<Polygon, Polygon, Polyline, std::vector<Airdrop>> getConfig();
+    std::tuple<Polygon, Polygon, Polygon, Polyline, std::vector<Airdrop>> getConfig();
 
     // returns error string to be displayed back to the user
     // Ensure CartesianConverter<GPSProtoVec> template parameter matches usage
@@ -54,6 +55,7 @@ class MissionParameters {
 
     Polygon flightBoundary;
     Polygon airdropBoundary;
+    Polygon mappingBoundary;
     Polyline waypoints;
     // CHANGE: Member variable type is now std::vector<Airdrop>
     std::vector<Airdrop> airdrops;

include/ticks/ids.hpp

@@ -19,6 +19,7 @@ enum class TickID {
     MissionDone,
     ActiveTakeoff,
     WaitForTakeoff,
+    Refueling,
 };
 
 #define _SET_TICK_ID_MAPPING(id) \
@@ -41,6 +42,7 @@ constexpr const char* TICK_ID_TO_STR(TickID id) {
         _SET_TICK_ID_MAPPING(MissionDone);
         _SET_TICK_ID_MAPPING(ActiveTakeoff);
         _SET_TICK_ID_MAPPING(WaitForTakeoff);
+        _SET_TICK_ID_MAPPING(Refueling);
         default: return "Unknown TickID";
     }
 }

include/ticks/manual_landing.hpp

@@ -11,11 +11,14 @@
  */
 class ManualLandingTick : public Tick {
  public:
-    explicit ManualLandingTick(std::shared_ptr<MissionState> state);
+    explicit ManualLandingTick(std::shared_ptr<MissionState> state, Tick* next_tick);
 
     std::chrono::milliseconds getWait() const override;
 
     Tick* tick() override;
+
+ private:
+    Tick* next_tick;
 };
 
 #endif  // INCLUDE_TICKS_MANUAL_LANDING_HPP_

include/ticks/refueling.hpp

@@ -0,0 +1,21 @@
+#ifndef INCLUDE_TICKS_REFUELING_HPP_
+#define INCLUDE_TICKS_REFUELING_HPP_
+
+#include <chrono>
+#include <memory>
+
+#include "ticks/tick.hpp"
+
+/*
+ * Wait for the refueling process to finish.
+ */
+class RefuelingTick : public Tick {
+ public:
+    explicit RefuelingTick(std::shared_ptr<MissionState> state);
+
+    std::chrono::milliseconds getWait() const override;
+
+    Tick* tick() override;
+};
+
+#endif  // INCLUDE_TICKS_REFUELING_HPP_

include/utilities/constants.hpp

@@ -77,6 +77,7 @@ const std::chrono::milliseconds AUTO_LANDING_TICK_WAIT = std::chrono::millisecon
 const std::chrono::milliseconds MISSION_DONE_TICK_WAIT = std::chrono::milliseconds(100);
 const std::chrono::milliseconds ACTIVE_TAKEOFF_TICK_WAIT = std::chrono::milliseconds(100);
 const std::chrono::milliseconds WAIT_FOR_TAKEOFF_TICK_WAIT = std::chrono::milliseconds(100);
+const std::chrono::milliseconds REFUELING_TICK_WAIT = std::chrono::milliseconds(100);
 
 const double EARTH_RADIUS_METERS = 6378137.0;
 

include/utilities/obc_config.hpp

@@ -32,6 +32,7 @@ struct NetworkConfig {
 
 struct TakeoffConfig {
     float altitude_m;
+    int payload_size;
 };
 
 struct CVConfig {
@@ -63,6 +64,7 @@ struct RRTConfig {
     PointFetchMethod::Enum point_fetch_method;
     bool allowed_to_skip_waypoints;  // if true, will skip waypoints if it can not connect after 1
                                      // RRT iteration
+    bool generate_deviations;
 };
 
 namespace AirdropCoverageMethod {

src/core/mission_parameters.cpp

@@ -49,6 +49,11 @@ Polygon MissionParameters::getAirdropBoundary() {
     return this->airdropBoundary;
 }
 
+Polygon MissionParameters::getMappingBoundary() {
+    ReadLock lock(this->mut);
+    return this->mappingBoundary;
+}
+
 Polyline MissionParameters::getWaypoints() {
     ReadLock lock(this->mut);
     return this->waypoints;
@@ -59,10 +64,12 @@ std::vector<Airdrop> MissionParameters::getAirdrops() {
     return this->airdrops;
 }
 
-std::tuple<Polygon, Polygon, Polyline, std::vector<Airdrop>> MissionParameters::getConfig() {
+std::tuple<Polygon, Polygon, Polygon, Polyline, std::vector<Airdrop>>
+MissionParameters::getConfig() {
     ReadLock lock(this->mut);
 
-    return std::make_tuple(this->flightBoundary, this->airdropBoundary, this->waypoints,
+    return std::make_tuple(this->flightBoundary, this->airdropBoundary,
+                           this->mappingBoundary, this->waypoints,
                            this->airdrops);
 }
 
@@ -94,13 +101,19 @@ std::optional<std::string> MissionParameters::setMission(
     if (mission.airdropboundary().size() < 3) {
         err += "Airdrop boundary must have at least 3 coordinates.";
     }
+
+    if (mission.mappingboundary().size() < 3) {
+        err += "Mapping boundary must have at least 3 coordinates.";
+    }
+
     if (!err.empty()) {
         return err;
     }
 
     this->cached_mission = mission;
     this->flightBoundary = cconverter.toXYZ(mission.flightboundary());
     this->airdropBoundary = cconverter.toXYZ(mission.airdropboundary());
+    this->mappingBoundary = cconverter.toXYZ(mission.mappingboundary());
     this->waypoints = cconverter.toXYZ(mission.waypoints());
     for (const auto& airdrop : mission.airdropassignments()) {  // Use const& for efficiency
         this->_setAirdrop(airdrop);

src/cv/pipeline.cpp

@@ -74,9 +74,6 @@ PipelineResults Pipeline::run(const ImageData& imageData) {
         GPSCoord targetPosition;
         if (imageData.TELEMETRY.has_value()) {
             targetPosition = this->gsdLocalizer.localize(imageData.TELEMETRY.value(), box);
-            std::cout << "pipeline: " <<
-            targetPosition.latitude() << " "
-            << targetPosition.longitude();
         }
 
 

src/pathing/static.cpp

@@ -451,10 +451,11 @@ std::vector<XYZCoord> HoverCoveragePathing::run() {
               this->drop_zone.size());
     }
 
-    XYZCoord top_left = this->drop_zone.at(3);
-    XYZCoord top_right = this->drop_zone.at(2);
+    // Input Coordinates MUST BE in this order
     XYZCoord bottom_left = this->drop_zone.at(0);
     XYZCoord bottom_right = this->drop_zone.at(1);
+    XYZCoord top_right = this->drop_zone.at(2);
+    XYZCoord top_left = this->drop_zone.at(3);
 
     std::vector<XYZCoord> hover_points;
 
@@ -545,11 +546,11 @@ std::vector<std::vector<XYZCoord>> generateGoalListDeviations(const std::vector<
 }
 
 std::vector<std::vector<XYZCoord>> generateRankedNewGoalsList(const std::vector<XYZCoord> &goals,
-                                                              const Environment &airspace) {
+                                                              const Environment &mapping_bounds) {
     // generate deviation points randomly in the mapping region
     std::vector<XYZCoord> deviation_points;
     for (int i = 0; i < 200; i++) {
-        deviation_points.push_back(airspace.getRandomPoint(true));
+        deviation_points.push_back(mapping_bounds.getRandomPoint(true));
         printf("Deviation point: %f, %f\n", deviation_points.back().x, deviation_points.back().y);
     }
 
@@ -565,7 +566,7 @@ std::vector<std::vector<XYZCoord>> generateRankedNewGoalsList(const std::vector<
     // run each goal list and get the area covered and the length of the path
     std::vector<std::pair<double, double>> area_length_pairs;
     for (const std::vector<XYZCoord> &new_goals : new_goals_list) {
-        area_length_pairs.push_back(airspace.estimateAreaCoveredAndPathLength(new_goals));
+        area_length_pairs.push_back(mapping_bounds.estimateAreaCoveredAndPathLength(new_goals));
     }
 
     // rank the new goal lists by the area covered and the length of the path
@@ -607,9 +608,10 @@ MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
     }
 
     // update goals here
-    // create a dummy environment
-    Environment airspace(state->mission_params.getFlightBoundary(), {}, {}, goals, {});
-    goals = generateRankedNewGoalsList(goals, airspace)[0];
+    if (state->config.pathing.rrt.generate_deviations) {
+        Environment mapping_bounds(state->mission_params.getMappingBoundary(), {}, {}, goals, {});
+        goals = generateRankedNewGoalsList(goals, mapping_bounds)[0];
+    }
 
     double init_angle =
         std::atan2(goals.front().y - state->mission_params.getWaypoints().front().y,
@@ -633,17 +635,29 @@ MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
 }
 
 MissionPath generateSearchPath(std::shared_ptr<MissionState> state) {
+    std::vector<GPSCoord> gps_coords;
     if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) {
         LOG_F(FATAL, "Forward search path not fully integrated yet.");
+
+        RRTPoint start(state->mission_params.getWaypoints().front(), 0);
+
+        // TODO , change the starting point to be something closer to loiter
+        // region
+        ForwardCoveragePathing pathing(start, SEARCH_RADIUS,
+                                       state->mission_params.getFlightBoundary(),
+                                       state->mission_params.getAirdropBoundary(), state->config);
+
+        for (const auto &coord : pathing.run()) {
+            gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord));
+        }
+
         return MissionPath(MissionPath::Type::FORWARD, {});
     } else {  // hover
         HoverCoveragePathing pathing(state);
 
-        std::vector<GPSCoord> gps_coords;
         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);
     }

src/ticks/CMakeLists.txt

@@ -16,6 +16,7 @@ set(FILES
     takeoff.cpp
     active_takeoff.cpp
     wait_for_takeoff.cpp
+    refueling.cpp
 )
 
 set(LIB_DEPS

src/ticks/active_takeoff.cpp

@@ -5,6 +5,7 @@
 
 #include "ticks/ids.hpp"
 #include "utilities/constants.hpp"
+#include "ticks/airdrop_prep.hpp"
 #include "ticks/fly_waypoints.hpp"
 #include "ticks/mav_upload.hpp"
 #include "ticks/mission_prep.hpp"
@@ -50,7 +51,13 @@ Tick* ActiveTakeoffTick::tick() {
     // NOTE: keep in sync with manual takeoff tick
     // transitions to flying waypoints tick, such that when the flying waypoints
     // tick is done it transitions to uploading the coverage path
-    return new FlyWaypointsTick(this->state, new MavUploadTick(
-        this->state, new FlySearchTick(this->state),
-        state->getCoveragePath(), false));
+
+    if (state->getDroppedAirdrops().size() >= this->state->config.takeoff.payload_size) {
+        return new FlyWaypointsTick(this->state, new AirdropPrepTick(this->state));
+        // return new AirdropPrepTick(this->state);
+    } else {
+        return new FlyWaypointsTick(this->state, new MavUploadTick(
+            this->state, new FlySearchTick(this->state),
+            state->getCoveragePath(), false));
+    }
 }

src/ticks/airdrop_approach.cpp

@@ -8,6 +8,8 @@
 #include "ticks/airdrop_prep.hpp"
 #include "ticks/manual_landing.hpp"
 #include "utilities/constants.hpp"
+#include "ticks/refueling.hpp"
+#include "ticks/wait_for_takeoff.hpp"
 
 AirdropApproachTick::AirdropApproachTick(std::shared_ptr<MissionState> state)
     :Tick(state, TickID::AirdropApproach) {}
@@ -35,7 +37,9 @@ Tick* AirdropApproachTick::tick() {
 
     if (state->getMav()->isMissionFinished()) {
         if (state->getDroppedAirdrops().size() >= NUM_AIRDROPS) {
-            return new ManualLandingTick(state);
+            return new ManualLandingTick(state, nullptr);
+        } else if (state->getDroppedAirdrops().size() % state->config.takeoff.payload_size == 0) {
+            return new ManualLandingTick(state, new RefuelingTick(state));
         } else {
             return new MavUploadTick(state, new FlyWaypointsTick(state,
                 new AirdropPrepTick(state)), state->getInitPath(), false);

src/ticks/airdrop_prep.cpp

@@ -23,7 +23,7 @@ Tick* AirdropPrepTick::tick() {
     auto dropped_airdrops = state->getDroppedAirdrops();
 
     if (dropped_airdrops.size() >= NUM_AIRDROPS) {
-        return new ManualLandingTick(state);
+        return new ManualLandingTick(state, nullptr);
     }
 
     LockPtr<MatchedResults> results = state->getCV()->getMatchedResults();

src/ticks/manual_landing.cpp

@@ -5,13 +5,17 @@
 #include "ticks/ids.hpp"
 #include "utilities/constants.hpp"
 
-ManualLandingTick::ManualLandingTick(std::shared_ptr<MissionState> state)
-    :Tick(state, TickID::ManualLanding) {}
+ManualLandingTick::ManualLandingTick(std::shared_ptr<MissionState> state, Tick* next_tick)
+    :Tick(state, TickID::ManualLanding), next_tick(next_tick) {}
 
 std::chrono::milliseconds ManualLandingTick::getWait() const {
     return MANUAL_LANDING_TICK_WAIT;
 }
 
 Tick* ManualLandingTick::tick() {
+    // TODO: Test this in mock testflight
+    if (!state.get()->getMav()->isArmed()) {
+        return next_tick;
+    }
     return nullptr;
 }

src/ticks/refueling.cpp

@@ -0,0 +1,22 @@
+#include "ticks/refueling.hpp"
+
+#include <memory>
+
+#include "ticks/active_takeoff.hpp"
+#include "ticks/ids.hpp"
+#include "ticks/mav_upload.hpp"
+#include "utilities/constants.hpp"
+
+RefuelingTick::RefuelingTick(std::shared_ptr<MissionState> state)
+    : Tick(state, TickID::Refueling) {}
+
+std::chrono::milliseconds RefuelingTick::getWait() const { return REFUELING_TICK_WAIT; }
+
+Tick* RefuelingTick::tick() {
+    if (state.get()->getMav()->isArmed()) {
+        return new MavUploadTick(state, new WaitForTakeoffTick(state), state->getInitPath(), false);
+    }
+
+    return nullptr;
+    // return new MavUploadTick(state, new WaitForTakeoffTick(state), state->getInitPath(), false);
+}

src/ticks/takeoff.cpp

@@ -22,6 +22,8 @@ Tick* TakeoffTick::tick() {
         // NOTE: keep in sync with active_takeoff tick
         // transitions to flying waypoints tick, such that when the flying waypoints
         // tick is done it transitions to uploading the coverage path
+
+        // This is MANUAL Takeoff (idk why its named so confusing)
         return new FlyWaypointsTick(this->state, new MavUploadTick(
             this->state, new FlySearchTick(this->state),
             state->getCoveragePath(), false));

src/utilities/obc_config.cpp

@@ -61,6 +61,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) {
     SET_CONFIG_OPT(pathing, rrt, iterations_per_waypoint);
     SET_CONFIG_OPT(pathing, rrt, rewire_radius);
     SET_CONFIG_OPT(pathing, rrt, optimize);
+    SET_CONFIG_OPT(pathing, rrt, generate_deviations);
 
     SET_CONFIG_OPT_VARIANT(PointFetchMethod, pathing, rrt, point_fetch_method);
 
@@ -95,6 +96,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) {
     SET_CONFIG_OPT(camera, mock, images_dir);
 
     SET_CONFIG_OPT(takeoff, altitude_m);
+    SET_CONFIG_OPT(takeoff, payload_size);
 
     std::string common_params_path = params_dir / "common.json";
     std::ifstream common_params_stream(common_params_path);