From 706e9b4975871fcb3147f1e62e4233825532e6a0 Mon Sep 17 00:00:00 2001 From: Kimi Chen Date: Fri, 8 May 2026 01:04:24 +0000 Subject: [PATCH 1/3] Safe aggregator termination --- include/cv/aggregator.hpp | 8 +++++- src/cv/aggregator.cpp | 57 ++++++++++++++++++++++++++++++++++---- src/network/gcs_routes.cpp | 28 +++++++++++-------- 3 files changed, 75 insertions(+), 18 deletions(-) diff --git a/include/cv/aggregator.hpp b/include/cv/aggregator.hpp index 003173a9..4169577d 100644 --- a/include/cv/aggregator.hpp +++ b/include/cv/aggregator.hpp @@ -3,15 +3,16 @@ #include #include +#include #include #include +#include #include #include #include #include #include #include -#include #include "cv/pipeline.hpp" #include "cv/utilities.hpp" @@ -42,6 +43,9 @@ class CVAggregator { // Spawn a thread to run the pipeline on the given imageData void runPipeline(const ImageData& image); + // Stop accepting work, discard queued images, and wait for active workers to finish + void terminate(); + // Lockable pointer to retrieve aggregator results LockPtr getResults(); @@ -67,6 +71,8 @@ class CVAggregator { std::mutex mut; int num_worker_threads; + bool terminating; + std::condition_variable workers_done_cv; // For when too many pipelines are active at once std::queue overflow_queue; diff --git a/src/cv/aggregator.cpp b/src/cv/aggregator.cpp index 1e61b098..f1c15935 100644 --- a/src/cv/aggregator.cpp +++ b/src/cv/aggregator.cpp @@ -1,5 +1,7 @@ #include "cv/aggregator.hpp" +#include + #include "utilities/constants.hpp" #include "utilities/lockptr.hpp" #include "utilities/locks.hpp" @@ -7,6 +9,7 @@ CVAggregator::CVAggregator(Pipeline&& p) : pipeline(std::move(p)) { this->num_worker_threads = 0; + this->terminating = false; this->results = std::make_shared(); this->matched_results = std::make_shared(); this->cv_record = std::make_shared>(); @@ -26,7 +29,7 @@ CVAggregator::CVAggregator(Pipeline&& p) : pipeline(std::move(p)) { this->matched_results->matched_airdrop[AirdropType::Beacon] = dummy; } -CVAggregator::~CVAggregator() {} +CVAggregator::~CVAggregator() { this->terminate(); } LockPtr CVAggregator::getResults() { return LockPtr(this->results, &this->mut); @@ -52,6 +55,11 @@ void CVAggregator::updateRecords(std::vector& new_values) { void CVAggregator::runPipeline(const ImageData& image) { Lock lock(this->mut); + if (this->terminating) { + LOG_F(WARNING, "Terminating CVAggregator. Rejecting new pipeline run."); + return; + } + if (this->num_worker_threads >= MAX_CV_PIPELINES) { // If we have too many running workers, just queue the new image LOG_F(WARNING, "Too many CVAggregator workers (%d). Pushing to overflow queue...", @@ -63,8 +71,35 @@ void CVAggregator::runPipeline(const ImageData& image) { static int thread_counter = 0; ++this->num_worker_threads; - std::thread worker_thread(&CVAggregator::worker, this, image, ++thread_counter); - worker_thread.detach(); // We don’t need to join in the caller + try { + std::thread worker_thread(&CVAggregator::worker, this, image, ++thread_counter); + worker_thread.detach(); + } catch (const std::exception& err) { + --this->num_worker_threads; + if (this->num_worker_threads == 0) { + this->workers_done_cv.notify_all(); + } + LOG_F(ERROR, "Failed to spawn CVAggregator worker: %s", err.what()); + } +} + +void CVAggregator::terminate() { + Lock lock(this->mut); + + if (this->terminating && this->num_worker_threads == 0) { + return; + } + + this->terminating = true; + const std::size_t dropped_images = this->overflow_queue.size(); + std::queue empty_queue; + this->overflow_queue.swap(empty_queue); + + LOG_F(INFO, + "Terminating CVAggregator. Dropped %zu queued images, waiting for %d active workers.", + dropped_images, this->num_worker_threads); + + this->workers_done_cv.wait(lock, [this] { return this->num_worker_threads == 0; }); } static std::atomic global_run_id{0}; @@ -74,6 +109,13 @@ void CVAggregator::worker(ImageData image, int thread_num) { LOG_F(INFO, "New CVAggregator worker #%d spawned.", thread_num); while (true) { + { + Lock lock(this->mut); + if (this->terminating) { + break; + } + } + // 1) Run the pipeline auto pipeline_results = this->pipeline.run(image); @@ -91,13 +133,15 @@ void CVAggregator::worker(ImageData image, int thread_num) { { Lock lock(this->mut); - this->results->runs.push_back(std::move(run)); + if (!this->terminating) { + this->results->runs.push_back(std::move(run)); + } } // 3) If no more queued images, break { Lock lock(this->mut); - if (this->overflow_queue.empty()) { + if (this->terminating || this->overflow_queue.empty()) { break; } image = std::move(this->overflow_queue.front()); @@ -111,6 +155,9 @@ void CVAggregator::worker(ImageData image, int thread_num) { LOG_F(INFO, "CVAggregator worker #%d terminating. Active threads: %d -> %d", thread_num, this->num_worker_threads, this->num_worker_threads - 1); --this->num_worker_threads; + if (this->num_worker_threads == 0) { + this->workers_done_cv.notify_all(); + } } } diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 0247f588..0c14549e 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -414,21 +414,25 @@ DEF_GCS_HANDLE(Post, targets, matched) { LOG_S(INFO) << j_root; - LockPtr matched_results = state->getCV()->getMatchedResults(); + state->getCV()->terminate(); - if (matched_results.data == nullptr) { - LOG_S(ERROR) << "lockptr is null"; - } + { + LockPtr matched_results = state->getCV()->getMatchedResults(); + + if (matched_results.data == nullptr) { + LOG_S(ERROR) << "lockptr is null"; + } - AirdropTarget returned_matched_result; + AirdropTarget returned_matched_result; - for (const auto& instance : j_root) { - LOG_S(INFO) << returned_matched_result.index(); - google::protobuf::util::JsonStringToMessage(instance.dump(), &returned_matched_result); - LOG_S(WARNING) << returned_matched_result.index(); - matched_results.data->matched_airdrop[returned_matched_result.index()] = - returned_matched_result; - LOG_S(ERROR) << returned_matched_result.index(); + for (const auto& instance : j_root) { + LOG_S(INFO) << returned_matched_result.index(); + google::protobuf::util::JsonStringToMessage(instance.dump(), &returned_matched_result); + LOG_S(WARNING) << returned_matched_result.index(); + matched_results.data->matched_airdrop[returned_matched_result.index()] = + returned_matched_result; + LOG_S(ERROR) << returned_matched_result.index(); + } } auto lock_ptr = state->getTickLockPtr(); From cda7b6abe0529016b371d9d4ba13aab1c364704d Mon Sep 17 00:00:00 2001 From: Kimi Chen Date: Fri, 8 May 2026 01:13:37 +0000 Subject: [PATCH 2/3] Remove CVLoiter check in targets/checked --- src/network/gcs_routes.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 0c14549e..829fbafd 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -436,11 +436,13 @@ DEF_GCS_HANDLE(Post, targets, matched) { } auto lock_ptr = state->getTickLockPtr(); - if (!lock_ptr.has_value()) { - LOG_RESPONSE(WARNING, "Not currently in Loiter Tick", BAD_REQUEST); - return; + if (lock_ptr.has_value()) { + lock_ptr->data->setStatus(CVLoiterTick::Status::Validated); + } else { + CVLoiterTick* cv_loiter_tick = new CVLoiterTick(state); + cv_loiter_tick->setStatus(CVLoiterTick::Status::Validated); + state->setTick(cv_loiter_tick); } - lock_ptr->data->setStatus(CVLoiterTick::Status::Validated); LOG_RESPONSE(INFO, "Finished setting targets (and thus loitering)", OK); } From eb44e3caa281cf7503fe40dd0c99cbf8518ddf06 Mon Sep 17 00:00:00 2001 From: Kimi Chen Date: Tue, 12 May 2026 15:08:25 -0700 Subject: [PATCH 3/3] Update aggregator termination and checking --- include/core/mission_state.hpp | 13 ++++++ include/cv/aggregator.hpp | 7 ++-- include/ticks/cv_loiter.hpp | 14 ------- src/core/mission_state.cpp | 10 +++++ src/cv/aggregator.cpp | 76 +++++++++++++++------------------- src/network/gcs_routes.cpp | 12 +----- src/ticks/cv_loiter.cpp | 12 +++--- src/ticks/fly_search.cpp | 11 ++++- 8 files changed, 75 insertions(+), 80 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 73f4af7d..10d92eb1 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -98,6 +98,15 @@ class MissionState { std::shared_ptr getCV(); void setCV(std::shared_ptr cv); + enum class CVStatus { + None = 0, + Validated = 1, + Rejected = 2, + }; + + CVStatus getCVStatus(); + void setCVStatus(CVStatus status); + /* * Gets a shared_ptr to the camera client, which lets you * take photos of ground targets. @@ -144,6 +153,10 @@ class MissionState { std::shared_ptr mav; std::shared_ptr airdrop; std::shared_ptr cv; + + std::mutex cv_status_mut; + CVStatus cv_status = CVStatus::None; + std::shared_ptr camera; std::mutex cv_mut; diff --git a/include/cv/aggregator.hpp b/include/cv/aggregator.hpp index 4169577d..a11d146f 100644 --- a/include/cv/aggregator.hpp +++ b/include/cv/aggregator.hpp @@ -3,7 +3,6 @@ #include #include -#include #include #include #include @@ -70,9 +69,9 @@ class CVAggregator { Pipeline pipeline; std::mutex mut; - int num_worker_threads; - bool terminating; - std::condition_variable workers_done_cv; + std::atomic num_worker_threads; + std::atomic accepting_images; + std::vector worker_threads; // For when too many pipelines are active at once std::queue overflow_queue; diff --git a/include/ticks/cv_loiter.hpp b/include/ticks/cv_loiter.hpp index 4794cc58..c3e53829 100644 --- a/include/ticks/cv_loiter.hpp +++ b/include/ticks/cv_loiter.hpp @@ -22,24 +22,10 @@ class CVLoiterTick : public Tick { public: - /* - * Status of the CV processing - * (Will be) Modified through GCS Manual Control - */ - enum class Status { - None = 0, - Validated = 1, - Rejected = 2, - }; - explicit CVLoiterTick(std::shared_ptr state); std::chrono::milliseconds getWait() const override; - void setStatus(Status status); Tick* tick() override; - - private: - Status status; }; #endif // INCLUDE_TICKS_CV_LOITER_HPP_ diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 5bee1d7d..38e14021 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -121,6 +121,16 @@ std::shared_ptr MissionState::getCV() { return this->cv; } void MissionState::setCV(std::shared_ptr cv) { this->cv = cv; } +MissionState::CVStatus MissionState::getCVStatus() { + Lock lock(this->cv_status_mut); + return this->cv_status; +} + +void MissionState::setCVStatus(CVStatus status) { + Lock lock(this->cv_status_mut); + this->cv_status = status; +} + std::shared_ptr MissionState::getCamera() { return this->camera; } void MissionState::setCamera(std::shared_ptr camera) { this->camera = camera; } diff --git a/src/cv/aggregator.cpp b/src/cv/aggregator.cpp index f1c15935..bfce6146 100644 --- a/src/cv/aggregator.cpp +++ b/src/cv/aggregator.cpp @@ -8,8 +8,8 @@ #include "utilities/logging.hpp" CVAggregator::CVAggregator(Pipeline&& p) : pipeline(std::move(p)) { - this->num_worker_threads = 0; - this->terminating = false; + this->num_worker_threads.store(0); + this->accepting_images.store(true); this->results = std::make_shared(); this->matched_results = std::make_shared(); this->cv_record = std::make_shared>(); @@ -55,51 +55,54 @@ void CVAggregator::updateRecords(std::vector& new_values) { void CVAggregator::runPipeline(const ImageData& image) { Lock lock(this->mut); - if (this->terminating) { - LOG_F(WARNING, "Terminating CVAggregator. Rejecting new pipeline run."); + if (!this->accepting_images.load()) { + LOG_F(WARNING, "CVAggregator is not accepting new images. Dropping pipeline run."); return; } - if (this->num_worker_threads >= MAX_CV_PIPELINES) { + const int active_workers = this->num_worker_threads.load(); + if (active_workers >= MAX_CV_PIPELINES) { // If we have too many running workers, just queue the new image LOG_F(WARNING, "Too many CVAggregator workers (%d). Pushing to overflow queue...", - this->num_worker_threads); + active_workers); this->overflow_queue.push(image); LOG_F(WARNING, "Overflow queue size is now %ld", this->overflow_queue.size()); return; } static int thread_counter = 0; - ++this->num_worker_threads; + this->num_worker_threads.fetch_add(1); try { - std::thread worker_thread(&CVAggregator::worker, this, image, ++thread_counter); - worker_thread.detach(); + this->worker_threads.emplace_back(&CVAggregator::worker, this, image, ++thread_counter); } catch (const std::exception& err) { - --this->num_worker_threads; - if (this->num_worker_threads == 0) { - this->workers_done_cv.notify_all(); - } + this->num_worker_threads.fetch_sub(1); LOG_F(ERROR, "Failed to spawn CVAggregator worker: %s", err.what()); } } void CVAggregator::terminate() { - Lock lock(this->mut); - - if (this->terminating && this->num_worker_threads == 0) { - return; - } + std::vector threads; + { + Lock lock(this->mut); + this->accepting_images.store(false); + const std::size_t dropped_images = this->overflow_queue.size(); + std::queue empty_queue; + this->overflow_queue.swap(empty_queue); - this->terminating = true; - const std::size_t dropped_images = this->overflow_queue.size(); - std::queue empty_queue; - this->overflow_queue.swap(empty_queue); + if (this->worker_threads.empty()) { + return; + } - LOG_F(INFO, - "Terminating CVAggregator. Dropped %zu queued images, waiting for %d active workers.", - dropped_images, this->num_worker_threads); + LOG_F(INFO, "Dropped %zu queued images. Waiting for %zu CVAggregator worker threads.", + dropped_images, this->worker_threads.size()); + threads.swap(this->worker_threads); + } - this->workers_done_cv.wait(lock, [this] { return this->num_worker_threads == 0; }); + for (std::thread& worker_thread : threads) { + if (worker_thread.joinable()) { + worker_thread.join(); + } + } } static std::atomic global_run_id{0}; @@ -109,13 +112,6 @@ void CVAggregator::worker(ImageData image, int thread_num) { LOG_F(INFO, "New CVAggregator worker #%d spawned.", thread_num); while (true) { - { - Lock lock(this->mut); - if (this->terminating) { - break; - } - } - // 1) Run the pipeline auto pipeline_results = this->pipeline.run(image); @@ -133,15 +129,13 @@ void CVAggregator::worker(ImageData image, int thread_num) { { Lock lock(this->mut); - if (!this->terminating) { - this->results->runs.push_back(std::move(run)); - } + this->results->runs.push_back(std::move(run)); } // 3) If no more queued images, break { Lock lock(this->mut); - if (this->terminating || this->overflow_queue.empty()) { + if (this->overflow_queue.empty()) { break; } image = std::move(this->overflow_queue.front()); @@ -151,13 +145,9 @@ void CVAggregator::worker(ImageData image, int thread_num) { // 4) Mark ourselves as finished { - Lock lock(this->mut); + const int active_workers = this->num_worker_threads.fetch_sub(1); LOG_F(INFO, "CVAggregator worker #%d terminating. Active threads: %d -> %d", thread_num, - this->num_worker_threads, this->num_worker_threads - 1); - --this->num_worker_threads; - if (this->num_worker_threads == 0) { - this->workers_done_cv.notify_all(); - } + active_workers, active_workers - 1); } } diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 829fbafd..32f4054a 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -7,7 +7,6 @@ #include #include - #include #include "core/mission_state.hpp" @@ -16,7 +15,6 @@ #include "pathing/mission_path.hpp" #include "protos/obc.pb.h" #include "ticks/airdrop_approach.hpp" -#include "ticks/cv_loiter.hpp" #include "ticks/path_gen.hpp" #include "ticks/path_validate.hpp" #include "ticks/tick.hpp" @@ -421,6 +419,7 @@ DEF_GCS_HANDLE(Post, targets, matched) { if (matched_results.data == nullptr) { LOG_S(ERROR) << "lockptr is null"; + return; } AirdropTarget returned_matched_result; @@ -435,14 +434,7 @@ DEF_GCS_HANDLE(Post, targets, matched) { } } - auto lock_ptr = state->getTickLockPtr(); - if (lock_ptr.has_value()) { - lock_ptr->data->setStatus(CVLoiterTick::Status::Validated); - } else { - CVLoiterTick* cv_loiter_tick = new CVLoiterTick(state); - cv_loiter_tick->setStatus(CVLoiterTick::Status::Validated); - state->setTick(cv_loiter_tick); - } + state->setCVStatus(MissionState::CVStatus::Validated); LOG_RESPONSE(INFO, "Finished setting targets (and thus loitering)", OK); } diff --git a/src/ticks/cv_loiter.cpp b/src/ticks/cv_loiter.cpp index ccd6d4e4..f29bd349 100644 --- a/src/ticks/cv_loiter.cpp +++ b/src/ticks/cv_loiter.cpp @@ -7,14 +7,10 @@ #include "ticks/ids.hpp" #include "utilities/constants.hpp" -CVLoiterTick::CVLoiterTick(std::shared_ptr state) : Tick(state, TickID::CVLoiter) { - this->status = CVLoiterTick::Status::None; -} +CVLoiterTick::CVLoiterTick(std::shared_ptr state) : Tick(state, TickID::CVLoiter) {} std::chrono::milliseconds CVLoiterTick::getWait() const { return CV_LOITER_TICK_WAIT; } -void CVLoiterTick::setStatus(Status status) { this->status = status; } - Tick* CVLoiterTick::tick() { // Tick is called if Search Zone coverage path is finished @@ -32,8 +28,10 @@ Tick* CVLoiterTick::tick() { // } // } + MissionState::CVStatus status = this->state->getCVStatus(); + // Check status of the CV Results - if (status == Status::Validated) { + if (status == MissionState::CVStatus::Validated) { /* const std::array ALL_AIRDROPS = { AirdropType::Water, AirdropType::Beacon, @@ -74,7 +72,7 @@ Tick* CVLoiterTick::tick() { // } return new AirdropPrepTick(this->state); - } else if (status == Status::Rejected) { + } else if (status == MissionState::CVStatus::Rejected) { // TODO: Tell Mav to restart Search Mission return new FlySearchTick(this->state); } diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 685f0bff..1d733d3c 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -3,10 +3,11 @@ #include #include +#include "pathing/environment.hpp" +#include "ticks/airdrop_prep.hpp" +#include "ticks/cv_loiter.hpp" #include "ticks/ids.hpp" #include "utilities/common.hpp" -#include "ticks/cv_loiter.hpp" -#include "pathing/environment.hpp" using namespace std::chrono_literals; // NOLINT @@ -44,6 +45,12 @@ void FlySearchTick::init() { } Tick* FlySearchTick::tick() { + MissionState::CVStatus status = this->state->getCVStatus(); + if (status == MissionState::CVStatus::Validated) { + this->state->setCVStatus(MissionState::CVStatus::None); + return new AirdropPrepTick(this->state); + } + if (!this->mission_started) { this->mission_started = this->state->getMav()->startMission(); return nullptr;