Delete main_bot and set up the dev_orin camera

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"
     }
   ]
 }

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,13 @@ 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)
+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 "")

src/camera/camera_constants.cc

@@ -23,12 +23,27 @@ void SetConstant(const std::string_view config_name, std::optional<T>& 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;

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<double> stream_ratio = std::nullopt;
   std::optional<uint> port = std::nullopt;
   std::optional<uint> 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& {

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<UVCCamera>(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<UVCCamera>(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::CVCamera>(
+            camera::GetCameraConstants()[choice]);
+      case camera::CameraType::MIPI:
+        return std::make_unique<camera::CVCamera>(
+            camera::GetCameraConstants()[choice]);
+      case camera::CameraType::INVALID:
+        return SelectCameraConfig(camera_constants);
     }
-    return std::make_unique<camera::CVCamera>(
-        camera::GetCameraConstants()[choice]);
-  } else {
     return SelectCameraConfig(camera_constants);
   }
-  // return camera_constants.contains(choice)
-  //            ? std::make_unique<camera::CVCamera>(
-  //                  camera::GetCameraConstants()[choice])
-  //            : SelectCameraConfig(camera_constants);
+  return SelectCameraConfig(camera_constants);
 }
 
 auto SelectCameraConfig(std::optional<std::string> choice,
@@ -77,9 +81,6 @@ auto SelectCameraConfig(std::optional<std::string> choice,
   } else {
     return SelectCameraConfig(camera_constants);
   }
-  // return choice.has_value()
-  //            ? SelectCameraConfig(choice.value(), camera_constants)
-  //            : SelectCameraConfig(camera_constants);
 }
 
 }  // namespace camera

src/camera/uvc_camera.cc

@@ -61,21 +61,24 @@ void callback(uvc_frame_t* frame, void* ptr) {
 UVCCamera::UVCCamera(const CameraConstant& camera_constant,
                      absl::Status& status, std::optional<std::string> 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(
         fmt::format("Unable to create context for camera {} with error code {}",
                     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: ";
@@ -153,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 {

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<camera::DiskCamera>(
           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::UVCCamera>(
-          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::CVCamera>(
-          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::UVCCamera>(
+              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::CVCamera>(camera_constants_[i]));
+          break;
+        case camera::CameraType::OPENCV:
+          cameras_.push_back(
+              std::make_unique<camera::CVCamera>(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<OpenCVAprilTagDetector>(
             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<GPUAprilTagDetector>(
             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";
     }
   }