From 92310c60d2f057d8cb1d942df1d4bc2b6a596cd5 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Mon, 29 Dec 2025 05:44:06 +0000 Subject: [PATCH 1/4] Up to thread issue --- src/gamepiece/CMakeLists.txt | 3 + src/gamepiece/gamepiece.cc | 122 +++++++++++++++++++++++++++++++++++ src/gamepiece/gamepiece.h | 16 +++++ src/software_bot_main.cc | 58 +++++++---------- src/utils/camera_utils.h | 1 + src/utils/nt_utils.h | 1 + src/utils/timer.h | 1 + 7 files changed, 169 insertions(+), 33 deletions(-) create mode 100644 src/gamepiece/CMakeLists.txt create mode 100644 src/gamepiece/gamepiece.cc create mode 100644 src/gamepiece/gamepiece.h diff --git a/src/gamepiece/CMakeLists.txt b/src/gamepiece/CMakeLists.txt new file mode 100644 index 0000000..5b4d15e --- /dev/null +++ b/src/gamepiece/CMakeLists.txt @@ -0,0 +1,3 @@ +add_executable(game_piece_main game_piece_main.cc) +target_link_libraries(game_piece_main wpilibc ntcore ${OpenCV_LIBS} camera utils yolo) + diff --git a/src/gamepiece/gamepiece.cc b/src/gamepiece/gamepiece.cc new file mode 100644 index 0000000..bb9b37f --- /dev/null +++ b/src/gamepiece/gamepiece.cc @@ -0,0 +1,122 @@ +#include "gamepiece.h" +#include +#include +#include +#include +#include +#include +#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" + + +namespace gamepiece { +static constexpr int MAX_DETECTIONS = 6; +static std::mutex mutex; + +std::ostream& operator<<(std::ostream& os, const frc::Pose3d& p) { + os << "Point(" << p.X().value() << ", " << p.Y().value() << ", " + << p.Z().value() << ")" + << "\nRotation:\nPitch:\t" << p.Rotation().Y().value() << "\nRoll:\t" + << p.Rotation().X().value() << "\nYaw:\t" << p.Rotation().Z().value() + << ")"; + return os; +} + +std::ostream& operator<<(std::ostream& os, const frc::Transform3d& p) { + os << "Point(" << p.X().value() << ", " << p.Y().value() << ", " + << p.Z().value() << ")" + << "\nRotation:\nPitch:\t" << p.Rotation().Y().value() << "\nRoll:\t" + << p.Rotation().X().value() << "\nYaw:\t" << p.Rotation().Z().value() + << ")"; + return os; +} + +void run_gamepiece_detect(yolo::Yolo& model, + const std::vector& class_names, + std::shared_ptr camera, + nt::StructTopic& coral_topic, + nt::StructTopic& algae_topic, + nlohmann::json intrinsics, nlohmann::json extrinsics, + bool debug) { + nt::StructPublisher coral_pub = coral_topic.Publish(); + nt::StructPublisher algae_pub = algae_topic.Publish(); + cv::Mat color; + std::vector bboxes(MAX_DETECTIONS); + std::vector confidences(MAX_DETECTIONS); + std::vector class_ids(MAX_DETECTIONS); + const float pinhole_height = extrinsics + ["translation_z"]; // Height of the center of the camera, must be from GROUND not bumpers or smthn + const float cam_cx = intrinsics["cx"]; + const float cam_cy = intrinsics["cy"]; + const float focal_length_vertical = + intrinsics["fy"].get(); // needs to be meters + const float focal_length_horizontal = intrinsics["fx"].get(); + const float cam_pitch = extrinsics["rotation_y"]; + const frc::Pose3d cam_pose{ + frc::Translation3d{ + units::meter_t{extrinsics["translation_x"].get()}, + units::meter_t{extrinsics["translation_y"].get()}, + units::meter_t{extrinsics["translation_z"].get()}}, + frc::Rotation3d{ + units::radian_t{extrinsics["rotation_x"].get()}, + units::radian_t{extrinsics["rotation_y"].get()}, + units::radian_t{(float)extrinsics["rotation_z"].get()}}}; + frc::Transform3d target_pose_cam_relative; + frc::Pose3d target_pose_robot_relative; + while (true) { + color = camera->GetFrame(); + mutex.lock(); + model.Postprocess(color.rows, color.cols, model.RunModel(color), bboxes, + confidences, class_ids); + mutex.unlock(); + for (size_t i = 0; i < MAX_DETECTIONS; i++) { + if (bboxes[i].empty()) { + if (i == 0) { + std::cout << "No detections" << std::endl; + } + break; + } + const int c_y = bboxes[i].y + bboxes[i].height / 2; + const int c_x = bboxes[i].x + bboxes[i].width / 2; + const float cam_relative_pitch = + atan2(c_y - cam_cy, focal_length_vertical); + const float phi = cam_relative_pitch + cam_pitch; + const float distance = pinhole_height / sin(phi); + const std::string& class_name = class_names[class_ids[i]]; + const float cam_relative_yaw = + atan2(c_x - cam_cx, focal_length_horizontal); + target_pose_cam_relative = { + frc::Translation3d{ + units::meter_t{distance * cos(cam_relative_pitch) * + cos(cam_relative_yaw)}, + units::meter_t{distance * cos(cam_relative_pitch) * + sin(cam_relative_yaw)}, + units::meter_t{distance * -sin(cam_relative_pitch)}}, + frc::Rotation3d{0_rad, units::radian_t{cam_relative_pitch}, + units::radian_t{-cam_relative_yaw}}}; + target_pose_robot_relative = + cam_pose.TransformBy(target_pose_cam_relative); + if (class_name == "coral") { + coral_pub.Set(target_pose_robot_relative.ToPose2d()); + } else { + algae_pub.Set(target_pose_robot_relative.ToPose2d()); + } + if (debug) { + std::cout << "\tc_y:\t" << c_y << "\tcam_relative_pitch:\t" + << cam_relative_pitch << "\tphi:\t" << phi << "\tyaw:\t" + << cam_relative_yaw << std::endl; + std::cout << "Detected a " << class_name << " " << distance + << " meters away" << std::endl; + std::cout << "TargetPose: " << target_pose_cam_relative << std::endl; + std::cout << "Robot_relative: " << target_pose_robot_relative + << std::endl; + } + } + } +} +} // gamepiece diff --git a/src/gamepiece/gamepiece.h b/src/gamepiece/gamepiece.h new file mode 100644 index 0000000..2319c25 --- /dev/null +++ b/src/gamepiece/gamepiece.h @@ -0,0 +1,16 @@ +#pragma once +#include +#include +#include "src/camera/camera_source.h" +#include "src/yolo/yolo.h" +#include + +namespace gamepiece { +void run_gamepiece_detect(yolo::Yolo& model, + const std::vector& class_names, + std::shared_ptr camera, + nt::StructTopic& coral_topic, + nt::StructTopic& algae_topic, + nlohmann::json intrinsics, nlohmann::json extrinsics, + bool debug); +} diff --git a/src/software_bot_main.cc b/src/software_bot_main.cc index 396ac95..021b03e 100644 --- a/src/software_bot_main.cc +++ b/src/software_bot_main.cc @@ -16,6 +16,8 @@ #include "src/utils/timer.h" #include "src/yolo/model_constants.h" #include "src/yolo/yolo.h" +#include "src/gamepiece/gamepiece.h" +#include "src/utils/camera_utils.h" using json = nlohmann::json; @@ -40,55 +42,45 @@ void run_estimator(const int frame_width, const int frame_height, } } -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 bboxes(6); - std::vector confidences(6); - std::vector class_ids(6); - - while (true) { - camera::timestamped_frame_t timestamped_frame = source.Get(); - - std::vector 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 back_left_camera = std::make_shared( "back_left", std::make_unique(cv::VideoCapture( camera::camera_constants[camera::Camera::USB0].pipeline))); - camera::CameraSource back_right_camera( + std::shared_ptr back_right_camera = std::make_shared( "back_right", std::make_unique(cv::VideoCapture( camera::camera_constants[camera::Camera::USB1].pipeline))); - std::thread usb0_thread( - run_estimator, 640, 480, std::ref(back_left_camera), + std::thread usb0_localization_thread( + run_estimator, 640, 480, back_left_camera, camera::camera_constants[camera::Camera::USB0].intrinsics_path, camera::camera_constants[camera::Camera::USB0].extrinsics_path, 4971); - std::thread usb1_thread( - run_estimator, 1280, 720, std::ref(back_right_camera), + std::thread usb1_localization_thread( + run_estimator, 1280, 720, back_right_camera, camera::camera_constants[camera::Camera::USB1].intrinsics_path, camera::camera_constants[camera::Camera::USB1].extrinsics_path, 4971); - - usb1_thread.join(); + nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); + std::shared_ptr coral_table = + inst.GetTable("Orin/Gamepiece/coral"); + std::shared_ptr algae_table = + inst.GetTable("Orin/Gamepiece/algae"); + nt::StructTopic coral_topic = + coral_table->GetStructTopic("Pose"); + nt::StructTopic algae_topic = + algae_table->GetStructTopic("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); + + usb1_localization_thread.join(); return 0; } diff --git a/src/utils/camera_utils.h b/src/utils/camera_utils.h index 16b2764..49cb965 100644 --- a/src/utils/camera_utils.h +++ b/src/utils/camera_utils.h @@ -1,3 +1,4 @@ +#pragma once #include #include "nlohmann/json.hpp" diff --git a/src/utils/nt_utils.h b/src/utils/nt_utils.h index a2166db..2de41cc 100644 --- a/src/utils/nt_utils.h +++ b/src/utils/nt_utils.h @@ -1,3 +1,4 @@ +#pragma once namespace utils { void StartNetworktables(int team_number = 971); } // namespace utils diff --git a/src/utils/timer.h b/src/utils/timer.h index 9f6fda0..e645b47 100644 --- a/src/utils/timer.h +++ b/src/utils/timer.h @@ -1,3 +1,4 @@ +#pragma once #include #include namespace utils { From 5c2c1de943b50e6df1e884695d50ff27740f5b02 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Mon, 29 Dec 2025 05:56:51 +0000 Subject: [PATCH 2/4] Make threads work and add cmake --- src/CMakeLists.txt | 6 ++---- src/gamepiece/CMakeLists.txt | 4 ++-- src/gamepiece/gamepiece.cc | 4 ++-- src/gamepiece/gamepiece.h | 2 +- src/software_bot_main.cc | 13 +++++++------ 5 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4431850..28678aa 100644 --- a/src/CMakeLists.txt +++ b/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) diff --git a/src/gamepiece/CMakeLists.txt b/src/gamepiece/CMakeLists.txt index 5b4d15e..df24c4c 100644 --- a/src/gamepiece/CMakeLists.txt +++ b/src/gamepiece/CMakeLists.txt @@ -1,3 +1,3 @@ -add_executable(game_piece_main game_piece_main.cc) -target_link_libraries(game_piece_main wpilibc ntcore ${OpenCV_LIBS} camera utils yolo) +add_library(gamepiece gamepiece.cc) +target_link_libraries(gamepiece wpilibc ntcore ${OpenCV_LIBS} camera utils yolo) diff --git a/src/gamepiece/gamepiece.cc b/src/gamepiece/gamepiece.cc index bb9b37f..5f8c9a9 100644 --- a/src/gamepiece/gamepiece.cc +++ b/src/gamepiece/gamepiece.cc @@ -65,7 +65,7 @@ void run_gamepiece_detect(yolo::Yolo& model, frc::Rotation3d{ units::radian_t{extrinsics["rotation_x"].get()}, units::radian_t{extrinsics["rotation_y"].get()}, - units::radian_t{(float)extrinsics["rotation_z"].get()}}}; + units::radian_t{extrinsics["rotation_z"].get()}}}; frc::Transform3d target_pose_cam_relative; frc::Pose3d target_pose_robot_relative; while (true) { @@ -119,4 +119,4 @@ void run_gamepiece_detect(yolo::Yolo& model, } } } -} // gamepiece +} // namespace gamepiece diff --git a/src/gamepiece/gamepiece.h b/src/gamepiece/gamepiece.h index 2319c25..ea8dfb0 100644 --- a/src/gamepiece/gamepiece.h +++ b/src/gamepiece/gamepiece.h @@ -13,4 +13,4 @@ void run_gamepiece_detect(yolo::Yolo& model, nt::StructTopic& algae_topic, nlohmann::json intrinsics, nlohmann::json extrinsics, bool debug); -} +} // namespace gamepiece diff --git a/src/software_bot_main.cc b/src/software_bot_main.cc index 021b03e..169d237 100644 --- a/src/software_bot_main.cc +++ b/src/software_bot_main.cc @@ -18,22 +18,23 @@ #include "src/yolo/yolo.h" #include "src/gamepiece/gamepiece.h" #include "src/utils/camera_utils.h" +#include "src/gamepiece/gamepiece.h" using json = nlohmann::json; void run_estimator(const int frame_width, const int frame_height, - camera::CameraSource& source, std::string intrinsics, + std::shared_ptr source, std::string intrinsics, std::string extrinsics, uint port) { localization::TagEstimator tag_estimator(frame_width, frame_height, intrinsics, extrinsics); - localization::PositionSender position_sender(source.GetName()); + localization::PositionSender position_sender(source->GetName()); - camera::CscoreStreamer streamer(source.GetName(), 4971, 30, 1080, 1080); + camera::CscoreStreamer streamer(source->GetName(), 4971, 30, 1080, 1080); while (true) { - utils::Timer timer(source.GetName(), false); - camera::timestamped_frame_t timestamped_frame = source.Get(); + utils::Timer timer(source->GetName(), false); + camera::timestamped_frame_t timestamped_frame = source->Get(); streamer.WriteFrame(timestamped_frame.frame); std::vector estimates = tag_estimator.Estimate(timestamped_frame.frame, @@ -80,7 +81,7 @@ int main() { 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); - usb1_localization_thread.join(); + usb0_gamepiece_thread.join(); return 0; } From ebaf3d3f09cb796cb234d1118033488993ca5785 Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sat, 3 Jan 2026 23:10:25 +0000 Subject: [PATCH 3/4] Add gamepiece_test and remove game_piece_main.cc --- src/game_piece_main.cc | 150 ------------------------------------- src/test/CMakeLists.txt | 3 + src/test/gamepiece_test.cc | 30 ++++++++ 3 files changed, 33 insertions(+), 150 deletions(-) delete mode 100644 src/game_piece_main.cc create mode 100644 src/test/gamepiece_test.cc diff --git a/src/game_piece_main.cc b/src/game_piece_main.cc deleted file mode 100644 index 4cad863..0000000 --- a/src/game_piece_main.cc +++ /dev/null @@ -1,150 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#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" - -static constexpr int MAX_DETECTIONS = 6; -static std::mutex mutex; - -std::ostream& operator<<(std::ostream& os, const frc::Pose3d& p) { - os << "Point(" << p.X().value() << ", " << p.Y().value() << ", " - << p.Z().value() << ")" - << "\nRotation:\nPitch:\t" << p.Rotation().Y().value() << "\nRoll:\t" - << p.Rotation().X().value() << "\nYaw:\t" << p.Rotation().Z().value() - << ")"; - return os; -} - -std::ostream& operator<<(std::ostream& os, const frc::Transform3d& p) { - os << "Point(" << p.X().value() << ", " << p.Y().value() << ", " - << p.Z().value() << ")" - << "\nRotation:\nPitch:\t" << p.Rotation().Y().value() << "\nRoll:\t" - << p.Rotation().X().value() << "\nYaw:\t" << p.Rotation().Z().value() - << ")"; - return os; -} - -void run_gamepiece_detect(yolo::Yolo& model, - const std::vector& class_names, - std::unique_ptr camera, - nt::StructTopic& coral_topic, - nt::StructTopic& algae_topic, - nlohmann::json intrinsics, nlohmann::json extrinsics, - bool debug) { - nt::StructPublisher coral_pub = coral_topic.Publish(); - nt::StructPublisher algae_pub = algae_topic.Publish(); - cv::Mat color; - std::vector bboxes(MAX_DETECTIONS); - std::vector confidences(MAX_DETECTIONS); - std::vector class_ids(MAX_DETECTIONS); - const float pinhole_height = extrinsics - ["translation_z"]; // Height of the center of the camera, must be from GROUND not bumpers or smthn - const float cam_cx = intrinsics["cx"]; - const float cam_cy = intrinsics["cy"]; - const float focal_length_vertical = - intrinsics["fy"].get(); // needs to be meters - const float focal_length_horizontal = intrinsics["fx"].get(); - const float cam_pitch = extrinsics["rotation_y"]; - const frc::Pose3d cam_pose{ - frc::Translation3d{ - units::meter_t{extrinsics["translation_x"].get()}, - units::meter_t{extrinsics["translation_y"].get()}, - units::meter_t{extrinsics["translation_z"].get()}}, - frc::Rotation3d{ - units::radian_t{extrinsics["rotation_x"].get()}, - units::radian_t{extrinsics["rotation_y"].get()}, - units::radian_t{(float)extrinsics["rotation_z"].get()}}}; - frc::Transform3d target_pose_cam_relative; - frc::Pose3d target_pose_robot_relative; - while (true) { - camera->GetFrame(color); - mutex.lock(); - model.Postprocess(color.rows, color.cols, model.RunModel(color), bboxes, - confidences, class_ids); - mutex.unlock(); - for (size_t i = 0; i < MAX_DETECTIONS; i++) { - if (bboxes[i].empty()) { - if (i == 0) { - std::cout << "No detections" << std::endl; - } - break; - } - const int c_y = bboxes[i].y + bboxes[i].height / 2; - const int c_x = bboxes[i].x + bboxes[i].width / 2; - const float cam_relative_pitch = - atan2(c_y - cam_cy, focal_length_vertical); - const float phi = cam_relative_pitch + cam_pitch; - const float distance = pinhole_height / sin(phi); - const std::string& class_name = class_names[class_ids[i]]; - const float cam_relative_yaw = - atan2(c_x - cam_cx, focal_length_horizontal); - target_pose_cam_relative = { - frc::Translation3d{ - units::meter_t{distance * cos(cam_relative_pitch) * - cos(cam_relative_yaw)}, - units::meter_t{distance * cos(cam_relative_pitch) * - sin(cam_relative_yaw)}, - units::meter_t{distance * -sin(cam_relative_pitch)}}, - frc::Rotation3d{0_rad, units::radian_t{cam_relative_pitch}, - units::radian_t{-cam_relative_yaw}}}; - target_pose_robot_relative = - cam_pose.TransformBy(target_pose_cam_relative); - if (class_name == "coral") { - coral_pub.Set(target_pose_robot_relative.ToPose2d()); - } else { - algae_pub.Set(target_pose_robot_relative.ToPose2d()); - } - if (debug) { - std::cout << "\tc_y:\t" << c_y << "\tcam_relative_pitch:\t" - << cam_relative_pitch << "\tphi:\t" << phi << "\tyaw:\t" - << cam_relative_yaw << std::endl; - std::cout << "Detected a " << class_name << " " << distance - << " meters away" << std::endl; - std::cout << "TargetPose: " << target_pose_cam_relative << std::endl; - std::cout << "Robot_relative: " << target_pose_robot_relative - << std::endl; - } - } - } -} - -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 coral_table = - inst.GetTable("Orin/Gamepiece/coral"); - std::shared_ptr algae_table = - inst.GetTable("Orin/Gamepiece/algae"); - nt::StructTopic coral_topic = - coral_table->GetStructTopic("Pose"); - nt::StructTopic algae_topic = - algae_table->GetStructTopic("Pose"); - - std::vector camera_threads; - camera_threads.emplace_back( - run_gamepiece_detect, std::ref(color_model), - std::ref(model_info.class_names), - std::make_unique(), 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(); -} diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 8a1c4c9..455c992 100644 --- a/src/test/CMakeLists.txt +++ b/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) target_link_options(nvidia_apriltag_test PRIVATE diff --git a/src/test/gamepiece_test.cc b/src/test/gamepiece_test.cc new file mode 100644 index 0000000..b0e2119 --- /dev/null +++ b/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 = std::make_shared("nvidia_apriltag_test", + camera::GetCameraStream(config)); + camera::CscoreStreamer streamer("yolo_test", 4971, 30, 1080, 1080); + + nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); + std::shared_ptr coral_table = + inst.GetTable("Orin/Gamepiece/coral"); + std::shared_ptr algae_table = + inst.GetTable("Orin/Gamepiece/algae"); + nt::StructTopic coral_topic = + coral_table->GetStructTopic("Pose"); + nt::StructTopic algae_topic = + algae_table->GetStructTopic("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(); +} From d25c46d27f36fff9aac78b3a79c0776e58282c8f Mon Sep 17 00:00:00 2001 From: yasen5 Date: Sat, 3 Jan 2026 23:17:38 +0000 Subject: [PATCH 4/4] Remove unnecessary cmake quickfix (docker chopped) --- src/test/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 455c992..08a7374 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -24,7 +24,4 @@ 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) -target_link_options(nvidia_apriltag_test PRIVATE - -Wl,-rpath-link,/bos/docker/lib -)