From edf359d8928110025d6871367c027d5b836b59b1 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 22 May 2026 10:31:10 -0700 Subject: [PATCH 1/2] Delete main_bot and set up the dev_orin camera Signed-off-by: Charlie Huang --- constants/camera_constants.json | 64 ++++-------------- src/CMakeLists.txt | 7 -- src/camera/camera_constants.cc | 28 +++++++- src/camera/camera_constants.h | 6 +- src/camera/select_camera.cc | 37 ++++++----- src/camera/uvc_camera.cc | 25 ++++--- src/localization/multi_camera_detector.cc | 36 ++++++---- src/main_bot_main.cc | 80 ----------------------- src/unambiguous_first.cc | 39 ----------- 9 files changed, 101 insertions(+), 221 deletions(-) delete mode 100644 src/main_bot_main.cc delete mode 100644 src/unambiguous_first.cc diff --git a/constants/camera_constants.json b/constants/camera_constants.json index 9ef22e72..077ccd60 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -8,67 +8,28 @@ "pipeline": "nvarguscamerasrc sensor-id=0 aelock=true exposuretimerange=\"100000 200000\" gainrange=\"1 15\" ispdigitalgainrange=\"1 1\" ! video/x-raw(memory:NVMM), width=1456, height=1088, framerate=30/1, format=NV12 ! nvvidconv ! video/x-raw, format=BGRx ! queue ! appsink", "intrinsics_path": "/bos/constants/imx296_camera0_intrinsics.json", "extrinsics_path": "/bos/constants/imx296_camera0_extrinsics.json", - "name": "mipi0" + "name": "mipi0", + "camera_type": "mipi" }, { "pipeline": "nvarguscamerasrc sensor-id=1 aelock=true exposuretimerange=\"100000 200000\" gainrange=\"1 15\" ispdigitalgainrange=\"1 1\" ! video/x-raw(memory:NVMM), width=1456, height=1088, framerate=30/1, format=NV12 ! nvvidconv ! video/x-raw, format=BGRx ! queue ! appsink", "intrinsics_path": "/bos/constants/imx296_camera1_intrinsics.json", "extrinsics_path": "/bos/constants/imx296_camera1_extrinsics.json", - "name": "mipi1" + "name": "mipi1", + "camera_type": "mipi" }, { - "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", + "name": "dev_orin", "intrinsics_path": "/bos/constants/misc/dev_orin_intrinsics.json", "extrinsics_path": "/bos/constants/misc/dev_orin_extrinsics.json", - "name": "dev_orin", - "backlight": null, "frame_width": 1280, "frame_height": 800, "fps": 100.0, - "exposure": null, + "max_payload_size": 1024, + "stream_ratio": 0.5, "port": 5801, "detector_type": "austin_gpu", - "serial_id": "devorin", - "streamer_fps": 1 - }, - { - "name": "main_bot_front", - "pipeline": "nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), width=1456, height=1088, framerate=30/1, format=NV12 ! nvvidconv ! video/x-raw, format=BGRx ! queue ! appsink", - "intrinsics_path": "/bos/constants/main_bot/front_intrinsics.json", - "extrinsics_path": "/bos/constants/main_bot/front_extrinsics.json", - "backlight": null, - "frame_width": null, - "frame_height": null, - "fps": null, - "exposure": null, - "port": 5803, - "detector_type": "austin_gpu" - }, - { - "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.3:1.0-video-index0", - "intrinsics_path": "/bos/constants/main_bot/left_intrinsics.json", - "extrinsics_path": "/bos/constants/main_bot/left_extrinsics.json", - "name": "main_bot_left", - "backlight": null, - "frame_width": 1280, - "frame_height": 800, - "fps": 60.0, - "exposure": null, - "port": 5802, - "detector_type": "austin_gpu" - }, - { - "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", - "intrinsics_path": "/bos/constants/main_bot/right_intrinsics.json", - "extrinsics_path": "/bos/constants/main_bot/right_extrinsics.json", - "name": "main_bot_right", - "backlight": null, - "frame_width": 1280, - "frame_height": 800, - "fps": 60.0, - "exposure": null, - "port": 5801, - "detector_type": "austin_gpu" + "camera_type": "uvc" }, { "name": "second_bot_front", @@ -81,7 +42,8 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5803, - "detector_type": "austin_gpu" + "detector_type": "austin_gpu", + "camera_type": "uvc" }, { "name": "second_bot_right", @@ -94,7 +56,8 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5802, - "detector_type": "austin_gpu" + "detector_type": "austin_gpu", + "camera_type": "uvc" }, { "name": "second_bot_left", @@ -107,7 +70,8 @@ "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5801, - "detector_type": "austin_gpu" + "detector_type": "austin_gpu", + "camera_type": "uvc" } ] } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 87697488..8157523f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,5 @@ cmake_minimum_required(VERSION 3.16...4.0) - include_directories(${CMAKE_SOURCE_DIR}) if(ENABLE_CLANG_TIDY) @@ -22,16 +21,10 @@ 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 utils localization pathing camera) - add_executable(second_bot_main second_bot_main.cc) target_link_libraries(second_bot_main PRIVATE utils localization pathing camera) add_executable(unambiguous_second unambiguous_second.cc) target_link_libraries(unambiguous_second PRIVATE utils localization pathing camera) -add_executable(unambiguous_first unambiguous_first.cc) -target_link_libraries(unambiguous_first PRIVATE utils localization pathing camera) - set(CMAKE_CXX_CLANG_TIDY "") diff --git a/src/camera/camera_constants.cc b/src/camera/camera_constants.cc index 4f25e73a..5e567824 100644 --- a/src/camera/camera_constants.cc +++ b/src/camera/camera_constants.cc @@ -23,12 +23,27 @@ void SetConstant(const std::string_view config_name, std::optional& config, auto StringToDetectorType(const std::string& detector_type) -> DetectorType { if (detector_type == "austin_gpu") { - return AUSTIN_GPU; + return DetectorType::AUSTIN_GPU; } if (detector_type == "opencv_cpu") { - return OPENCV_CPU; + return DetectorType::OPENCV_CPU; } - return INVALID; + LOG(WARNING) << "Invalid detector type"; + return DetectorType::INVALID; +} + +auto StringToCameraType(const std::string& camera_type) -> CameraType { + if (camera_type == "uvc") { + return CameraType::UVC; + } + if (camera_type == "mipi") { + return CameraType::MIPI; + } + if (camera_type == "opencv") { + return CameraType::OPENCV; + } + LOG(WARNING) << "Invalid camera type"; + return CameraType::INVALID; } auto GetCameraConstants(const std::string& path) -> camera_constants_t { @@ -90,6 +105,13 @@ auto GetCameraConstants(const std::string& path) -> camera_constants_t { camera_constant.detector_type = StringToDetectorType(camera_config["detector_type"]); } + + if (camera_config.contains("camera_type") && + !camera_config["camera_type"].is_null()) { + camera_constant.camera_type = + StringToCameraType(camera_config["camera_type"]); + } + camera_constants.insert({camera_constant.name, camera_constant}); } return camera_constants; diff --git a/src/camera/camera_constants.h b/src/camera/camera_constants.h index 2d5dc505..0f97a1a4 100644 --- a/src/camera/camera_constants.h +++ b/src/camera/camera_constants.h @@ -5,7 +5,8 @@ #include "src/utils/pch.h" namespace camera { -enum DetectorType { OPENCV_CPU, AUSTIN_GPU, INVALID }; +enum class DetectorType { OPENCV_CPU, AUSTIN_GPU, INVALID }; +enum class CameraType { UVC, MIPI, OPENCV, INVALID }; using camera_constant_t = struct CameraConstant { std::string name; @@ -25,7 +26,8 @@ using camera_constant_t = struct CameraConstant { std::optional stream_ratio = std::nullopt; std::optional port = std::nullopt; std::optional streamer_fps = std::nullopt; - DetectorType detector_type = INVALID; + DetectorType detector_type = DetectorType::INVALID; + CameraType camera_type = CameraType::INVALID; friend auto operator<<(std::ostream& os, const CameraConstant& c) -> std::ostream& { diff --git a/src/camera/select_camera.cc b/src/camera/select_camera.cc index a895352f..b34981d4 100644 --- a/src/camera/select_camera.cc +++ b/src/camera/select_camera.cc @@ -47,25 +47,29 @@ auto SelectCameraConfig(const std::string& choice, } } if (camera_constants.contains(choice)) { - if (camera_constants.at(choice).serial_id.has_value()) { - LOG(INFO) << "Initializing via uvc"; - absl::Status status; - auto camera = - std::make_unique(camera_constants.at(choice), status); - if (!status.ok()) { - LOG(FATAL) << "Failed to select camera via uvc: " << status.message(); + switch (camera_constants.at(choice).camera_type) { + case camera::CameraType::UVC: { + LOG(INFO) << "Initializing via uvc"; + absl::Status status; + auto camera = + std::make_unique(camera_constants.at(choice), status); + if (!status.ok()) { + LOG(FATAL) << "Failed to select camera via uvc: " << status.message(); + } + return camera; } - return camera; + case camera::CameraType::OPENCV: + return std::make_unique( + camera::GetCameraConstants()[choice]); + case camera::CameraType::MIPI: + return std::make_unique( + camera::GetCameraConstants()[choice]); + case camera::CameraType::INVALID: + return SelectCameraConfig(camera_constants); } - return std::make_unique( - camera::GetCameraConstants()[choice]); - } else { return SelectCameraConfig(camera_constants); } - // return camera_constants.contains(choice) - // ? std::make_unique( - // camera::GetCameraConstants()[choice]) - // : SelectCameraConfig(camera_constants); + return SelectCameraConfig(camera_constants); } auto SelectCameraConfig(std::optional choice, @@ -77,9 +81,6 @@ auto SelectCameraConfig(std::optional choice, } else { return SelectCameraConfig(camera_constants); } - // return choice.has_value() - // ? SelectCameraConfig(choice.value(), camera_constants) - // : SelectCameraConfig(camera_constants); } } // namespace camera diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index 11be2949..cd9d068e 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -61,11 +61,7 @@ void callback(uvc_frame_t* frame, void* ptr) { UVCCamera::UVCCamera(const CameraConstant& camera_constant, absl::Status& status, std::optional log_path) : camera_constant_(camera_constant), log_path_(std::move(log_path)) { - if (!camera_constant.serial_id.has_value()) { - status = absl::InvalidArgumentError(fmt::format( - "Must provide a serial id for uvc camera {}", camera_constant.name)); - return; - } + int res = uvc_init(&context_, nullptr); if (res != 0) { status = absl::AbortedError( @@ -73,9 +69,16 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, camera_constant.name, res)); return; } - res = uvc_find_device(context_, &device_, 0, 0, - camera_constant_.serial_id->c_str()); - LOG(INFO) << "Serial id: " << camera_constant_.serial_id.value(); + if (!camera_constant.serial_id.has_value()) { + LOG(WARNING) << "Was not provided with serial id. This shuold only be done " + "if there is only one uvc camera connected"; + + res = uvc_find_device(context_, &device_, 0, 0, nullptr); + } else { + res = uvc_find_device(context_, &device_, 0, 0, + camera_constant_.serial_id->c_str()); + LOG(INFO) << "Serial id: " << camera_constant_.serial_id.value(); + } if (res != 0) { status = absl::AbortedError( fmt::format("Unable to find device for camera {} with error code {}", @@ -138,8 +141,10 @@ auto UVCCamera::Restart() -> void { uvc_close(device_handle_); uvc_unref_device(device_); - uvc_find_device(context_, &device_, 0, 0, - camera_constant_.serial_id->c_str()); + const char* serial_id = camera_constant_.serial_id.has_value() + ? camera_constant_.serial_id.value().c_str() + : nullptr; + uvc_find_device(context_, &device_, 0, 0, serial_id); uvc_open(device_, &device_handle_); LOG(INFO) << "Restarting device UVC Camera. Device ctrl: "; diff --git a/src/localization/multi_camera_detector.cc b/src/localization/multi_camera_detector.cc index 03db9b82..d3ddf4da 100644 --- a/src/localization/multi_camera_detector.cc +++ b/src/localization/multi_camera_detector.cc @@ -2,6 +2,7 @@ #include "absl/status/status.h" #include "src/camera/camera.h" #include "src/camera/cv_camera.h" +#include "src/camera/select_camera.h" #include "src/camera/uvc_camera.h" #include "src/localization/gpu_apriltag_detector.h" #include "src/localization/opencv_apriltag_detector.h" @@ -33,33 +34,44 @@ MultiCameraDetector::MultiCameraDetector( if (image_paths.has_value()) { cameras_.push_back(std::make_unique( image_paths.value()[i], camera_constants_[i], disk_replay_speed)); - } else if (camera_constants_[i].serial_id.has_value()) { - absl::Status status; - cameras_.push_back(std::make_unique( - camera_constants_[i], status, camera_log_dest)); - if (!status.ok()) { - LOG(WARNING) << "Unable to create uvc camera: " << status.message(); - } } else { - cameras_.push_back(std::make_unique( - camera_constants_[i], camera_log_dest)); + switch (camera_constants_[i].camera_type) { + case camera::CameraType::UVC: { + absl::Status status; + cameras_.push_back(std::make_unique( + camera_constants_[i], status, camera_log_dest)); + if (!status.ok()) { + LOG(WARNING) << "Unable to create uvc camera: " << status.message(); + } + } break; + case camera::CameraType::MIPI: + cameras_.push_back( + std::make_unique(camera_constants_[i])); + break; + case camera::CameraType::OPENCV: + cameras_.push_back( + std::make_unique(camera_constants_[i])); + break; + case camera::CameraType::INVALID: + LOG(FATAL) << "Invalid camera type: " << camera_constants_[i].name; + } } auto intrinsics = utils::ReadIntrinsics(camera_constants_[i].intrinsics_path.value()); switch (camera_constants_[i].detector_type) { - case camera::OPENCV_CPU: { + case camera::DetectorType::OPENCV_CPU: { detectors_.push_back(std::make_unique( camera_constants_[i].frame_width.value(), camera_constants_[i].frame_height.value(), intrinsics)); break; } - case camera::AUSTIN_GPU: { + case camera::DetectorType::AUSTIN_GPU: { detectors_.push_back(std::make_unique( camera_constants_[i].frame_width.value(), camera_constants_[i].frame_height.value(), intrinsics)); break; } - case camera::INVALID: + case camera::DetectorType::INVALID: LOG(FATAL) << "Invalid detector type"; } } diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc deleted file mode 100644 index 37755516..00000000 --- a/src/main_bot_main.cc +++ /dev/null @@ -1,80 +0,0 @@ -#include "src/camera/camera_constants.h" -#include "src/camera/camera_source.h" -#include "src/camera/cv_camera.h" -#include "src/localization/gpu_apriltag_detector.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/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 - - std::string log_path = frc::DataLogManager::GetLogDir(); - camera_constants_t camera_constants = camera::GetCameraConstants(); - - LOG(INFO) << "Starting estimators"; - - 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"), - 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( - 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), 5802); - }); - - 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"), - 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)); - // const std::stop_token& stop_token - localization::RunLocalization( - 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), 5803); - }); - - LOG(INFO) << "Started estimators"; - - std::jthread pathing_thread(pathing::RunController, - "/bos/constants/navgrid.json", false); - - LOG(INFO) << "pathing started"; - - stop::WaitUntilStop(); -} diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc deleted file mode 100644 index 065d4c11..00000000 --- a/src/unambiguous_first.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include "src/camera/camera_constants.h" -#include "src/camera/camera_source.h" -#include "src/camera/cv_camera.h" -#include "src/localization/multi_camera_detector.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/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(); - camera_constants_t camera_constants = camera::GetCameraConstants(); - - LOG(INFO) << "Starting cameras"; - - std::vector cameras{ - camera_constants.at("main_bot_left"), - camera_constants.at("main_bot_right")}; - - 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(2)); - localization::RunJointLocalization( - stop_token, detector_source, - std::make_unique(cameras), - std::make_unique("Left", false)); - }); - stop::WaitUntilStop(); -} From 32418487fce7f3b9a9d1b2ef3e991856b3bb4354 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 22 May 2026 11:50:55 -0700 Subject: [PATCH 2/2] add dev orin main Signed-off-by: Charlie Huang --- src/CMakeLists.txt | 3 +++ src/camera/uvc_camera.cc | 1 + src/unambiguous_dev_orin.cc | 36 ++++++++++++++++++++++++++++++++++++ src/utils/stop.h | 13 +++++++++++++ 4 files changed, 53 insertions(+) create mode 100644 src/unambiguous_dev_orin.cc diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8157523f..b04194b5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -27,4 +27,7 @@ target_link_libraries(second_bot_main PRIVATE utils localization pathing camera) add_executable(unambiguous_second unambiguous_second.cc) target_link_libraries(unambiguous_second PRIVATE utils localization pathing camera) +add_executable(unambiguous_dev_orin unambiguous_dev_orin.cc) +target_link_libraries(unambiguous_dev_orin PRIVATE utils localization pathing camera) + set(CMAKE_CXX_CLANG_TIDY "") diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index cd9d068e..91ecc930 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -158,6 +158,7 @@ UVCCamera::~UVCCamera() { uvc_close(device_handle_); uvc_unref_device(device_); uvc_exit(context_); + LOG(INFO) << camera_constant_.name << " has been destructed"; } auto UVCCamera::GetCameraConstant() const -> camera_constant_t { diff --git a/src/unambiguous_dev_orin.cc b/src/unambiguous_dev_orin.cc new file mode 100644 index 00000000..52005c9e --- /dev/null +++ b/src/unambiguous_dev_orin.cc @@ -0,0 +1,36 @@ +#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/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(); + camera_constants_t camera_constants = camera::GetCameraConstants(); + + std::vector cameras{camera_constants.at("dev_orin")}; + + std::jthread thread([cameras](const std::stop_token& stop_token) { + localization::MultiCameraDetector detector_source(cameras); + std::this_thread::sleep_for(std::chrono::duration(2)); + localization::RunJointLocalization( + stop_token, detector_source, + std::make_unique(cameras), + std::make_unique("Left", false)); + }); + + LOG(INFO) << "Started localization"; + stop::WaitUntilStop(); + LOG(INFO) << "Stopping"; +} diff --git a/src/utils/stop.h b/src/utils/stop.h index b30a1ed0..63c08b20 100644 --- a/src/utils/stop.h +++ b/src/utils/stop.h @@ -11,6 +11,7 @@ namespace stop { using namespace std::literals::chrono_literals; constexpr std::chrono::seconds kwait_interval = 1s; +constexpr std::chrono::seconds kwait_until_kill = 10s; std::atomic stop(false); std::atomic registered_handler(false); @@ -35,6 +36,18 @@ inline void RegisterHandler() { // std::signal(SIGKILL, SignalHandler); // std::signal(SIGPIPE, SignalHander); // std::signal(SIGALRM, SignalHander); + + std::thread([] { + while (!stop) { + std::this_thread::sleep_for(stop::kwait_interval); + } + std::this_thread::sleep_for(stop::kwait_until_kill); + if (stop) { + LOG(ERROR) << "Failed to exit cleanly"; + std::raise(SIGKILL); + } + }).detach(); + registered_handler = true; }