New Thread for Picture Taking

configs/dev.json

@@ -30,6 +30,10 @@
             "allowed_to_skip_waypoints": false,
             "generate_deviations": false
         },
+        "camera timing":{
+            "timeout":100,
+            "new_photo_time":300
+        },
         "coverage": {
             "altitude_m": 30.0,
             "camera_vision_m": 20.0,
@@ -62,6 +66,7 @@
         "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations",
         "type": "mock",
         "save_dir": "/workspaces/obcpp/images/mock",
+        "photo_delay":300,
         "save_images_to_file": true,
         "mock": {
             "not_stolen_port": 6060,

include/camera/picamera.hpp

@@ -56,6 +56,8 @@ class PiCamera : public CameraInterface {
                                          std::shared_ptr<MavlinkClient> mavlinkClient) override;
     void startStreaming() override;
 
+    bool getIsTakingPictures();
+
  private:
     /**
      * Takes an image and sleeps for the specified interval before

include/core/mission_state.hpp

@@ -56,6 +56,12 @@ class MissionState {
     void markAirdropAsDropped(AirdropType airdrop);
     std::unordered_set<AirdropType> getDroppedAirdrops();
 
+    void zoneHandler(const std::chrono::milliseconds& interval,
+                        std::shared_ptr<MavlinkClient> mavlinkClient);
+    void initCameraThread(const std::chrono::milliseconds& interval,
+                        std::shared_ptr<MavlinkClient> mavlinkClient);
+    void stopCameraThread();
+
     /*
      * Gets a locking reference to the underlying tick for the given tick subclass T.
      *
@@ -108,6 +114,9 @@ class MissionState {
     bool getMappingIsDone();
     void setMappingIsDone(bool isDone);
 
+
+
+
     MissionParameters mission_params;  // has its own mutex
 
     OBCConfig config;
@@ -136,13 +145,19 @@ class MissionState {
     std::shared_ptr<CVAggregator> cv;
     std::shared_ptr<CameraInterface> camera;
 
+    std::thread captureThread;
+    std::size_t curr_mission_item;
+
     std::mutex cv_mut;
     // Represents a single detected target used in pipeline
     std::vector<DetectedTarget> cv_detected_targets;
     // Gives an index into cv_detected_targets, and specifies that that bottle is matched
     // with the detected_target specified by the index
     std::array<size_t, NUM_AIRDROPS> cv_matches;
 
+    bool cameraThreadActive = true;
+
+
     bool mappingIsDone;
 
     void _setTick(Tick* newTick);  // does not acquire the tick_mut

include/utilities/obc_config.hpp

@@ -117,6 +117,8 @@ struct CameraConfig {
     std::string type;
     // directory to save images to
     std::string save_dir;
+    // time to wait before taking another picture
+    std::chrono::milliseconds photo_delay;
     // whether or not to save to save_dir
     bool save_images_to_file;
     struct {

src/core/mission_state.cpp

@@ -15,6 +15,11 @@
 #include "utilities/locks.hpp"
 #include "utilities/logging.hpp"
 #include "utilities/obc_config.hpp"
+#include "pathing/environment.hpp"
+#include "utilities/common.hpp"
+#include "ticks/ids.hpp"
+
+using namespace std::chrono_literals;
 
 MissionState::MissionState(OBCConfig config) : config(config) {}
 
@@ -94,6 +99,73 @@ void MissionState::setAirdropPath(const MissionPath& airdrop_path) {
     this->airdrop_path = airdrop_path;
 }
 
+
+
+void MissionState::zoneHandler(const std::chrono::milliseconds& interval,
+                        std::shared_ptr<MavlinkClient> mavlinkClient) {
+    std::chrono::milliseconds last_photo_time = getUnixTime_ms();
+    while (this->cameraThreadActive) {
+        auto [lat_deg, lng_deg] = this->getMav()->latlng_deg();
+        double altitude_agl_m = this->getMav()->altitude_agl_m();
+        GPSCoord current_pos = makeGPSCoord(lat_deg, lng_deg, altitude_agl_m);
+        auto converter = this->getCartesianConverter();
+        if (!converter) {
+            std::this_thread::sleep_for(interval);
+            continue;
+        }
+        XYZCoord current_xyz = converter->toXYZ(current_pos);
+        Polygon airdrop_boundary = this->mission_params.getAirdropBoundary();
+        bool in_zone = Environment::isPointInPolygon(airdrop_boundary, current_xyz);
+        if (!in_zone) {
+            std::this_thread::sleep_for(interval);
+            continue;
+        }
+        auto curr_waypoint = this->getMav()->curr_waypoint();
+        if (this->curr_mission_item != curr_waypoint) {
+            LOG_F(INFO, "FlySearch Area reached (%zu, %d)",
+                this->curr_mission_item, curr_waypoint);
+            for (int i = 0; i < this->config.pathing.coverage.hover.pictures_per_stop; i++) {
+                auto photo = this->getCamera()->takePicture(500ms, this->getMav());
+                if (this->config.camera.save_images_to_file) {
+                    photo->saveToFile(this->config.camera.save_dir);
+                }
+
+                if (photo.has_value()) {
+                    last_photo_time = getUnixTime_ms();
+                    this->getCV()->runPipeline(photo.value());
+                }
+            }
+            this->curr_mission_item = curr_waypoint;
+        }
+        auto now = getUnixTime_ms();
+        if ((now - last_photo_time) >= std::chrono::milliseconds(this->config.camera.photo_delay)) {
+            auto photo = this->getCamera()->takePicture(100ms, this->getMav());
+            if (this->config.camera.save_images_to_file) {
+                photo->saveToFile(this->config.camera.save_dir);
+            }
+
+            if (photo.has_value()&&((this->getTickID() == TickID::FlySearch)||
+                (this->getTickID() == TickID::CVLoiter))) {
+                this->getCV()->runPipeline(photo.value());
+            }
+            last_photo_time = getUnixTime_ms();
+        }
+        std::this_thread::sleep_for(interval);
+    }
+}
+void MissionState::initCameraThread(const std::chrono::milliseconds& interval,
+    std::shared_ptr<MavlinkClient> mavlinkClient) {
+    this->cameraThreadActive = true;
+    this->captureThread = std::thread([this, interval, mavlinkClient]() {
+        this->zoneHandler(interval, mavlinkClient);
+    });
+    this->curr_mission_item = 1;
+}
+void MissionState::stopCameraThread() {
+    this->captureThread.join();
+    this->cameraThreadActive = false;
+}
+
 MissionPath MissionState::getAirdropPath() {
     Lock lock(this->airdrop_path_mut);
     return this->airdrop_path;

src/ticks/auto_landing.cpp

@@ -13,5 +13,6 @@ std::chrono::milliseconds AutoLandingTick::getWait() const {
 }
 
 Tick* AutoLandingTick::tick() {
+    this->state->stopCameraThread();
     return nullptr;
 }

src/ticks/fly_search.cpp

@@ -43,6 +43,7 @@ void FlySearchTick::init() {
     LOG_F(INFO, "Total Waypoint #: %zu", this->state->getMav()->totalWaypoints());
 }
 
+
 Tick* FlySearchTick::tick() {
     if (!this->mission_started) {
         this->mission_started = this->state->getMav()->startMission();
@@ -56,32 +57,49 @@ Tick* FlySearchTick::tick() {
         // so we can just return a CVLoiterTick
         return new CVLoiterTick(this->state);
     }
+    /*
+    auto [lat_deg, lng_deg] = state->getMav()->latlng_deg();
+    double altitude_agl_m = state->getMav()->altitude_agl_m();
+    GPSCoord current_pos = makeGPSCoord(lat_deg, lng_deg, altitude_agl_m);
+    auto converter = state->getCartesianConverter();
 
     // IMPORTANT: currently hardcoded to assume hover search pathing, so it
     // takes photos whenever it gets to a new waypoint (loiter position)
     // if we were doing forward pathing would probably want to make it
     // take photos at an interval but only when over the zone
-    auto curr_waypoint = this->state->getMav()->curr_waypoint();
-
-    if (this->curr_mission_item != curr_waypoint) {
-    LOG_F(INFO, "FlySearch Area reached (%zu, %d)", this->curr_mission_item, curr_waypoint);
-        for (int i = 0; i < this->state->config.pathing.coverage.hover.pictures_per_stop; i++) {
-        auto photo = this->state->getCamera()->takePicture(500ms, this->state->getMav());
-            if (state->config.camera.save_images_to_file) {
-                photo->saveToFile(state->config.camera.save_dir);
-            }
-
-            if (photo.has_value()) {
-                // Update the last photo time
-                this->last_photo_time = getUnixTime_ms();
-                // Run the pipeline on the photo
-                this->state->getCV()->runPipeline(photo.value());
+    // Convert GPS to local XYZ coords
+    if (converter) {
+        XYZCoord current_xyz = converter->toXYZ(current_pos);
+        // Get the airdrop boundary polygon
+        Polygon airdrop_boundary = state->mission_params.getAirdropBoundary();
+        // Check if we're inside the airdrop zone
+        bool in_zone = Environment::isPointInPolygon(airdrop_boundary, current_xyz);
+        if (in_zone) {
+            auto curr_waypoint = this->state->getMav()->curr_waypoint();
+            if (this->curr_mission_item != curr_waypoint) {
+                LOG_F(INFO, "FlySearch Area reached (%zu, %d)",
+                    this->curr_mission_item, curr_waypoint);
+
+                for (int i = 0; i < this->state->config.pathing.coverage.hover.pictures_per_stop; i++) {
+                    auto photo = this->state->getCamera()->takePicture(500ms, this->state->getMav());
+                    if (state->config.camera.save_images_to_file) {
+                        photo->saveToFile(state->config.camera.save_dir);
+                    }
+
+                    if (photo.has_value()) {
+                        // Update the last photo time
+                        this->last_photo_time = getUnixTime_ms();
+                        // Run the pipeline on the photo
+                        this->state->getCV()->runPipeline(photo.value());
+                    }
+                }
+
+                this->curr_mission_item = curr_waypoint;
+
+                return nullptr;
             }
         }
-        this->curr_mission_item = curr_waypoint;
-
-        return nullptr;
     }
-
+    */
     return nullptr;
 }

src/ticks/fly_waypoints.cpp

@@ -24,6 +24,7 @@ std::chrono::milliseconds FlyWaypointsTick::getWait() const {
 }
 
 Tick* FlyWaypointsTick::tick() {
+    /*
     auto [lat_deg, lng_deg] = state->getMav()->latlng_deg();
     double altitude_agl_m = state->getMav()->altitude_agl_m();
     GPSCoord current_pos = makeGPSCoord(lat_deg, lng_deg, altitude_agl_m);
@@ -56,6 +57,7 @@ Tick* FlyWaypointsTick::tick() {
             // Stop camera/CV operations
         }
     }
+    */
 
     // TODO: Eventually implement dynamic avoidance so we dont crash brrr
     bool isMissionFinished = state->getMav()->isMissionFinished();
@@ -66,7 +68,6 @@ Tick* FlyWaypointsTick::tick() {
             state->getMav()->startMission();
             return nullptr;
         }
-
         return next_tick;
     }
 

src/ticks/manual_landing.cpp

@@ -15,6 +15,7 @@ std::chrono::milliseconds ManualLandingTick::getWait() const { return MANUAL_LAN
 
 Tick* ManualLandingTick::tick() {
     if (state->getDroppedAirdrops().size() >= NUM_AIRDROPS) {
+        state->stopCameraThread();
         if (state->getMappingIsDone() == false) {
             // Currently does a two pass at the end
             // Probably should update for next year to optimize it.

src/ticks/takeoff.cpp

@@ -8,6 +8,9 @@
 #include "ticks/mav_upload.hpp"
 #include "ticks/fly_search.hpp"
 
+
+using namespace std::chrono_literals;
+
 TakeoffTick::TakeoffTick(std::shared_ptr<MissionState> state)
     :Tick(state, TickID::Takeoff) {}
 
@@ -18,7 +21,7 @@ std::chrono::milliseconds TakeoffTick::getWait() const {
 Tick* TakeoffTick::tick() {
     if (state->getMav()->flight_mode() == mavsdk::Telemetry::FlightMode::Mission) {
         LOG_F(INFO, "Plane has entered autonomous");
-
+        this->state->initCameraThread(100ms, this->state->getMav());
         // 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