Refactor gamepiece

src/CMakeLists.txt

@@ -4,16 +4,14 @@ include_directories(${CMAKE_SOURCE_DIR})
 
 add_subdirectory(utils)
 add_subdirectory(camera)
+add_subdirectory(gamepiece)
 add_subdirectory(localization)
 add_subdirectory(calibration)
 add_subdirectory(yolo)
 add_subdirectory(test)
 
-add_executable(game_piece_main game_piece_main.cc)
-target_link_libraries(game_piece_main wpilibc ntcore ${OpenCV_LIBS} camera utils yolo)
-
 add_executable(fiddler_main fiddler_main.cc)
 target_link_libraries(fiddler_main PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils)
 
 add_executable(software_bot_main software_bot_main.cc)
-target_link_libraries(software_bot_main PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils yolo)
+target_link_libraries(software_bot_main PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils yolo gamepiece)

src/fiddler_main.cc

@@ -23,18 +23,18 @@ using json = nlohmann::json;
 int main() {
   utils::StartNetworktables();
 
-  camera::CameraSource back_left_camera(
+  std::shared_ptr<camera::CameraSource> back_left_camera = std::make_shared<camera::CameraSource>(
       "back_left",
       std::make_unique<camera::CVCamera>(cv::VideoCapture(
           camera::camera_constants[camera::Camera::USB0].pipeline)));
 
-  camera::CameraSource back_right_camera(
+  std::shared_ptr<camera::CameraSource> back_right_camera = std::make_shared<camera::CameraSource>(
       "back_right",
       std::make_unique<camera::CVCamera>(cv::VideoCapture(
           camera::camera_constants[camera::Camera::USB1].pipeline)));
 
   std::thread usb0_thread(
-      localization::run_localization, std::ref(back_left_camera),
+      localization::run_localization, back_left_camera,
       std::make_unique<localization::GPUAprilTagDetector>(
           640, 480,
           utils::read_intrinsics(
@@ -43,7 +43,7 @@ int main() {
       false);
 
   std::thread usb1_thread(
-      localization::run_localization, std::ref(back_right_camera),
+      localization::run_localization, back_right_camera,
       std::make_unique<localization::GPUAprilTagDetector>(
           1280, 720,
           utils::read_intrinsics(

src/gamepiece/CMakeLists.txt

@@ -0,0 +1,3 @@
+add_library(gamepiece gamepiece.cc)
+target_link_libraries(gamepiece wpilibc ntcore ${OpenCV_LIBS} camera utils yolo)
+

src/game_piece_main.cc → src/gamepiece/gamepiece.cc

@@ -1,21 +1,20 @@
-#include <frc/geometry/Pose2d.h>
+#include "gamepiece.h"
 #include <frc/geometry/Pose3d.h>
-#include <networktables/StructTopic.h>
 #include <chrono>
 #include <iomanip>
 #include <iostream>
 #include <opencv2/opencv.hpp>
 #include <thread>
-#include "src/camera/camera.h"
 #include "src/camera/camera_constants.h"
 #include "src/camera/cscore_streamer.h"
 #include "src/camera/cv_camera.h"
 #include "src/camera/realsense_camera.h"
 #include "src/utils/camera_utils.h"
 #include "src/utils/nt_utils.h"
 #include "src/yolo/model_constants.h"
-#include "src/yolo/yolo.h"
 
+
+namespace gamepiece {
 static constexpr int MAX_DETECTIONS = 6;
 static std::mutex mutex;
 
@@ -39,7 +38,7 @@ std::ostream& operator<<(std::ostream& os, const frc::Transform3d& p) {
 
 void run_gamepiece_detect(yolo::Yolo& model,
                           const std::vector<std::string>& class_names,
-                          std::unique_ptr<camera::ICamera> camera,
+                          std::shared_ptr<camera::CameraSource> camera,
                           nt::StructTopic<frc::Pose2d>& coral_topic,
                           nt::StructTopic<frc::Pose2d>& algae_topic,
                           nlohmann::json intrinsics, nlohmann::json extrinsics,
@@ -66,11 +65,11 @@ void run_gamepiece_detect(yolo::Yolo& model,
       frc::Rotation3d{
           units::radian_t{extrinsics["rotation_x"].get<float>()},
           units::radian_t{extrinsics["rotation_y"].get<float>()},
-          units::radian_t{(float)extrinsics["rotation_z"].get<float>()}}};
+          units::radian_t{extrinsics["rotation_z"].get<float>()}}};
   frc::Transform3d target_pose_cam_relative;
   frc::Pose3d target_pose_robot_relative;
   while (true) {
-    camera->GetFrame(color);
+    color = camera->GetFrame();
     mutex.lock();
     model.Postprocess(color.rows, color.cols, model.RunModel(color), bboxes,
                       confidences, class_ids);
@@ -120,31 +119,4 @@ void run_gamepiece_detect(yolo::Yolo& model,
     }
   }
 }
-
-int main() {
-  std::cout << std::fixed << std::setprecision(2);
-  std::cout << "Starting gamepiece main" << std::endl;
-  std::cout << "Started networktables" << std::endl;
-  yolo::ModelInfo model_info = yolo::models[yolo::Model::COLOR];
-  yolo::Yolo color_model(model_info.path, model_info.color);
-  utils::StartNetworktables();
-  nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
-  std::shared_ptr<nt::NetworkTable> coral_table =
-      inst.GetTable("Orin/Gamepiece/coral");
-  std::shared_ptr<nt::NetworkTable> algae_table =
-      inst.GetTable("Orin/Gamepiece/algae");
-  nt::StructTopic<frc::Pose2d> coral_topic =
-      coral_table->GetStructTopic<frc::Pose2d>("Pose");
-  nt::StructTopic<frc::Pose2d> algae_topic =
-      algae_table->GetStructTopic<frc::Pose2d>("Pose");
-
-  std::vector<std::thread> camera_threads;
-  camera_threads.emplace_back(
-      run_gamepiece_detect, std::ref(color_model),
-      std::ref(model_info.class_names),
-      std::make_unique<camera::RealSenseCamera>(), std::ref(coral_topic),
-      std::ref(algae_topic),
-      utils::read_intrinsics("/bos/constants/realsense_intrinsics.json"),
-      utils::read_extrinsics("/bos/constants/realsense_extrinsics.json"), true);
-  camera_threads[0].join();
-}
+} // namespace gamepiece

src/gamepiece/gamepiece.h

@@ -0,0 +1,16 @@
+#pragma once
+#include <frc/geometry/Pose2d.h>
+#include <networktables/StructTopic.h>
+#include "src/camera/camera_source.h"
+#include "src/yolo/yolo.h"
+#include <nlohmann/json.hpp>
+
+namespace gamepiece {
+void run_gamepiece_detect(yolo::Yolo& model,
+                          const std::vector<std::string>& class_names,
+                          std::shared_ptr<camera::CameraSource> camera,
+                          nt::StructTopic<frc::Pose2d>& coral_topic,
+                          nt::StructTopic<frc::Pose2d>& algae_topic,
+                          nlohmann::json intrinsics, nlohmann::json extrinsics,
+                          bool debug);
+} // namespace gamepiece

src/localization/run_localization.cc

@@ -8,20 +8,20 @@
 
 namespace localization {
 
-void run_localization(camera::CameraSource& source,
+void run_localization(std::shared_ptr<camera::CameraSource> source,
                       std::unique_ptr<localization::IAprilTagDetector> detector,
                       std::string extrinsics, uint port, bool verbose) {
 
-  localization::PositionSender position_sender(source.GetName());
+  localization::PositionSender position_sender(source->GetName());
 
-  camera::CscoreStreamer streamer(source.GetName(), port, 30, 1080, 1080);
+  camera::CscoreStreamer streamer(source->GetName(), port, 30, 1080, 1080);
 
   nlohmann::json extrinsics_json = utils::read_extrinsics(extrinsics);
   frc::Transform3d camera_to_robot =
       localization::ExtrinsicsJsonToCameraToRobot(extrinsics_json);
   while (true) {
-    utils::Timer timer(source.GetName(), verbose);
-    camera::timestamped_frame_t timestamped_frame = source.Get();
+    utils::Timer timer(source->GetName(), verbose);
+    camera::timestamped_frame_t timestamped_frame = source->Get();
     streamer.WriteFrame(timestamped_frame.frame);
     std::vector<localization::tag_detection_t> estimates =
         localization::ToFeildRelitivePosition(

src/localization/run_localization.h

@@ -4,7 +4,7 @@
 #include "src/localization/apriltag_detector.h"
 #include "src/localization/gpu_apriltag_detector.h"
 namespace localization {
-void run_localization(camera::CameraSource& source,
+void run_localization(std::shared_ptr<camera::CameraSource> source,
                       std::unique_ptr<localization::IAprilTagDetector> detector,
                       std::string extrinsics, uint port, bool verbose = false);
 }

src/software_bot_main.cc

@@ -19,49 +19,25 @@
 #include "src/utils/timer.h"
 #include "src/yolo/model_constants.h"
 #include "src/yolo/yolo.h"
+#include "src/gamepiece/gamepiece.h"
 
 using json = nlohmann::json;
 
-void run_yolo(const int frame_width, const int frame_height,
-              yolo::ModelInfo& model_info, camera::CameraSource& source,
-              std::string extrinsics, uint port) {
-  yolo::Yolo model(model_info.path, model_info.color, true);
-
-  camera::CscoreStreamer streamer(source.GetName(), 4971, 30, 1080, 1080);
-
-  std::vector<cv::Rect> bboxes(6);
-  std::vector<float> confidences(6);
-  std::vector<int> class_ids(6);
-
-  while (true) {
-    camera::timestamped_frame_t timestamped_frame = source.Get();
-
-    std::vector<float> detections = model.RunModel(timestamped_frame.frame);
-    model.Postprocess(timestamped_frame.frame.rows,
-                      timestamped_frame.frame.cols, detections, bboxes,
-                      confidences, class_ids);
-
-    yolo::Yolo::DrawDetections(timestamped_frame.frame, bboxes, class_ids,
-                               confidences, model_info.class_names);
-    streamer.WriteFrame(timestamped_frame.frame);
-  }
-}
-
 int main() {
   utils::StartNetworktables();
 
-  camera::CameraSource back_left_camera(
+  std::shared_ptr<camera::CameraSource> back_left_camera = std::make_shared<camera::CameraSource>(
       "back_left",
       std::make_unique<camera::CVCamera>(cv::VideoCapture(
           camera::camera_constants[camera::Camera::USB0].pipeline)));
 
-  camera::CameraSource back_right_camera(
+  std::shared_ptr<camera::CameraSource> back_right_camera = std::make_shared<camera::CameraSource>(
       "back_right",
       std::make_unique<camera::CVCamera>(cv::VideoCapture(
           camera::camera_constants[camera::Camera::USB1].pipeline)));
 
   std::thread usb0_thread(
-      localization::run_localization, std::ref(back_left_camera),
+      localization::run_localization, back_left_camera,
       std::make_unique<localization::GPUAprilTagDetector>(
           640, 480,
           utils::read_intrinsics(
@@ -70,15 +46,31 @@ int main() {
       false);
 
   std::thread usb1_thread(
-      localization::run_localization, std::ref(back_right_camera),
+      localization::run_localization, back_right_camera,
       std::make_unique<localization::GPUAprilTagDetector>(
           1280, 720,
           utils::read_intrinsics(
               camera::camera_constants[camera::Camera::USB1].intrinsics_path)),
       camera::camera_constants[camera::Camera::USB1].extrinsics_path, 4972,
       false);
 
-  usb0_thread.join();
+  nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
+  std::shared_ptr<nt::NetworkTable> coral_table =
+      inst.GetTable("Orin/Gamepiece/coral");
+  std::shared_ptr<nt::NetworkTable> algae_table =
+      inst.GetTable("Orin/Gamepiece/algae");
+  nt::StructTopic<frc::Pose2d> coral_topic =
+      coral_table->GetStructTopic<frc::Pose2d>("Pose");
+  nt::StructTopic<frc::Pose2d> algae_topic =
+      algae_table->GetStructTopic<frc::Pose2d>("Pose");
+
+  yolo::ModelInfo model_info = yolo::models[yolo::Model::COLOR];
+  yolo::Yolo color_model(model_info.path, model_info.color);
+
+  std::thread usb0_gamepiece_thread(gamepiece::run_gamepiece_detect, std::ref(color_model), std::ref(model_info.class_names), back_left_camera, std::ref(coral_topic), std::ref(algae_topic), utils::read_intrinsics(camera::camera_constants[camera::Camera::USB1].intrinsics_path),
+      utils::read_extrinsics(camera::camera_constants[camera::Camera::USB1].extrinsics_path), true);
+
+  usb0_gamepiece_thread.join();
 
   return 0;
 }

src/test/CMakeLists.txt

@@ -19,6 +19,9 @@ target_link_libraries(timer_test PRIVATE utils)
 add_executable(stress_test stress_test.cc)
 target_link_libraries(stress_test PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils yolo)
 
+add_executable(gamepiece_test gamepiece_test.cc)
+target_link_libraries(gamepiece_test PRIVATE gamepiece yolo camera utils)
+
 add_executable(nvidia_apriltag_test nvidia_apriltag_test.cc)
 target_link_libraries(nvidia_apriltag_test PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils yolo vpi utils)
 

src/test/gamepiece_test.cc

@@ -0,0 +1,30 @@
+#include "src/gamepiece/gamepiece.h"
+#include "src/yolo/yolo.h"
+#include "src/yolo/model_constants.h"
+#include "src/camera/select_camera.h"
+#include "src/camera/cscore_streamer.h"
+#include "src/utils/camera_utils.h"
+
+int main() {
+  camera::Camera config = camera::SelectCameraConfig();
+  std::shared_ptr<camera::CameraSource> camera = std::make_shared<camera::CameraSource>("nvidia_apriltag_test",
+                              camera::GetCameraStream(config));
+  camera::CscoreStreamer streamer("yolo_test", 4971, 30, 1080, 1080);
+
+  nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
+  std::shared_ptr<nt::NetworkTable> coral_table =
+      inst.GetTable("Orin/Gamepiece/coral");
+  std::shared_ptr<nt::NetworkTable> algae_table =
+      inst.GetTable("Orin/Gamepiece/algae");
+  nt::StructTopic<frc::Pose2d> coral_topic =
+      coral_table->GetStructTopic<frc::Pose2d>("Pose");
+  nt::StructTopic<frc::Pose2d> algae_topic =
+      algae_table->GetStructTopic<frc::Pose2d>("Pose");
+  yolo::ModelInfo model_info = yolo::models[yolo::Model::COLOR];
+  yolo::Yolo color_model(model_info.path, model_info.color);
+
+  std::thread usb0_gamepiece_thread(gamepiece::run_gamepiece_detect, std::ref(color_model), std::ref(model_info.class_names), camera, std::ref(coral_topic), std::ref(algae_topic), utils::read_intrinsics(camera::camera_constants[config].intrinsics_path),
+      utils::read_extrinsics(camera::camera_constants[config].extrinsics_path), true);
+
+  usb0_gamepiece_thread.join();
+}

src/test/stress_test.cc

@@ -26,12 +26,12 @@ int main() {
   utils::StartNetworktables();
 
   camera::Camera config = camera::SelectCameraConfig();
-  camera::CameraSource source("stress_test_camera",
+  std::shared_ptr<camera::CameraSource> source = std::make_shared<camera::CameraSource>("stress_test_camera",
                               camera::GetCameraStream(config));
-  cv::Mat frame = source.GetFrame();
+  cv::Mat frame = source->GetFrame();
 
   std::thread usb0_thread(
-      localization::run_localization, std::ref(source),
+      localization::run_localization, source,
       std::make_unique<localization::GPUAprilTagDetector>(
           frame.cols, frame.rows,
           utils::read_intrinsics(
@@ -40,7 +40,7 @@ int main() {
       false);
 
   std::thread usb1_thread(
-      localization::run_localization, std::ref(source),
+      localization::run_localization, source,
       std::make_unique<localization::GPUAprilTagDetector>(
           frame.cols, frame.rows,
           utils::read_intrinsics(
@@ -49,7 +49,7 @@ int main() {
       false);
 
   std::thread usb2_thread(
-      localization::run_localization, std::ref(source),
+      localization::run_localization, source,
       std::make_unique<localization::GPUAprilTagDetector>(
           frame.cols, frame.rows,
           utils::read_intrinsics(
@@ -58,7 +58,7 @@ int main() {
       false);
 
   std::thread usb3_thread(
-      localization::run_localization, std::ref(source),
+      localization::run_localization, source,
       std::make_unique<localization::GPUAprilTagDetector>(
           frame.cols, frame.rows,
           utils::read_intrinsics(

src/utils/camera_utils.h

@@ -1,3 +1,4 @@
+#pragma once
 #include <string>
 #include "nlohmann/json.hpp"
 

src/utils/nt_utils.h

@@ -1,3 +1,4 @@
+#pragma once
 namespace utils {
 void StartNetworktables(int team_number = 971);
 }  // namespace utils

src/utils/timer.h

@@ -1,3 +1,4 @@
+#pragma once
 #include <chrono>
 #include <string>
 namespace utils {