From f9f4277083ae5140366b2f02ea5a18b002528c80 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 18 Apr 2026 05:56:23 +0000 Subject: [PATCH 1/3] Builds untested --- src/CMakeLists.txt | 6 -- src/calibration/focus_calibrate.cc | 12 +-- src/calibration/frame_shower.cc | 11 +-- src/camera/camera.h | 4 +- src/camera/camera_source.cc | 4 +- src/camera/cv_camera.cc | 19 ++--- src/camera/cv_camera.h | 2 +- src/camera/disk_camera.cc | 12 ++- src/camera/disk_camera.h | 2 +- src/camera/multi_camera_source.cc | 2 +- src/camera/uvc_camera.cc | 17 ++--- src/camera/uvc_camera.h | 2 +- src/main_bot_main.cc | 100 ------------------------- src/second_bot_main.cc | 75 ------------------- src/test/integration_test/yolo_test.cc | 19 +++-- 15 files changed, 52 insertions(+), 235 deletions(-) delete mode 100644 src/main_bot_main.cc delete mode 100644 src/second_bot_main.cc diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1f016da1..983a2e3a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -21,12 +21,6 @@ add_subdirectory(yolo) add_subdirectory(test) add_subdirectory(pathing) -add_executable(main_bot_main main_bot_main.cc) -target_link_libraries(main_bot_main PRIVATE camera localization utils) - -add_executable(second_bot_main second_bot_main.cc) -target_link_libraries(second_bot_main PRIVATE camera localization utils) - add_executable(unambiguous_second unambiguous_second.cc) target_link_libraries(unambiguous_second PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils) diff --git a/src/calibration/focus_calibrate.cc b/src/calibration/focus_calibrate.cc index c7025548..824eec0b 100644 --- a/src/calibration/focus_calibrate.cc +++ b/src/calibration/focus_calibrate.cc @@ -10,15 +10,17 @@ auto main(int argc, char* argv[]) -> int { std::unique_ptr camera = camera::SelectCameraConfig(camera::GetCameraConstants()); + camera::timestamped_frame_t timestamped_frame; + camera->GetFrame(×tamped_frame); camera::CscoreStreamer streamer("focus_calibrate", 5801, 30, - camera->GetFrame().frame); + timestamped_frame.frame); - cv::Mat frame, gray, laplace; + cv::Mat gray, laplace; while (true) { - frame = camera->GetFrame().frame; - streamer.WriteFrame(frame); + camera->GetFrame(×tamped_frame); + streamer.WriteFrame(timestamped_frame.frame); - cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); + cv::cvtColor(timestamped_frame.frame, gray, cv::COLOR_BGR2GRAY); cv::Laplacian(gray, laplace, CV_64F); cv::Scalar mean, stddev; cv::meanStdDev(laplace, mean, stddev, cv::Mat()); diff --git a/src/calibration/frame_shower.cc b/src/calibration/frame_shower.cc index d018bbd8..418fb066 100644 --- a/src/calibration/frame_shower.cc +++ b/src/calibration/frame_shower.cc @@ -18,17 +18,18 @@ auto main(int argc, char* argv[]) -> int { std::unique_ptr camera = camera::SelectCameraConfig( absl::GetFlag(FLAGS_camera_name), camera::GetCameraConstants()); + camera::timestamped_frame_t timestamped_frame; + camera->GetFrame(×tamped_frame); camera::CscoreStreamer streamer("frame_shower", absl::GetFlag(FLAGS_port).value_or(5801), 30, - camera->GetFrame().frame); + timestamped_frame.frame); LOG(INFO) << "Camera opened successfully" << std::endl; - cv::Mat frame = camera->GetFrame().frame; - LOG(INFO) << "Size of frame: " << frame.size; + LOG(INFO) << "Size of frame: " << timestamped_frame.frame.size; while (true) { - frame = camera->GetFrame().frame; - streamer.WriteFrame(frame); + camera->GetFrame(×tamped_frame); + streamer.WriteFrame(timestamped_frame.frame); } return 0; } diff --git a/src/camera/camera.h b/src/camera/camera.h index 07736b7c..bcef0c12 100644 --- a/src/camera/camera.h +++ b/src/camera/camera.h @@ -28,8 +28,8 @@ using timestamped_frame_t = struct TimestampedFrame { class ICamera { public: - virtual auto GetFrame() -> timestamped_frame_t = 0; - virtual auto Restart() -> void = 0; + virtual void GetFrame(timestamped_frame_t* output) = 0; + virtual void Restart() = 0; [[nodiscard]] virtual auto GetCameraConstant() const -> camera_constant_t = 0; virtual auto IsDone() -> bool { return false; } virtual ~ICamera() = default; diff --git a/src/camera/camera_source.cc b/src/camera/camera_source.cc index 8c35e0ad..5951783e 100644 --- a/src/camera/camera_source.cc +++ b/src/camera/camera_source.cc @@ -7,7 +7,7 @@ CameraSource::CameraSource(std::string name, std::unique_ptr camera, : name_(std::move(name)), camera_(std::move(camera)), simulation_(simulation) { - timestamped_frame_ = camera_->GetFrame(); + camera_->GetFrame(×tamped_frame_); thread_ = std::thread([this] { while (true) { if (camera_->IsDone()) { @@ -15,7 +15,7 @@ CameraSource::CameraSource(std::string name, std::unique_ptr camera, return; } timestamped_frame_t timestamped_frame; - timestamped_frame = camera_->GetFrame(); + camera_->GetFrame(×tamped_frame_); mutex_.lock(); timestamped_frame_ = timestamped_frame; mutex_.unlock(); diff --git a/src/camera/cv_camera.cc b/src/camera/cv_camera.cc index 96b6227f..faff488c 100644 --- a/src/camera/cv_camera.cc +++ b/src/camera/cv_camera.cc @@ -63,29 +63,26 @@ CVCamera::CVCamera(const CameraConstant& c, std::optional log_path) } } -auto CVCamera::GetFrame() -> timestamped_frame_t { - timestamped_frame_t timestamped_frame; +void CVCamera::GetFrame(timestamped_frame_t* output) { cv::Mat raw_image; if (!cap_.grab()) { Restart(); LOG(WARNING) << "Restarting camera"; } - timestamped_frame.timestamp = frc::Timer::GetFPGATimestamp().to(); + output->timestamp = frc::Timer::GetFPGATimestamp().to(); cap_.retrieve(raw_image); - raw_image.copyTo(timestamped_frame.frame); + raw_image.copyTo(output->frame); - if (timestamped_frame.frame.empty()) { - timestamped_frame.frame = backup_image_; + if (output->frame.empty()) { + output->frame = backup_image_; } - if (timestamped_frame.frame.channels() == 4) { - cv::cvtColor(timestamped_frame.frame, timestamped_frame.frame, - cv::COLOR_BGRA2BGR); + if (output->frame.channels() == 4) { + cv::cvtColor(output->frame, output->frame, cv::COLOR_BGRA2BGR); } if (log_path_.has_value()) { - WriteFrame(log_path_.value(), timestamped_frame); + WriteFrame(log_path_.value(), *output); } - return timestamped_frame; } auto CVCamera::Restart() -> void { diff --git a/src/camera/cv_camera.h b/src/camera/cv_camera.h index 048d96b7..72a8941e 100644 --- a/src/camera/cv_camera.h +++ b/src/camera/cv_camera.h @@ -11,7 +11,7 @@ class CVCamera : public ICamera { public: CVCamera(const CameraConstant& camera_constant, std::optional log_path = std::nullopt); - auto GetFrame() -> timestamped_frame_t override; + void GetFrame(timestamped_frame_t* output) override; auto Restart() -> void override; [[nodiscard]] auto GetCameraConstant() const -> camera_constant_t override; diff --git a/src/camera/disk_camera.cc b/src/camera/disk_camera.cc index f3663c58..cc31369d 100644 --- a/src/camera/disk_camera.cc +++ b/src/camera/disk_camera.cc @@ -55,27 +55,25 @@ DiskCamera::DiskCamera(std::string image_folder_path, } image_paths_ = std::move(normalized); } -auto DiskCamera::GetFrame() -> timestamped_frame_t { +void DiskCamera::GetFrame(timestamped_frame_t* output) { if (image_paths_.empty()) { std::cout << "Finished reading all frames from DiskCamera. Folder path: " << image_folder_path_ << std::endl; frc::DataLogManager::Stop(); - return {.invalid = true}; + output->invalid = true; + return; } double recorded_ts = image_paths_.top().timestamp; std::cout << "Recorded ts: " << recorded_ts + start_.value_or(0) << std::endl; - timestamped_frame_t timestamped_frame{ - .frame = cv::imread(image_paths_.top().path), - .timestamp = recorded_ts + start_.value_or(0)}; + output->frame = cv::imread(image_paths_.top().path); + output->timestamp = recorded_ts + start_.value_or(0); image_paths_.pop(); if (!image_paths_.empty()) { double delta = image_paths_.top().timestamp - recorded_ts; std::this_thread::sleep_for(std::chrono::duration(delta / speed_)); } - - return timestamped_frame; } auto DiskCamera::Restart() -> void {} diff --git a/src/camera/disk_camera.h b/src/camera/disk_camera.h index 08d98d2d..c0004183 100644 --- a/src/camera/disk_camera.h +++ b/src/camera/disk_camera.h @@ -29,7 +29,7 @@ class DiskCamera : public ICamera { std::optional camera_constant = std::nullopt, double speed = 1.0, std::optional start = std::nullopt, std::optional end = std::nullopt); - auto GetFrame() -> timestamped_frame_t override; + void GetFrame(timestamped_frame_t* output) override; auto Restart() -> void override; auto IsDone() -> bool override { return image_paths_.empty(); } [[nodiscard]] auto GetCameraConstant() const -> camera_constant_t override; diff --git a/src/camera/multi_camera_source.cc b/src/camera/multi_camera_source.cc index 841c23c4..4bec9728 100644 --- a/src/camera/multi_camera_source.cc +++ b/src/camera/multi_camera_source.cc @@ -22,7 +22,7 @@ MultiCameraSource::MultiCameraSource( } } timestamped_frame_t timestamped_frame; - timestamped_frame = cameras_[i]->GetFrame(); + cameras_[i]->GetFrame(×tamped_frame); mutex_.lock(); timestamped_frames_[i] = timestamped_frame; frames_used_ = false; diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index 7e7d2cf6..3a2cccf1 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -104,25 +104,22 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, } } -auto UVCCamera::GetFrame() -> timestamped_frame_t { - timestamped_frame_t copied_timestamped_frame; +void UVCCamera::GetFrame(timestamped_frame_t* output) { while (frame_index_ == previous_frame_index_) { std::this_thread::yield(); } mutex_.try_lock(); if (frame_buffer.frame.empty()) { - backup_image_.copyTo(copied_timestamped_frame.frame); - copied_timestamped_frame.invalid = true; - copied_timestamped_frame.timestamp = - frc::Timer::GetFPGATimestamp().to(); + backup_image_.copyTo(output->frame); + output->invalid = true; + output->timestamp = frc::Timer::GetFPGATimestamp().to(); } else { - frame_buffer.frame.copyTo(copied_timestamped_frame.frame); - copied_timestamped_frame.invalid = frame_buffer.invalid; - copied_timestamped_frame.timestamp = frame_buffer.timestamp; + frame_buffer.frame.copyTo(output->frame); + output->invalid = frame_buffer.invalid; + output->timestamp = frame_buffer.timestamp; } mutex_.unlock(); previous_frame_index_ = frame_index_; - return copied_timestamped_frame; } auto UVCCamera::Restart() -> void { diff --git a/src/camera/uvc_camera.h b/src/camera/uvc_camera.h index b239baf0..3915c789 100644 --- a/src/camera/uvc_camera.h +++ b/src/camera/uvc_camera.h @@ -12,7 +12,7 @@ class UVCCamera : public ICamera { public: UVCCamera(const CameraConstant& camera_constant, absl::Status& status, std::optional log_path = std::nullopt); - auto GetFrame() -> timestamped_frame_t override; + void GetFrame(timestamped_frame_t* output) override; auto Restart() -> void override; ~UVCCamera() override; [[nodiscard]] auto GetCameraConstant() const -> camera_constant_t override; diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc deleted file mode 100644 index 332846a0..00000000 --- a/src/main_bot_main.cc +++ /dev/null @@ -1,100 +0,0 @@ -#include "src/camera/camera_constants.h" -#include "src/camera/camera_source.h" -#include "src/camera/cv_camera.h" -#include "src/localization/multi_tag_solver.h" -#include "src/localization/networktable_sender.h" -#include "src/localization/nvidia_apriltag_detector.h" -#include "src/localization/opencv_apriltag_detector.h" -#include "src/localization/run_localization.h" -#include "src/localization/square_solver.h" -#include "src/utils/camera_utils.h" -#include "src/utils/nt_utils.h" - -using camera::camera_constants_t; -auto main() -> int { - utils::StartNetworktables(9971); - // TODO configure vision bot camera paths - - std::string log_path = frc::DataLogManager::GetLogDir(); - camera_constants_t camera_constants = camera::GetCameraConstants(); - - LOG(INFO) << "Starting estimators"; - - std::thread front_thread([&]() { - auto front_camera = std::make_unique( - "Front", std::make_unique( - camera_constants.at("main_bot_front"))); - cv::Mat front_camera_frame = front_camera->GetFrame(); - - std::vector> front_sender; - front_sender.emplace_back( - std::make_unique( - camera_constants.at("main_bot_front").name)); - - localization::RunLocalization( - std::move(front_camera), - std::make_unique( - front_camera_frame.cols, front_camera_frame.rows, - utils::ReadIntrinsics( - camera_constants.at("main_bot_front").intrinsics_path.value())), - std::make_unique( - camera_constants.at("main_bot_front")), - std::move(front_sender), - camera_constants.at("main_bot_front").extrinsics_path.value(), 5801, - false); - }); - - std::thread left_thread([&]() { - auto left_camera = std::make_unique( - "Left", - std::make_unique(camera_constants.at("main_bot_left"), - fmt::format("{}/left", log_path))); - cv::Mat left_camera_frame = left_camera->GetFrame(); - - std::vector> left_sender; - left_sender.emplace_back(std::make_unique( - camera_constants.at("main_bot_left").name)); - - localization::RunLocalization( - 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::thread right_thread([&]() { - auto right_camera = std::make_unique( - "Right", std::make_unique( - camera_constants.at("main_bot_right"), - fmt::format("{}/right", log_path))); - cv::Mat right_camera_frame = right_camera->GetFrame(); - - std::vector> right_sender; - right_sender.emplace_back( - std::make_unique( - camera_constants.at("main_bot_right").name)); - - localization::RunLocalization( - 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); - }); - - LOG(INFO) << "Started estimators"; - - // TODO find better way - left_thread.join(); -} diff --git a/src/second_bot_main.cc b/src/second_bot_main.cc deleted file mode 100644 index a2e305df..00000000 --- a/src/second_bot_main.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include "src/camera/camera_constants.h" -#include "src/camera/camera_source.h" -#include "src/camera/cv_camera.h" -#include "src/localization/multi_tag_solver.h" -#include "src/localization/networktable_sender.h" -#include "src/localization/opencv_apriltag_detector.h" -#include "src/localization/run_localization.h" -#include "src/localization/square_solver.h" -#include "src/utils/camera_utils.h" -#include "src/utils/nt_utils.h" - -using camera::camera_constants_t; -auto main() -> int { - utils::StartNetworktables(); - - std::string log_path = frc::DataLogManager::GetLogDir(); - camera_constants_t camera_constants = camera::GetCameraConstants(); - - LOG(INFO) << "Starting estimators"; - - std::thread left_thread([&]() { - auto left_camera = std::make_unique( - "Left", std::make_unique( - camera_constants.at("second_bot_left"), - fmt::format("{}/left", log_path))); - cv::Mat left_camera_frame = left_camera->GetFrame(); - - std::vector> left_sender; - left_sender.emplace_back(std::make_unique( - camera_constants.at("second_bot_left").name)); - - localization::RunLocalization( - 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::thread right_thread([&]() { - auto right_camera = std::make_unique( - "Right", std::make_unique( - camera_constants.at("second_bot_right"), - fmt::format("{}/right", log_path))); - cv::Mat right_camera_frame = right_camera->GetFrame(); - - std::vector> right_sender; - right_sender.emplace_back( - std::make_unique( - camera_constants.at("second_bot_right").name)); - - localization::RunLocalization( - 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); - }); - - // TODO front camera - - LOG(INFO) << "Started estimators"; - - left_thread.join(); -} diff --git a/src/test/integration_test/yolo_test.cc b/src/test/integration_test/yolo_test.cc index 4f58f434..44970df4 100644 --- a/src/test/integration_test/yolo_test.cc +++ b/src/test/integration_test/yolo_test.cc @@ -20,9 +20,11 @@ auto main() -> int { yolo::Yolo model(model_info.path, model_info.color, true); std::unique_ptr camera = camera::SelectCameraConfig(camera::GetCameraConstants()); + camera::timestamped_frame_t timestamped_frame; + camera->GetFrame(×tamped_frame); camera::CscoreStreamer streamer("yolo_test", 5801, 30, - camera->GetFrame().frame); + timestamped_frame.frame); std::vector bboxes(MAX_DETECTIONS); std::vector confidences(MAX_DETECTIONS); @@ -30,23 +32,24 @@ auto main() -> int { // Chopped because I screwed up on the dataset, and technically the model outputs "CORAL", "coral", "ALGAE" or "algae" while (true) { - cv::Mat frame; + cv::Mat* frame; utils::Timer timer("yolo"); - frame = camera->GetFrame().frame; - if (frame.empty()) { + camera->GetFrame(×tamped_frame); + frame = ×tamped_frame.frame; + if (frame->empty()) { std::cout << "Couldn't fetch frame properly" << std::endl; return 1; } - std::vector detections = model.RunModel(frame); - model.Postprocess(frame.rows, frame.cols, detections, bboxes, confidences, + std::vector detections = model.RunModel(*frame); + model.Postprocess(frame->rows, frame->cols, detections, bboxes, confidences, class_ids); - yolo::Yolo::DrawDetections(frame, bboxes, class_ids, confidences, + yolo::Yolo::DrawDetections(*frame, bboxes, class_ids, confidences, model_info.class_names); std::cout << "Object angle: " << yolo::Yolo::GetObjectAngle( (detections[0] + detections[2]) / 2.0, std::numbers::pi * (3.0 / 4.0)) << "\n"; - streamer.WriteFrame(frame); + streamer.WriteFrame(*frame); } } From b8dee6d9b71c209e7c2ad06de8bf279fb82ef46f Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 18 Apr 2026 20:19:08 +0000 Subject: [PATCH 2/3] Cleanup yolo test --- src/test/integration_test/yolo_test.cc | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/test/integration_test/yolo_test.cc b/src/test/integration_test/yolo_test.cc index 44970df4..946ec653 100644 --- a/src/test/integration_test/yolo_test.cc +++ b/src/test/integration_test/yolo_test.cc @@ -32,24 +32,23 @@ auto main() -> int { // Chopped because I screwed up on the dataset, and technically the model outputs "CORAL", "coral", "ALGAE" or "algae" while (true) { - cv::Mat* frame; utils::Timer timer("yolo"); camera->GetFrame(×tamped_frame); - frame = ×tamped_frame.frame; - if (frame->empty()) { + cv::Mat& frame = timestamped_frame.frame; + if (frame.empty()) { std::cout << "Couldn't fetch frame properly" << std::endl; return 1; } - std::vector detections = model.RunModel(*frame); - model.Postprocess(frame->rows, frame->cols, detections, bboxes, confidences, + std::vector detections = model.RunModel(frame); + model.Postprocess(frame.rows, frame.cols, detections, bboxes, confidences, class_ids); - yolo::Yolo::DrawDetections(*frame, bboxes, class_ids, confidences, + yolo::Yolo::DrawDetections(frame, bboxes, class_ids, confidences, model_info.class_names); std::cout << "Object angle: " << yolo::Yolo::GetObjectAngle( (detections[0] + detections[2]) / 2.0, std::numbers::pi * (3.0 / 4.0)) << "\n"; - streamer.WriteFrame(*frame); + streamer.WriteFrame(frame); } } From 98d8249637ec91bfab5ff8cbe53a57f524c1879d Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 18 Apr 2026 20:20:50 +0000 Subject: [PATCH 3/3] Fix camera_source --- src/camera/camera_source.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/camera/camera_source.cc b/src/camera/camera_source.cc index 5951783e..338b7a26 100644 --- a/src/camera/camera_source.cc +++ b/src/camera/camera_source.cc @@ -15,7 +15,7 @@ CameraSource::CameraSource(std::string name, std::unique_ptr camera, return; } timestamped_frame_t timestamped_frame; - camera_->GetFrame(×tamped_frame_); + camera_->GetFrame(×tamped_frame); mutex_.lock(); timestamped_frame_ = timestamped_frame; mutex_.unlock();