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..338b7a26 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 620cb39a..995549a1 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_.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..946ec653 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,9 +32,9 @@ 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"); - frame = camera->GetFrame().frame; + camera->GetFrame(×tamped_frame); + cv::Mat& frame = timestamped_frame.frame; if (frame.empty()) { std::cout << "Couldn't fetch frame properly" << std::endl; return 1;