From 09d2c6990104ed64c47e2d73568c830fb950dd31 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 23:15:44 +0000 Subject: [PATCH 1/9] wip Signed-off-by: Charlie Huang --- src/camera/camera_source.cc | 13 +++- src/camera/camera_source.h | 2 +- src/camera/multi_camera_source.cc | 38 ++++----- src/camera/multi_camera_source.h | 2 +- src/camera/select_camera.cc | 2 +- src/camera/uvc_camera.cc | 78 ++++++++++--------- src/localization/run_localization.cc | 5 +- src/localization/run_localization.h | 7 +- src/main_bot_main.cc | 24 +++--- src/second_bot_main.cc | 28 +++---- .../integration_test/localization_test.cc | 3 +- src/utils/stop.h | 47 +++++++++++ 12 files changed, 148 insertions(+), 101 deletions(-) create mode 100644 src/utils/stop.h diff --git a/src/camera/camera_source.cc b/src/camera/camera_source.cc index 8c35e0ad..7e78522c 100644 --- a/src/camera/camera_source.cc +++ b/src/camera/camera_source.cc @@ -8,8 +8,8 @@ CameraSource::CameraSource(std::string name, std::unique_ptr camera, camera_(std::move(camera)), simulation_(simulation) { timestamped_frame_ = camera_->GetFrame(); - thread_ = std::thread([this] { - while (true) { + thread_ = std::jthread([this](const std::stop_token& stop_token) { + while (!stop_token.stop_requested()) { if (camera_->IsDone()) { exit(0); return; @@ -25,14 +25,19 @@ CameraSource::CameraSource(std::string name, std::unique_ptr camera, auto CameraSource::Get() -> timestamped_frame_t { mutex_.lock(); - timestamped_frame_t timestamped_frame = timestamped_frame_; + cv::Mat frame; + timestamped_frame_t timestamped_frame{ + .frame = timestamped_frame_.frame.clone(), + .timestamp = timestamped_frame_.timestamp, + .invalid = timestamped_frame_.invalid}; mutex_.unlock(); if (!simulation_ && !camera_->IsDone()) { auto current_time = frc::Timer::GetFPGATimestamp().to(); if (current_time - timestamped_frame.timestamp > 5.0) { LOG(INFO) << "Restarting camera because of old timestamp"; - timestamped_frame_.timestamp = current_time; mutex_.lock(); + timestamped_frame_.timestamp = + current_time; // 5 second buffer to get frame camera frames again camera_->Restart(); mutex_.unlock(); } diff --git a/src/camera/camera_source.h b/src/camera/camera_source.h index ed1fb7d2..b24a8281 100644 --- a/src/camera/camera_source.h +++ b/src/camera/camera_source.h @@ -18,7 +18,7 @@ class CameraSource { std::string name_; std::unique_ptr camera_; timestamped_frame_t timestamped_frame_; - std::thread thread_; + std::jthread thread_; std::mutex mutex_; const bool simulation_; }; diff --git a/src/camera/multi_camera_source.cc b/src/camera/multi_camera_source.cc index 841c23c4..876876ef 100644 --- a/src/camera/multi_camera_source.cc +++ b/src/camera/multi_camera_source.cc @@ -10,25 +10,27 @@ MultiCameraSource::MultiCameraSource( use_all_frames_(use_all_frames) { camera_threads_.reserve(cameras_.size()); for (size_t i = 0; i < cameras_.size(); i++) { - camera_threads_.emplace_back([this, i]() -> void { - while (true) { - if (use_all_frames_) { - mutex_.lock(); - bool frames_used = frames_used_; - mutex_.unlock(); - if (!frames_used) { - std::this_thread::sleep_for(std::chrono::duration(0.002)); - continue; + camera_threads_.emplace_back( + [this, i](const std::stop_token& stop_token) -> void { + while (true) { + if (use_all_frames_) { + mutex_.lock(); + bool frames_used = frames_used_; + mutex_.unlock(); + if (!frames_used) { + std::this_thread::sleep_for( + std::chrono::duration(0.002)); + continue; + } + } + timestamped_frame_t timestamped_frame; + timestamped_frame = cameras_[i]->GetFrame(); + mutex_.lock(); + timestamped_frames_[i] = timestamped_frame; + frames_used_ = false; + mutex_.unlock(); } - } - timestamped_frame_t timestamped_frame; - timestamped_frame = cameras_[i]->GetFrame(); - mutex_.lock(); - timestamped_frames_[i] = timestamped_frame; - frames_used_ = false; - mutex_.unlock(); - } - }); + }); } } diff --git a/src/camera/multi_camera_source.h b/src/camera/multi_camera_source.h index db665913..4452c24b 100644 --- a/src/camera/multi_camera_source.h +++ b/src/camera/multi_camera_source.h @@ -20,7 +20,7 @@ class MultiCameraSource { std::vector invalid_frame_start_times_; // time since the frame has not been old std::vector timestamped_frames_; - std::vector camera_threads_; + std::vector camera_threads_; std::mutex mutex_; const bool use_all_frames_; bool frames_used_ = true; // only for when use_all_frames_ is true diff --git a/src/camera/select_camera.cc b/src/camera/select_camera.cc index fdadf53f..a895352f 100644 --- a/src/camera/select_camera.cc +++ b/src/camera/select_camera.cc @@ -53,7 +53,7 @@ auto SelectCameraConfig(const std::string& choice, auto camera = std::make_unique(camera_constants.at(choice), status); if (!status.ok()) { - LOG(WARNING) << "Failed to select camera via uvc: " << status.message(); + LOG(FATAL) << "Failed to select camera via uvc: " << status.message(); } return camera; } diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index bd391c71..c75e4d4d 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -11,47 +11,51 @@ const cv::Mat UVCCamera::backup_image_ = void callback(uvc_frame_t* frame, void* ptr) { auto ptr_ = static_cast(ptr); - ptr_->mutex_.lock(); - switch (frame->frame_format) { - case UVC_COLOR_FORMAT_MJPEG: { - char* data = static_cast(frame->data); - std::vector buffer(data, data + frame->data_bytes); - ptr_->frame_buffer.frame = cv::imdecode(buffer, UVCCamera::read_type); - break; - } - case UVC_COLOR_FORMAT_YUYV: { - uvc_frame_t* bgr = uvc_allocate_frame(frame->width * frame->height * 3); - if (!bgr) { - LOG(WARNING) << "Camera " << ptr_->camera_constant_.name - << " failed to allocate "; + if (ptr_->mutex_.try_lock()) { + switch (frame->frame_format) { + case UVC_COLOR_FORMAT_MJPEG: { + char* data = static_cast(frame->data); + std::vector buffer(data, data + frame->data_bytes); + ptr_->frame_buffer.frame = cv::imdecode(buffer, UVCCamera::read_type); + break; } - uvc_error_t ret = uvc_yuyv2bgr(frame, bgr); - if (ret != 0) { - LOG(WARNING) << "YUYV failed to convert to BGR"; + case UVC_COLOR_FORMAT_YUYV: { + uvc_frame_t* bgr = uvc_allocate_frame(frame->width * frame->height * 3); + if (!bgr) { + LOG(WARNING) << "Camera " << ptr_->camera_constant_.name + << " failed to allocate "; + ptr_->mutex_.unlock(); + return; + } + uvc_error_t ret = uvc_yuyv2bgr(frame, bgr); + if (ret != 0) { + LOG(WARNING) << "YUYV failed to convert to BGR"; + } + IplImage* ipl_image; + ipl_image = cvCreateImageHeader(cvSize(bgr->width, bgr->height), + IPL_DEPTH_8U, 3); + cvSetData(ipl_image, bgr->data, bgr->width * 3); + ptr_->frame_buffer.frame = cv::cvarrToMat(ipl_image, true); + uvc_free_frame(bgr); + break; } - IplImage* ipl_image; - ipl_image = - cvCreateImageHeader(cvSize(bgr->width, bgr->height), IPL_DEPTH_8U, 3); - cvSetData(ipl_image, bgr->data, bgr->width * 3); - ptr_->frame_buffer.frame = cv::cvarrToMat(ipl_image, true); - uvc_free_frame(bgr); - break; + default: + LOG(WARNING) << "Unknown frame format"; + break; } - default: - LOG(WARNING) << "Unknown frame format"; - break; - } - if (ptr_->frame_buffer.frame.empty()) { - LOG(WARNING) << "Failed to decode frame from camera " - << ptr_->camera_constant_.name; - return; + if (ptr_->frame_buffer.frame.empty()) { + LOG(WARNING) << "Failed to decode frame from camera " + << ptr_->camera_constant_.name; + ptr_->mutex_.unlock(); + return; + } + ptr_->frame_buffer.invalid = false; + ptr_->frame_buffer.timestamp = + frc::Timer::GetFPGATimestamp() + .to(); // TODO: Use more accurate timestamp + ptr_->frame_index_ = frame->sequence; + ptr_->mutex_.unlock(); } - ptr_->frame_buffer.invalid = false; - ptr_->frame_buffer.timestamp = - frc::Timer::GetFPGATimestamp() - .to(); // TODO: Use more accurate timestamp - ptr_->frame_index_ = frame->sequence; - ptr_->mutex_.unlock(); } UVCCamera::UVCCamera(const CameraConstant& camera_constant, diff --git a/src/localization/run_localization.cc b/src/localization/run_localization.cc index ce8763d2..18c1d2f3 100644 --- a/src/localization/run_localization.cc +++ b/src/localization/run_localization.cc @@ -20,11 +20,12 @@ namespace localization { // TODO remove extrinsics void RunLocalization( + const std::stop_token& stop_token, std::unique_ptr source, std::unique_ptr detector, std::unique_ptr solver, std::vector> senders, - const std::string& extrinsics, std::optional port, bool verbose) { + std::optional port, bool verbose) { std::optional streamer = port.has_value() ? std::make_optional(camera::CscoreStreamer( @@ -55,7 +56,7 @@ void RunLocalizationSimulation( camera::CameraSource& source, std::unique_ptr detector, std::unique_ptr solver, - const std::string& extrinsics, std::optional port, bool verbose) { + std::optional port, bool verbose) { std::error_code ec; auto log = std::make_unique("multitag2.wpilog", ec); if (ec) { diff --git a/src/localization/run_localization.h b/src/localization/run_localization.h index ac37f9af..f58565a3 100644 --- a/src/localization/run_localization.h +++ b/src/localization/run_localization.h @@ -22,16 +22,15 @@ namespace localization { // .extrinsics_path, // 5801, false); void RunLocalization( + const std::stop_token& stop_token, std::unique_ptr source, std::unique_ptr detector, std::unique_ptr solver, std::vector> sender, - const std::string& extrinsics, std::optional port = std::nullopt, - bool verbose = false); + std::optional port = std::nullopt, bool verbose = false); void RunLocalizationSimulation( camera::CameraSource& source, std::unique_ptr detector, std::unique_ptr solver, - const std::string& extrinsics, std::optional port = std::nullopt, - bool verbose = false); + std::optional port = std::nullopt, bool verbose = false); } // namespace localization diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc index 3cd75110..f07c2462 100644 --- a/src/main_bot_main.cc +++ b/src/main_bot_main.cc @@ -11,6 +11,7 @@ #include "src/pathing/controller.h" #include "src/utils/camera_utils.h" #include "src/utils/nt_utils.h" +#include "src/utils/stop.h" using camera::camera_constants_t; auto main() -> int { @@ -22,7 +23,7 @@ auto main() -> int { LOG(INFO) << "Starting estimators"; - std::thread left_thread([&]() { + std::thread left_thread([&](const std::stop_token& stop_token) { auto left_camera = std::make_unique( "Left", std::make_unique(camera_constants.at("main_bot_left"), @@ -34,19 +35,17 @@ auto main() -> int { camera_constants.at("main_bot_left").name)); localization::RunLocalization( - std::move(left_camera), + stop_token, std::move(left_camera), std::make_unique( left_camera_frame.cols, left_camera_frame.rows, utils::ReadIntrinsics( camera_constants.at("main_bot_left").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_left")), - std::move(left_sender), - camera_constants.at("main_bot_left").extrinsics_path.value(), 5802, - false); + std::move(left_sender), 5802, false); }); - std::thread right_thread([&]() { + std::thread right_thread([&](const std::stop_token& stop_token) { auto right_camera = std::make_unique( "Right", std::make_unique( camera_constants.at("main_bot_right"), @@ -57,18 +56,16 @@ auto main() -> int { right_sender.emplace_back( std::make_unique( camera_constants.at("main_bot_right").name)); - + // const std::stop_token& stop_token localization::RunLocalization( - std::move(right_camera), + stop_token, std::move(right_camera), std::make_unique( right_camera_frame.cols, right_camera_frame.rows, utils::ReadIntrinsics( camera_constants.at("main_bot_right").intrinsics_path.value())), std::make_unique( camera_constants.at("main_bot_right")), - std::move(right_sender), - camera_constants.at("main_bot_right").extrinsics_path.value(), 5803, - false); + std::move(right_sender), 5803, false); }); LOG(INFO) << "Started estimators"; @@ -78,8 +75,5 @@ auto main() -> int { LOG(INFO) << "pathing started"; - // TODO find better way - left_thread.join(); - right_thread.join(); - pathing_thread.join(); + stop::WaitUntilStop(); } diff --git a/src/second_bot_main.cc b/src/second_bot_main.cc index 082679ea..1296452d 100644 --- a/src/second_bot_main.cc +++ b/src/second_bot_main.cc @@ -10,6 +10,9 @@ #include "src/pathing/pathfinding.h" #include "src/utils/camera_utils.h" #include "src/utils/nt_utils.h" +#include "src/utils/stop.h" + +#include using camera::camera_constants_t; auto main() -> int { @@ -20,7 +23,7 @@ auto main() -> int { LOG(INFO) << "Starting estimators"; - std::thread left_thread([&]() { + std::jthread left_thread([&](const std::stop_token& stop_token) { auto left_camera = std::make_unique( "Left", std::make_unique( camera_constants.at("second_bot_left"), @@ -32,19 +35,17 @@ auto main() -> int { camera_constants.at("second_bot_left").name)); localization::RunLocalization( - std::move(left_camera), + stop_token, std::move(left_camera), std::make_unique( left_camera_frame.cols, left_camera_frame.rows, utils::ReadIntrinsics(camera_constants.at("second_bot_left") .intrinsics_path.value())), std::make_unique( camera_constants.at("second_bot_left")), - std::move(left_sender), - camera_constants.at("second_bot_left").extrinsics_path.value(), 5802, - false); + std::move(left_sender), 5802, false); }); - std::thread right_thread([&]() { + std::jthread right_thread([&](const std::stop_token& stop_token) { auto right_camera = std::make_unique( "Right", std::make_unique( camera_constants.at("second_bot_right"), @@ -57,25 +58,20 @@ auto main() -> int { camera_constants.at("second_bot_right").name)); localization::RunLocalization( - std::move(right_camera), + stop_token, std::move(right_camera), std::make_unique( right_camera_frame.cols, right_camera_frame.rows, utils::ReadIntrinsics(camera_constants.at("second_bot_right") .intrinsics_path.value())), std::make_unique( camera_constants.at("second_bot_right")), - std::move(right_sender), - camera_constants.at("second_bot_right").extrinsics_path.value(), 5803, - false); + std::move(right_sender), 5803, false); }); - // TODO front camera - - std::thread pathing_thread(pathing::RunController, - "/bos/constants/navgrid.json", false); + std::jthread pathing_thread(pathing::RunController, + "/bos/constants/navgrid.json", false); LOG(INFO) << "Started estimators"; - left_thread.join(); - pathing_thread.join(); + stop::WaitUntilStop(); } diff --git a/src/test/integration_test/localization_test.cc b/src/test/integration_test/localization_test.cc index 80555fcc..14951b3a 100644 --- a/src/test/integration_test/localization_test.cc +++ b/src/test/integration_test/localization_test.cc @@ -145,8 +145,7 @@ auto main(int argc, char** argv) -> int { frame.cols, frame.rows, utils::ReadIntrinsics(camera_constant.intrinsics_path.value())), std::make_unique(camera_constant), - std::move(senders), camera_constant.extrinsics_path.value(), - base_port + i, true); + std::move(senders), base_port + i, true); }); } diff --git a/src/utils/stop.h b/src/utils/stop.h new file mode 100644 index 00000000..bf374a8a --- /dev/null +++ b/src/utils/stop.h @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include +#include +#include "src/utils/log.h" + +namespace stop { + +using namespace std::literals::chrono_literals; + +constexpr std::chrono::seconds kwait_interval = 1s; +std::atomic stop(false); +std::atomic registered_handler(false); + +void SignalHander(int signal) { + LOG(INFO) << "Received signal: " << signal; + stop = true; +} + +void RegisterHandler() { + if (registered_handler) { + LOG(WARNING) << "Handler has already been registred"; + return; + } + std::signal(SIGINT, SignalHander); + std::signal(SIGILL, SignalHander); + std::signal(SIGABRT, SignalHander); + std::signal(SIGFPE, SignalHander); + std::signal(SIGSEGV, SignalHander); + std::signal(SIGTERM, SignalHander); + std::signal(SIGHUP, SignalHander); + std::signal(SIGQUIT, SignalHander); + // std::signal(SIGTRAP, SignalHander); + std::signal(SIGKILL, SignalHander); + // std::signal(SIGPIPE, SignalHander); + // std::signal(SIGALRM, SignalHander); + registered_handler = true; +} + +void WaitUntilStop() { + while (!stop) { + std::this_thread::sleep_for(stop::kwait_interval); + } +} +} // namespace stop From 2add22ef42ef3ea0d182c8e242a43fcb55408f45 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 23:24:06 +0000 Subject: [PATCH 2/9] wip Signed-off-by: Charlie Huang --- scripts/build.sh | 4 ---- scripts/copy_constants.sh | 4 ++++ src/main_bot_main.cc | 8 ++++---- src/test/integration_test/localization_test.cc | 11 +++++------ 4 files changed, 13 insertions(+), 14 deletions(-) create mode 100755 scripts/copy_constants.sh diff --git a/scripts/build.sh b/scripts/build.sh index 7c5446f9..9721518e 100755 --- a/scripts/build.sh +++ b/scripts/build.sh @@ -22,9 +22,5 @@ else fi git submodule update --init --progress --depth 1 -if [ "$(pwd)" != "/bos" ]; then - mkdir -p /bos - sudo cp -r constants /bos -fi cmake -DENABLE_CLANG_TIDY=OFF -DCMAKE_BUILD_TYPE=Release -B "$BUILD_DIR" -G Ninja . cmake --build "$BUILD_DIR" diff --git a/scripts/copy_constants.sh b/scripts/copy_constants.sh new file mode 100755 index 00000000..07f4baee --- /dev/null +++ b/scripts/copy_constants.sh @@ -0,0 +1,4 @@ +if [ "$(pwd)" != "/bos" ]; then + mkdir -p /bos + sudo cp -r constants /bos +fi diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc index f07c2462..00ffdf42 100644 --- a/src/main_bot_main.cc +++ b/src/main_bot_main.cc @@ -23,7 +23,7 @@ auto main() -> int { LOG(INFO) << "Starting estimators"; - std::thread left_thread([&](const std::stop_token& stop_token) { + std::jthread left_thread([&](const std::stop_token& stop_token) { auto left_camera = std::make_unique( "Left", std::make_unique(camera_constants.at("main_bot_left"), @@ -45,7 +45,7 @@ auto main() -> int { std::move(left_sender), 5802, false); }); - std::thread right_thread([&](const std::stop_token& stop_token) { + std::jthread right_thread([&](const std::stop_token& stop_token) { auto right_camera = std::make_unique( "Right", std::make_unique( camera_constants.at("main_bot_right"), @@ -70,8 +70,8 @@ auto main() -> int { LOG(INFO) << "Started estimators"; - std::thread pathing_thread(pathing::RunController, - "/bos/constants/navgrid.json", false); + std::jthread pathing_thread(pathing::RunController, + "/bos/constants/navgrid.json", false); LOG(INFO) << "pathing started"; diff --git a/src/test/integration_test/localization_test.cc b/src/test/integration_test/localization_test.cc index 14951b3a..0fb71eb2 100644 --- a/src/test/integration_test/localization_test.cc +++ b/src/test/integration_test/localization_test.cc @@ -103,7 +103,7 @@ auto main(int argc, char** argv) -> int { LOG(FATAL) << "--camera_name may only be used with a single camera folder"; } - std::vector localization_threads; + std::vector localization_threads; localization_threads.reserve(camera_folders.size()); const int base_port = absl::GetFlag(FLAGS_port); @@ -125,7 +125,8 @@ auto main(int argc, char** argv) -> int { } localization_threads.emplace_back([camera_name, camera_folder, speed, - constants, base_port, i] { + constants, base_port, + i](const std::stop_token& stop_token) { const auto& camera_constant = constants.at(camera_name); auto camera_source = std::make_unique( camera_name, std::make_unique( @@ -140,7 +141,7 @@ auto main(int argc, char** argv) -> int { senders.emplace_back(std::make_unique( camera_name, base_port + i - 1000)); localization::RunLocalization( - std::move(camera_source), + stop_token, std::move(camera_source), std::make_unique( frame.cols, frame.rows, utils::ReadIntrinsics(camera_constant.intrinsics_path.value())), @@ -155,8 +156,6 @@ auto main(int argc, char** argv) -> int { } for (auto& thread : localization_threads) { - if (thread.joinable()) { - thread.join(); - } + thread.join(); } } From 02d31db3480b2dbde95c7bbbc3f65e2b832e2290 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 23:39:41 +0000 Subject: [PATCH 3/9] wip Signed-off-by: Charlie Huang --- src/pathing/controller.cc | 3 ++- src/pathing/controller.h | 8 ++++++-- src/unambiguous_first.cc | 5 +++-- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index 22e86f39..3dfb7a7e 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -16,6 +16,7 @@ namespace pathing { auto RunController( + const std::stop_token& stop_token, const std::string& navgrid_path = "/root/bos/constants/navgrid.json", bool verbose = false) -> void { const int lookahead_offset_ = 50; @@ -58,7 +59,7 @@ auto RunController( std::vector spline_points; - while (true) { + while (!stop_token.stop_requested()) { if (!enabled_sub.Get()) { vx_pub.Set(0.0); vy_pub.Set(0.0); diff --git a/src/pathing/controller.h b/src/pathing/controller.h index f4c42ba7..5f0c9df2 100644 --- a/src/pathing/controller.h +++ b/src/pathing/controller.h @@ -1,8 +1,12 @@ #pragma once +#include #include namespace pathing { -auto RunController(const std::string& navgrid_path, bool verbose) -> void; +auto RunController( + const std::stop_token& stop_token, + const std::string& navgrid_path = "/root/bos/constants/navgrid.json", + bool verbose = false) -> void; -} // namespace pathing \ No newline at end of file +} // namespace pathing diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index c1fad942..62722e67 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -30,8 +30,9 @@ auto main() -> int { localization::UnambiguousEstimator localizer(cameras); LOG(INFO) << "starting pathing"; - std::jthread pathing_thread( - [&]() { pathing::RunController("/bos/constants/navgrid.json", false); }); + std::jthread pathing_thread([&](const std::stop_token& stop_token) { + pathing::RunController(stop_token, "/bos/constants/navgrid.json", false); + }); LOG(INFO) << "starting pathing"; std::jthread([&] { localizer.Run(); }); From 5758bee1745df18a04cb38cf123c5519b6de363e Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 18 May 2026 22:31:25 +0000 Subject: [PATCH 4/9] wip Signed-off-by: Charlie Huang --- src/localization/run_localization.cc | 2 +- src/main_bot_main.cc | 1 + src/second_bot_main.cc | 1 + src/unambiguous_first.cc | 4 ++++ src/unambiguous_second.cc | 4 ++++ 5 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/localization/run_localization.cc b/src/localization/run_localization.cc index 18c1d2f3..7986e6e3 100644 --- a/src/localization/run_localization.cc +++ b/src/localization/run_localization.cc @@ -32,7 +32,7 @@ void RunLocalization( source->GetName(), port.value(), 30, 1080, 1080)) : std::nullopt; - while (true) { + while (!stop_token.stop_requested()) { utils::Timer timer(source->GetName(), verbose); camera::timestamped_frame_t timestamped_frame = source->Get(); if (streamer.has_value()) { diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc index 00ffdf42..9d954c76 100644 --- a/src/main_bot_main.cc +++ b/src/main_bot_main.cc @@ -15,6 +15,7 @@ using camera::camera_constants_t; auto main() -> int { + stop::RegisterHandler(); utils::StartNetworktables(9971); // TODO configure vision bot camera paths diff --git a/src/second_bot_main.cc b/src/second_bot_main.cc index 1296452d..c192b1c8 100644 --- a/src/second_bot_main.cc +++ b/src/second_bot_main.cc @@ -17,6 +17,7 @@ using camera::camera_constants_t; auto main() -> int { utils::StartNetworktables(9971); + stop::RegisterHandler(); std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index 62722e67..69bf7740 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -10,9 +10,11 @@ #include "src/pathing/controller.h" #include "src/utils/camera_utils.h" #include "src/utils/nt_utils.h" +#include "src/utils/stop.h" using camera::camera_constants_t; auto main() -> int { + stop::RegisterHandler(); utils::StartNetworktables(9971); std::string log_path = frc::DataLogManager::GetLogDir(); @@ -36,4 +38,6 @@ auto main() -> int { LOG(INFO) << "starting pathing"; std::jthread([&] { localizer.Run(); }); + + stop::WaitUntilStop(); } diff --git a/src/unambiguous_second.cc b/src/unambiguous_second.cc index 9ed04588..c7551fff 100644 --- a/src/unambiguous_second.cc +++ b/src/unambiguous_second.cc @@ -8,9 +8,11 @@ #include "src/localization/unambiguous_estimator.h" #include "src/utils/camera_utils.h" #include "src/utils/nt_utils.h" +#include "src/utils/stop.h" using camera::camera_constants_t; auto main() -> int { + stop::RegisterHandler(); utils::StartNetworktables(); std::string log_path = frc::DataLogManager::GetLogDir(); @@ -28,4 +30,6 @@ auto main() -> int { localization::UnambiguousEstimator localizer(cameras); localizer.Run(); + + stop::WaitUntilStop(); } From cd0b577358e6c9a71b8d7a0d93b0ddaf36585794 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 18 May 2026 22:34:06 +0000 Subject: [PATCH 5/9] wip Signed-off-by: Charlie Huang --- src/camera/camera_source.h | 2 +- src/camera/multi_camera_source.h | 2 +- src/utils/stop.h | 20 ++++++++++---------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/camera/camera_source.h b/src/camera/camera_source.h index b24a8281..490af51b 100644 --- a/src/camera/camera_source.h +++ b/src/camera/camera_source.h @@ -18,8 +18,8 @@ class CameraSource { std::string name_; std::unique_ptr camera_; timestamped_frame_t timestamped_frame_; - std::jthread thread_; std::mutex mutex_; + std::jthread thread_; const bool simulation_; }; diff --git a/src/camera/multi_camera_source.h b/src/camera/multi_camera_source.h index 4452c24b..f50f7b3a 100644 --- a/src/camera/multi_camera_source.h +++ b/src/camera/multi_camera_source.h @@ -20,8 +20,8 @@ class MultiCameraSource { std::vector invalid_frame_start_times_; // time since the frame has not been old std::vector timestamped_frames_; - std::vector camera_threads_; std::mutex mutex_; + std::vector camera_threads_; const bool use_all_frames_; bool frames_used_ = true; // only for when use_all_frames_ is true }; diff --git a/src/utils/stop.h b/src/utils/stop.h index bf374a8a..b2d1da22 100644 --- a/src/utils/stop.h +++ b/src/utils/stop.h @@ -14,7 +14,7 @@ constexpr std::chrono::seconds kwait_interval = 1s; std::atomic stop(false); std::atomic registered_handler(false); -void SignalHander(int signal) { +void SignalHandler(int signal) { LOG(INFO) << "Received signal: " << signal; stop = true; } @@ -24,16 +24,16 @@ void RegisterHandler() { LOG(WARNING) << "Handler has already been registred"; return; } - std::signal(SIGINT, SignalHander); - std::signal(SIGILL, SignalHander); - std::signal(SIGABRT, SignalHander); - std::signal(SIGFPE, SignalHander); - std::signal(SIGSEGV, SignalHander); - std::signal(SIGTERM, SignalHander); - std::signal(SIGHUP, SignalHander); - std::signal(SIGQUIT, SignalHander); + std::signal(SIGINT, SignalHandler); + // std::signal(SIGILL, SignalHandler); + // std::signal(SIGABRT, SignalHandler); + // std::signal(SIGFPE, SignalHandler); + // std::signal(SIGSEGV, SignalHandler); + std::signal(SIGTERM, SignalHandler); + std::signal(SIGHUP, SignalHandler); + std::signal(SIGQUIT, SignalHandler); // std::signal(SIGTRAP, SignalHander); - std::signal(SIGKILL, SignalHander); + // std::signal(SIGKILL, SignalHandler); // std::signal(SIGPIPE, SignalHander); // std::signal(SIGALRM, SignalHander); registered_handler = true; From 922c720577f3f9a619739c41234c07ddafe5d0be Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 18 May 2026 22:34:32 +0000 Subject: [PATCH 6/9] wip Signed-off-by: Charlie Huang --- src/localization/run_localization.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/localization/run_localization.h b/src/localization/run_localization.h index f58565a3..4961c1b7 100644 --- a/src/localization/run_localization.h +++ b/src/localization/run_localization.h @@ -18,8 +18,6 @@ namespace localization { // .intrinsics_path)), // std::make_unique( // camera::Camera::TURRET_BOT_FRONT_RIGHT), -// camera::camera_constants[camera::Camera::TURRET_BOT_FRONT_RIGHT] -// .extrinsics_path, // 5801, false); void RunLocalization( const std::stop_token& stop_token, From e6d23c1a8fd9118877429ec375dde01df56667e5 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Tue, 19 May 2026 21:40:24 +0000 Subject: [PATCH 7/9] wip Signed-off-by: Charlie Huang --- src/camera/multi_camera_source.cc | 2 +- src/localization/run_localization.cc | 4 ++-- src/utils/stop.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/camera/multi_camera_source.cc b/src/camera/multi_camera_source.cc index 876876ef..c95e0a29 100644 --- a/src/camera/multi_camera_source.cc +++ b/src/camera/multi_camera_source.cc @@ -12,7 +12,7 @@ MultiCameraSource::MultiCameraSource( for (size_t i = 0; i < cameras_.size(); i++) { camera_threads_.emplace_back( [this, i](const std::stop_token& stop_token) -> void { - while (true) { + while (!stop_token.stop_requested()) { if (use_all_frames_) { mutex_.lock(); bool frames_used = frames_used_; diff --git a/src/localization/run_localization.cc b/src/localization/run_localization.cc index bd6700b5..7c846814 100644 --- a/src/localization/run_localization.cc +++ b/src/localization/run_localization.cc @@ -52,10 +52,10 @@ void RunLocalization( } void RunJointLocalization( - const std::stop_token& stop_toekn, MultiCameraDetector& detector_source, + const std::stop_token& stop_token, MultiCameraDetector& detector_source, std::unique_ptr solver, std::unique_ptr sender, bool verbose) { - while (true) { + while (!stop_token.stop_requested()) { auto detections = detector_source.GetTagDetections(); std::optional estimated_pose = solver->EstimatePosition(detections); diff --git a/src/utils/stop.h b/src/utils/stop.h index b2d1da22..35038ab0 100644 --- a/src/utils/stop.h +++ b/src/utils/stop.h @@ -15,8 +15,8 @@ std::atomic stop(false); std::atomic registered_handler(false); void SignalHandler(int signal) { - LOG(INFO) << "Received signal: " << signal; stop = true; + LOG(INFO) << "Received signal: " << signal; } void RegisterHandler() { From ba5633d6e4b02a33bb49cbeaedd69e1b12c2006e Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Tue, 19 May 2026 21:48:15 +0000 Subject: [PATCH 8/9] wip Signed-off-by: Charlie Huang --- src/utils/stop.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/utils/stop.h b/src/utils/stop.h index 35038ab0..42d2c562 100644 --- a/src/utils/stop.h +++ b/src/utils/stop.h @@ -16,7 +16,6 @@ std::atomic registered_handler(false); void SignalHandler(int signal) { stop = true; - LOG(INFO) << "Received signal: " << signal; } void RegisterHandler() { From 5fce6e9485c3234b191448dc98d342b8b4b987d4 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Tue, 19 May 2026 21:49:13 +0000 Subject: [PATCH 9/9] wip Signed-off-by: Charlie Huang --- src/utils/stop.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/utils/stop.h b/src/utils/stop.h index 42d2c562..b30a1ed0 100644 --- a/src/utils/stop.h +++ b/src/utils/stop.h @@ -14,11 +14,11 @@ constexpr std::chrono::seconds kwait_interval = 1s; std::atomic stop(false); std::atomic registered_handler(false); -void SignalHandler(int signal) { +inline void SignalHandler(int signal) { stop = true; } -void RegisterHandler() { +inline void RegisterHandler() { if (registered_handler) { LOG(WARNING) << "Handler has already been registred"; return; @@ -38,7 +38,7 @@ void RegisterHandler() { registered_handler = true; } -void WaitUntilStop() { +inline void WaitUntilStop() { while (!stop) { std::this_thread::sleep_for(stop::kwait_interval); }