diff --git a/configs/dev.json b/configs/dev.json index e55ddd1f..ea7b4db2 100644 --- a/configs/dev.json +++ b/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, diff --git a/include/camera/picamera.hpp b/include/camera/picamera.hpp index 5e31877d..20b5dcf1 100644 --- a/include/camera/picamera.hpp +++ b/include/camera/picamera.hpp @@ -56,6 +56,8 @@ class PiCamera : public CameraInterface { std::shared_ptr mavlinkClient) override; void startStreaming() override; + bool getIsTakingPictures(); + private: /** * Takes an image and sleeps for the specified interval before diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index f97de9b8..3298a1f1 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -56,6 +56,12 @@ class MissionState { void markAirdropAsDropped(AirdropType airdrop); std::unordered_set getDroppedAirdrops(); + void zoneHandler(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient); + void initCameraThread(const std::chrono::milliseconds& interval, + std::shared_ptr 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,6 +145,9 @@ class MissionState { std::shared_ptr cv; std::shared_ptr camera; + std::thread captureThread; + std::size_t curr_mission_item; + std::mutex cv_mut; // Represents a single detected target used in pipeline std::vector cv_detected_targets; @@ -143,6 +155,9 @@ class MissionState { // with the detected_target specified by the index std::array cv_matches; + bool cameraThreadActive = true; + + bool mappingIsDone; void _setTick(Tick* newTick); // does not acquire the tick_mut diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 186555a1..b7e0a022 100644 --- a/include/utilities/obc_config.hpp +++ b/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 { diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index bde2b23e..1108b3b4 100644 --- a/src/core/mission_state.cpp +++ b/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) { + 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) { + 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; diff --git a/src/ticks/auto_landing.cpp b/src/ticks/auto_landing.cpp index be96bbd1..60722471 100644 --- a/src/ticks/auto_landing.cpp +++ b/src/ticks/auto_landing.cpp @@ -13,5 +13,6 @@ std::chrono::milliseconds AutoLandingTick::getWait() const { } Tick* AutoLandingTick::tick() { + this->state->stopCameraThread(); return nullptr; } diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 685f0bff..04d90e51 100644 --- a/src/ticks/fly_search.cpp +++ b/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; } diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index dc66b1a2..93fb9b5e 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/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; } diff --git a/src/ticks/manual_landing.cpp b/src/ticks/manual_landing.cpp index 110a3aa0..a45fb182 100644 --- a/src/ticks/manual_landing.cpp +++ b/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. diff --git a/src/ticks/takeoff.cpp b/src/ticks/takeoff.cpp index 4cfe0560..cf484f63 100644 --- a/src/ticks/takeoff.cpp +++ b/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 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