Misc fixes

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"

scripts/copy_constants.sh

@@ -0,0 +1,4 @@
+if [ "$(pwd)" != "/bos" ]; then
+  mkdir -p /bos
+  sudo cp -r constants /bos
+fi

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();
     }

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_;
 };
 

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();
-      }
-    });
+        });
   }
 }
 

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
 };

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;
     }

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,

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);

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);

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();
 }

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);

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
+}  // namespace pathing