Refactor aggregator

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
@@ -54,9 +55,12 @@
     },
     "cv": {
         "yolo_model_dir": "/workspaces/obcpp/models/yolo-wittner-v2.onnx",
-        "detection_threshold": 0.35,
+        "detection_threshold": 0.0001,
         "input_width": 1024,
-        "input_height": 1024
+        "input_height": 1024,
+        "sample_every_n_images": 10,
+        "image_listener_poll_interval_ms": 500,
+        "image_listener_settle_time_ms": 200
     },
     "camera": {
         "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations",

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
@@ -56,7 +57,10 @@
         "yolo_model_dir": "/obcpp/models/yolo-wittner-v2.onnx",
         "detection_threshold": 0.35,
         "input_width": 1024,
-        "input_height": 1024
+        "input_height": 1024,
+        "sample_every_n_images": 10,
+        "image_listener_poll_interval_ms": 500,
+        "image_listener_settle_time_ms": 200
     },
     "camera": {
         "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations",

include/core/mission_state.hpp

@@ -9,6 +9,7 @@
 #include <mutex>
 #include <optional>
 #include <queue>
+#include <unordered_map>
 #include <unordered_set>
 #include <vector>
 #include <boost/asio.hpp>
@@ -48,6 +49,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();
 
@@ -98,6 +102,13 @@ class MissionState {
     std::shared_ptr<CVAggregator> getCV();
     void setCV(std::shared_ptr<CVAggregator> cv);
 
+
+    struct MatchedResults {
+        std::unordered_map<AirdropType, AirdropTarget> matched_airdrop;
+    };
+
+    LockPtr<MatchedResults> getMatchedResults();
+
     enum class CVStatus {
         None = 0,
         Validated = 1,
@@ -139,6 +150,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;
@@ -154,6 +167,9 @@ class MissionState {
     std::shared_ptr<AirdropClient> airdrop;
     std::shared_ptr<CVAggregator> cv;
 
+    std::mutex matched_results_mut;
+    std::shared_ptr<MatchedResults> matched_results;
+
     std::mutex cv_status_mut;
     CVStatus cv_status = CVStatus::None;
 

include/cv/aggregator.hpp

@@ -3,14 +3,16 @@
 
 #include <atomic>
 #include <cmath>
+#include <filesystem>
 #include <functional>
 #include <future>
 #include <map>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <queue>
+#include <string>
 #include <thread>
-#include <unordered_map>
 #include <vector>
 
 #include "cv/pipeline.hpp"
@@ -30,27 +32,28 @@ struct CVResults {
     std::vector<AggregatedRun> runs;  // Each pipeline invocation => 1 run
 };
 
-struct MatchedResults {
-    std::unordered_map<AirdropType, AirdropTarget> matched_airdrop;
-};
-
 class CVAggregator {
  public:
-    explicit CVAggregator(Pipeline&& p);
+    CVAggregator(Pipeline&& p, const std::string& image_dir, int sample_every_n_images,
+                 int image_listener_poll_interval_ms, int image_listener_settle_time_ms);
     ~CVAggregator();
 
     // Spawn a thread to run the pipeline on the given imageData
     void runPipeline(const ImageData& image);
 
+    // Watch a camera save directory and submit every nth saved image to the pipeline
+    void startListening(const std::string& image_dir, int sample_every_n_images,
+                        int image_listener_poll_interval_ms, int image_listener_settle_time_ms);
+
+    // Stop watching the camera save directory
+    void stopListening();
+
     // Stop accepting work, discard queued images, and wait for active workers to finish
     void terminate();
 
     // Lockable pointer to retrieve aggregator results
     LockPtr<CVResults> getResults();
 
-    // Lockable pointer to retrieve matched results (after manual match)
-    LockPtr<MatchedResults> getMatchedResults();
-
     // For the endpoint to reset the current list of structs
     std::vector<AggregatedRun> popAllRuns();
 
@@ -65,22 +68,26 @@ class CVAggregator {
     std::shared_ptr<std::map<int, IdentifiedTarget>> cv_record;
 
     void worker(ImageData image, int thread_num);
+    void listenForImages(std::filesystem::path image_dir, int sample_every_n_images,
+                         int image_listener_poll_interval_ms, int image_listener_settle_time_ms);
+    std::optional<ImageData> loadImageData(const std::filesystem::path& image_path) const;
+    std::optional<ImageTelemetry> loadTelemetry(
+        const std::filesystem::path& telemetry_path) const;
 
     Pipeline pipeline;
 
     std::mutex mut;
     std::atomic<int> num_worker_threads;
     std::atomic<bool> accepting_images;
+    std::atomic<bool> listening_images;
     std::vector<std::thread> worker_threads;
+    std::thread listener_thread;
 
     // For when too many pipelines are active at once
     std::queue<ImageData> overflow_queue;
 
     // Shared aggregator results
     std::shared_ptr<CVResults> results;
-
-    // Shared matched results
-    std::shared_ptr<MatchedResults> matched_results;
 };
 
 #endif  // INCLUDE_CV_AGGREGATOR_HPP_

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,27 @@ 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/airdrop_approach.hpp

@@ -20,6 +20,9 @@ class AirdropApproachTick : public Tick {
     std::chrono::milliseconds getWait() const override;
 
     Tick* tick() override;
+
+ private:
+    bool mission_started;
 };
 
 bool triggerAirdrop(std::shared_ptr<MavlinkClient> mav, airdrop_t airdrop_index);

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

@@ -40,6 +40,9 @@ struct CVConfig {
     float detection_threshold;
     int input_width;
     int input_height;
+    int sample_every_n_images;
+    int image_listener_poll_interval_ms;
+    int image_listener_settle_time_ms;
     std::string not_stolen_addr;
     uint16_t not_stolen_port;
 };
@@ -106,6 +109,7 @@ struct AirdropApproachConfig {
 
 struct PathingConfig {
     int laps;
+    double upload_distance_buffer_m;
     DubinsConfig dubins;
     RRTConfig rrt;
     AirdropCoverageConfig coverage;

src/camera/mock.cpp

@@ -133,9 +133,12 @@ std::optional<ImageData> MockCamera::takePicture(const std::chrono::milliseconds
         return std::nullopt;
     }
 
+    uint64_t timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+
     ImageData img_data(
         img,
-        0,
+        timestamp,
         telemetry);
 
     return img_data;

src/core/mission_state.cpp

@@ -16,7 +16,21 @@
 #include "utilities/logging.hpp"
 #include "utilities/obc_config.hpp"
 
-MissionState::MissionState(OBCConfig config) : config(config) {}
+MissionState::MissionState(OBCConfig config) : config(config) {
+    this->matched_results = std::make_shared<MissionState::MatchedResults>();
+
+    AirdropTarget dummy;
+    GPSCoord* coord_in_dummy = dummy.mutable_coordinate();
+    coord_in_dummy->set_altitude(0.0);
+    coord_in_dummy->set_latitude(0.0);
+    coord_in_dummy->set_longitude(0.0);
+
+    dummy.set_index(AirdropType::Water);
+    this->matched_results->matched_airdrop[AirdropType::Water] = dummy;
+
+    dummy.set_index(AirdropType::Beacon);
+    this->matched_results->matched_airdrop[AirdropType::Beacon] = dummy;
+}
 
 // Need to explicitly define now that Tick is no longer an incomplete class
 // See:
@@ -79,6 +93,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;
@@ -121,6 +145,11 @@ std::shared_ptr<CVAggregator> MissionState::getCV() { return this->cv; }
 
 void MissionState::setCV(std::shared_ptr<CVAggregator> cv) { this->cv = cv; }
 
+LockPtr<MissionState::MatchedResults> MissionState::getMatchedResults() {
+    return LockPtr<MissionState::MatchedResults>(this->matched_results,
+                                                &this->matched_results_mut);
+}
+
 MissionState::CVStatus MissionState::getCVStatus() {
     Lock lock(this->cv_status_mut);
     return this->cv_status;