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/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<ICamera> 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<ICamera> 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<double>();
     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..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<ICamera> camera_;
   timestamped_frame_t timestamped_frame_;
-  std::thread thread_;
   std::mutex mutex_;
+  std::jthread thread_;
   const bool simulation_;
 };
 
diff --git a/src/camera/multi_camera_source.cc b/src/camera/multi_camera_source.cc
index 841c23c4..c95e0a29 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<double>(0.002));
-            continue;
+    camera_threads_.emplace_back(
+        [this, i](const std::stop_token& stop_token) -> void {
+          while (!stop_token.stop_requested()) {
+            if (use_all_frames_) {
+              mutex_.lock();
+              bool frames_used = frames_used_;
+              mutex_.unlock();
+              if (!frames_used) {
+                std::this_thread::sleep_for(
+                    std::chrono::duration<double>(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..f50f7b3a 100644
--- a/src/camera/multi_camera_source.h
+++ b/src/camera/multi_camera_source.h
@@ -20,8 +20,8 @@ class MultiCameraSource {
   std::vector<double>
       invalid_frame_start_times_;  // time since the frame has not been old
   std::vector<timestamped_frame_t> timestamped_frames_;
-  std::vector<std::thread> camera_threads_;
   std::mutex mutex_;
+  std::vector<std::jthread> camera_threads_;
   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<UVCCamera>(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 17e87e17..11be2949 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<UVCCamera*>(ptr);
-  ptr_->mutex_.lock();
-  switch (frame->frame_format) {
-    case UVC_COLOR_FORMAT_MJPEG: {
-      char* data = static_cast<char*>(frame->data);
-      std::vector<uchar> 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<char*>(frame->data);
+        std::vector<uchar> 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<double>();  // 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<double>();  // 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 d8a767a2..7c846814 100644
--- a/src/localization/run_localization.cc
+++ b/src/localization/run_localization.cc
@@ -19,6 +19,7 @@
 namespace localization {
 
 void RunLocalization(
+    const std::stop_token& stop_token,
     std::unique_ptr<camera::CameraSource> source,
     std::unique_ptr<localization::IAprilTagDetector> detector,
     std::unique_ptr<localization::IPositionSolver> solver,
@@ -30,7 +31,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()) {
@@ -51,10 +52,10 @@ void RunLocalization(
 }
 
 void RunJointLocalization(
-    MultiCameraDetector& detector_source,
+    const std::stop_token& stop_token, MultiCameraDetector& detector_source,
     std::unique_ptr<localization::IJointPositionSolver> solver,
     std::unique_ptr<localization::IPositionSender> sender, bool verbose) {
-  while (true) {
+  while (!stop_token.stop_requested()) {
     auto detections = detector_source.GetTagDetections();
     std::optional<position_estimate_t> estimated_pose =
         solver->EstimatePosition(detections);
diff --git a/src/localization/run_localization.h b/src/localization/run_localization.h
index 2429961a..1e1ae2d0 100644
--- a/src/localization/run_localization.h
+++ b/src/localization/run_localization.h
@@ -19,17 +19,17 @@ namespace localization {
 //                 .intrinsics_path)),
 //     std::make_unique<localization::SquareSolver>(
 //         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,
     std::unique_ptr<camera::CameraSource> source,
     std::unique_ptr<localization::IAprilTagDetector> detector,
     std::unique_ptr<localization::IPositionSolver> solver,
     std::vector<std::unique_ptr<localization::IPositionSender>> sender,
     std::optional<uint> port = std::nullopt, bool verbose = false);
+
 void RunJointLocalization(
-    MultiCameraDetector& detector_source,
+    const std::stop_token& stop_token, MultiCameraDetector& detector_source,
     std::unique_ptr<localization::IJointPositionSolver> solver,
     std::unique_ptr<localization::IPositionSender> sender,
     bool verbose = false);
diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc
index 29ffd54a..37755516 100644
--- a/src/main_bot_main.cc
+++ b/src/main_bot_main.cc
@@ -11,9 +11,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);
   // TODO configure vision bot camera paths
 
@@ -22,7 +24,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<camera::CameraSource>(
         "Left",
         std::make_unique<camera::CVCamera>(camera_constants.at("main_bot_left"),
@@ -34,7 +36,7 @@ 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<localization::OpenCVAprilTagDetector>(
             left_camera_frame.cols, left_camera_frame.rows,
             utils::ReadIntrinsics(
@@ -44,7 +46,7 @@ auto main() -> int {
         std::move(left_sender), 5802);
   });
 
-  std::thread right_thread([&]() {
+  std::jthread right_thread([&](const std::stop_token& stop_token) {
     auto right_camera = std::make_unique<camera::CameraSource>(
         "Right", std::make_unique<camera::CVCamera>(
                      camera_constants.at("main_bot_right"),
@@ -55,9 +57,9 @@ auto main() -> int {
     right_sender.emplace_back(
         std::make_unique<localization::NetworkTableSender>(
             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<localization::OpenCVAprilTagDetector>(
             right_camera_frame.cols, right_camera_frame.rows,
             utils::ReadIntrinsics(
@@ -69,13 +71,10 @@ 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";
 
-  // TODO find better way
-  left_thread.join();
-  right_thread.join();
-  pathing_thread.join();
+  stop::WaitUntilStop();
 }
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<frc::Pose2d> 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 <stop_token>
 #include <string>
 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/second_bot_main.cc b/src/second_bot_main.cc
index 386edad4..9b0ae846 100644
--- a/src/second_bot_main.cc
+++ b/src/second_bot_main.cc
@@ -10,9 +10,13 @@
 #include "src/pathing/pathfinding.h"
 #include "src/utils/camera_utils.h"
 #include "src/utils/nt_utils.h"
+#include "src/utils/stop.h"
+
+#include <thread>
 
 using camera::camera_constants_t;
 auto main() -> int {
+  stop::RegisterHandler();
   utils::StartNetworktables(971);
 
   std::string log_path = frc::DataLogManager::GetLogDir();
@@ -20,7 +24,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<camera::CameraSource>(
         "Left", std::make_unique<camera::CVCamera>(
                     camera_constants.at("second_bot_left"),
@@ -32,7 +36,7 @@ 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<localization::GPUAprilTagDetector>(
             left_camera_frame.cols, left_camera_frame.rows,
             utils::ReadIntrinsics(camera_constants.at("second_bot_left")
@@ -42,7 +46,7 @@ auto main() -> int {
         std::move(left_sender));
   });
 
-  std::thread right_thread([&]() {
+  std::jthread right_thread([&](const std::stop_token& stop_token) {
     auto right_camera = std::make_unique<camera::CameraSource>(
         "Right", std::make_unique<camera::CVCamera>(
                      camera_constants.at("second_bot_right"),
@@ -55,7 +59,7 @@ 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<localization::GPUAprilTagDetector>(
             right_camera_frame.cols, right_camera_frame.rows,
             utils::ReadIntrinsics(camera_constants.at("second_bot_right")
@@ -65,13 +69,10 @@ auto main() -> int {
         std::move(right_sender));
   });
 
-  // 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 2799f961..c538cdba 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<std::thread> localization_threads;
+  std::vector<std::jthread> 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::CameraSource>(
           camera_name, std::make_unique<camera::DiskCamera>(
@@ -140,7 +141,7 @@ auto main(int argc, char** argv) -> int {
       senders.emplace_back(std::make_unique<localization::SimulationSender>(
           camera_name, base_port + i - 1000));
       localization::RunLocalization(
-          std::move(camera_source),
+          stop_token, std::move(camera_source),
           std::make_unique<localization::OpenCVAprilTagDetector>(
               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();
   }
 }
diff --git a/src/test/integration_test/localization_test2.cc b/src/test/integration_test/localization_test2.cc
index 5b9eca71..250603c0 100644
--- a/src/test/integration_test/localization_test2.cc
+++ b/src/test/integration_test/localization_test2.cc
@@ -22,6 +22,7 @@
 #include "src/localization/unambiguous_estimator.h"
 #include "src/utils/camera_utils.h"
 #include "src/utils/log.h"
+#include "src/utils/stop.h"
 
 ABSL_FLAG(std::string, image_folder, "",  //NOLINT
           "Path to folder of test images");
@@ -83,6 +84,7 @@ auto main(int argc, char** argv) -> int {
 
   absl::ParseCommandLine(argc, argv);
 
+  stop::RegisterHandler();
   frc::DataLogManager::Start(frc::DataLogManager::GetLogDir());
 
   const std::string image_folder_root = absl::GetFlag(FLAGS_image_folder);
@@ -120,12 +122,13 @@ auto main(int argc, char** argv) -> int {
 
     camera_constants.push_back(constants.at(camera_name));
   }
-  std::jthread thread([camera_constants, camera_folders, speed] {
+  std::jthread thread([camera_constants, camera_folders,
+                       speed](const std::stop_token& stop_token) {
     localization::MultiCameraDetector detector_source(camera_constants,
                                                       camera_folders, speed);
     LOG(INFO) << "Created camera source";
     localization::RunJointLocalization(
-        detector_source,
+        stop_token, detector_source,
         std::make_unique<localization::UnambiguousEstimator>(camera_constants),
         std::make_unique<localization::NetworkTableSender>("Joint", false,
                                                            true));
@@ -135,4 +138,6 @@ auto main(int argc, char** argv) -> int {
     LOG(FATAL) << "No readable images found in any camera folder under: "
                << image_folder_root;
   }
+
+  stop::WaitUntilStop();
 }
diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc
index ae59ed1f..065d4c11 100644
--- a/src/unambiguous_first.cc
+++ b/src/unambiguous_first.cc
@@ -10,9 +10,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(9971);
 
   std::string log_path = frc::DataLogManager::GetLogDir();
@@ -24,13 +26,14 @@ auto main() -> int {
       camera_constants.at("main_bot_left"),
       camera_constants.at("main_bot_right")};
 
-  std::jthread thread([cameras] {
+  std::jthread thread([cameras](const std::stop_token& stop_token) {
     localization::MultiCameraDetector detector_source(cameras);
     LOG(INFO) << "Started cameras";
     std::this_thread::sleep_for(std::chrono::duration<double>(2));
     localization::RunJointLocalization(
-        detector_source,
+        stop_token, detector_source,
         std::make_unique<localization::UnambiguousEstimator>(cameras),
         std::make_unique<localization::NetworkTableSender>("Left", false));
   });
+  stop::WaitUntilStop();
 }
diff --git a/src/unambiguous_second.cc b/src/unambiguous_second.cc
index ec353836..e7d2f7f7 100644
--- a/src/unambiguous_second.cc
+++ b/src/unambiguous_second.cc
@@ -9,9 +9,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(9971);
 
   std::string log_path = frc::DataLogManager::GetLogDir();
@@ -19,13 +21,15 @@ auto main() -> int {
 
   std::vector<camera::CameraConstant> cameras{camera_constants.at("dev_orin")};
 
-  std::jthread thread([cameras] {
+  std::jthread thread([cameras](const std::stop_token& stop_token) {
     localization::MultiCameraDetector detector_source(cameras);
     LOG(INFO) << "Started cameras";
     std::this_thread::sleep_for(std::chrono::duration<double>(2));
     localization::RunJointLocalization(
-        detector_source,
+        stop_token, detector_source,
         std::make_unique<localization::UnambiguousEstimator>(cameras),
         std::make_unique<localization::NetworkTableSender>("Left", false));
   });
+
+  stop::WaitUntilStop();
 }
diff --git a/src/utils/stop.h b/src/utils/stop.h
new file mode 100644
index 00000000..b30a1ed0
--- /dev/null
+++ b/src/utils/stop.h
@@ -0,0 +1,46 @@
+#pragma once
+
+#include <atomic>
+#include <csignal>
+#include <iostream>
+#include <thread>
+#include "src/utils/log.h"
+
+namespace stop {
+
+using namespace std::literals::chrono_literals;
+
+constexpr std::chrono::seconds kwait_interval = 1s;
+std::atomic<bool> stop(false);
+std::atomic<bool> registered_handler(false);
+
+inline void SignalHandler(int signal) {
+  stop = true;
+}
+
+inline void RegisterHandler() {
+  if (registered_handler) {
+    LOG(WARNING) << "Handler has already been registred";
+    return;
+  }
+  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, SignalHandler);
+  // std::signal(SIGPIPE, SignalHander);
+  // std::signal(SIGALRM, SignalHander);
+  registered_handler = true;
+}
+
+inline void WaitUntilStop() {
+  while (!stop) {
+    std::this_thread::sleep_for(stop::kwait_interval);
+  }
+}
+}  // namespace stop