Update constants and vision in y2024_swerve
Copy most of constants from y2024 and change to y2024_swerve instead of y2024
Set specific constants values from constants.cc file in the common.jinja2 and constants.fbs
Copy over vision from y2024 and changed files to use y2024_swerve
Change-Id: I3f27667e838e1a454ee1d21a5d4e9cee79565054
Signed-off-by: Yash Maheshwari <yashmahe2018@gmail.com>
diff --git a/y2024_swerve/vision/vision_util.h b/y2024_swerve/vision/vision_util.h
new file mode 100644
index 0000000..6fed3c8
--- /dev/null
+++ b/y2024_swerve/vision/vision_util.h
@@ -0,0 +1,41 @@
+#ifndef Y2024_SWERVE_VISION_VISION_UTIL_H_
+#define Y2024_SWERVE_VISION_VISION_UTIL_H_
+#include <map>
+#include <string_view>
+
+#include "opencv2/imgproc.hpp"
+
+#include "y2024_swerve/constants/constants_generated.h"
+
+namespace y2024_swerve::vision {
+
+// Generate unique colors for each camera
+const auto kOrinColors = std::map<std::string, cv::Scalar>{
+ {"/orin1/camera0", cv::Scalar(255, 0, 255)},
+ {"/orin1/camera1", cv::Scalar(255, 255, 0)},
+ {"/imu/camera0", cv::Scalar(0, 255, 255)},
+ {"/imu/camera1", cv::Scalar(255, 165, 0)},
+};
+
+// Structure to store node name (e.g., orin1, imu), number, and a usable string
+struct CameraNode {
+ std::string node_name;
+ int camera_number;
+
+ inline const std::string camera_name() const {
+ return "/" + node_name + "/camera" + std::to_string(camera_number);
+ }
+};
+
+std::vector<CameraNode> CreateNodeList();
+
+std::map<std::string, int> CreateOrderingMap(
+ std::vector<CameraNode> &node_list);
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024_swerve::Constants &calibration_data, std::string_view node_name,
+ int camera_number);
+
+} // namespace y2024_swerve::vision
+
+#endif // y2024_SWERVE_VISION_VISION_UTIL_H_