Refactor of apriltag detector to send out tags
Split into app in y2024, and libs in frc971
Added in calibration constants to y2024, including single basic camera
Modified team number parsing to work with orins
Pulled out year-by-year calibration call to a single FindCameraCalibration
call, and rest (extrinsics, intrinsics) are year-agnostic
Moving camera calibration files into constants/calib_files directory, instead
of vision/calib_files
Change-Id: Ic485086aad9665d2b571551a5afb337c9254d690
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/y2024/BUILD b/y2024/BUILD
index b362f6c..148c23d 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -53,6 +53,7 @@
"//aos/starter:irq_affinity",
"//aos/util:foxglove_websocket",
"//frc971/image_streamer:image_streamer",
+ "//frc971/vision:intrinsics_calibration",
"//y2023/vision:viewer",
"//y2024/constants:constants_sender",
"//y2024/vision:foxglove_image_converter",
@@ -75,8 +76,8 @@
"//aos/network:web_proxy_main",
"//aos/starter:irq_affinity",
"//frc971/orin:argus_camera",
- "//frc971/orin:gpu_apriltag",
"//y2024/orin:can_logger",
+ "//y2024/vision:apriltag_detector",
"//y2024/vision:image_logger",
],
target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 7e04438..a061f39 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -1,6 +1,11 @@
{% from 'y2024/constants/common.jinja2' import intake_pivot_zero %}
{
+ "cameras": [
+ {
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json' %}
+ }
+ ],
"robot": {
"intake_constants": {
{% set _ = intake_pivot_zero.update(
diff --git a/y2024/constants/BUILD b/y2024/constants/BUILD
index 078d616..18e506a 100644
--- a/y2024/constants/BUILD
+++ b/y2024/constants/BUILD
@@ -41,6 +41,7 @@
"9971.json",
"common.jinja2",
"common.json",
+ "//y2024/constants/calib_files",
"//y2024/control_loops/drivetrain:drivetrain_config.json",
"//y2024/control_loops/superstructure/intake_pivot:intake_pivot_json",
"//y2024/vision/maps",
@@ -56,6 +57,7 @@
deps = [
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config_fbs",
+ "//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/zeroing:constants_fbs",
],
diff --git a/y2024/constants/calib_files/BUILD b/y2024/constants/calib_files/BUILD
new file mode 100644
index 0000000..1f33d50
--- /dev/null
+++ b/y2024/constants/calib_files/BUILD
@@ -0,0 +1,5 @@
+filegroup(
+ name = "calib_files",
+ srcs = glob(["*.json"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json b/y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json
new file mode 100644
index 0000000..e083a13
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json
@@ -0,0 +1 @@
+{ "node_name": "orin1", "team_number": 971, "intrinsics": [ 893.617798, 0.0, 612.44397, 0.0, 893.193115, 375.196381, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.483961, 0.220781, 0.84678, 0.176109, 0.868846, 0.005849, 0.495048, -0.231149, 0.104344, 0.975306, -0.194656, 0.550508, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.443805, 0.238734, 0.000133, 0.000448, -0.071068 ], "calibration_timestamp": 1358499779650270322, "camera_id": "23-09" }
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index ee5ff60..e648df0 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -1,3 +1,4 @@
+include "frc971/vision/calibration.fbs";
include "frc971/vision/target_map.fbs";
include "frc971/control_loops/profiled_subsystem.fbs";
include "frc971/zeroing/constants.fbs";
@@ -5,6 +6,10 @@
namespace y2024;
+table CameraConfiguration {
+ calibration:frc971.vision.calibration.CameraCalibration (id: 0);
+}
+
table ShotParams {
shot_velocity: double (id: 0);
shot_angle: double (id: 1);
@@ -63,6 +68,7 @@
table Constants {
robot:RobotConstants (id: 0);
common:Common (id: 1);
+ cameras:[CameraConfiguration] (id: 2);
}
root_type Constants;
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
index 632de2b..795d9fa 100644
--- a/y2024/vision/BUILD
+++ b/y2024/vision/BUILD
@@ -33,3 +33,28 @@
"@com_github_google_glog//:glog",
],
)
+
+cc_binary(
+ name = "apriltag_detector",
+ srcs = [
+ "apriltag_detector.cc",
+ "vision_util.cc",
+ "vision_util.h",
+ ],
+ features = ["cuda"],
+ target_compatible_with = ["@platforms//cpu:arm64"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/orin:gpu_apriltag_lib",
+ "//third_party:cudart",
+ "//third_party/apriltag",
+ "//y2024/constants:constants_fbs",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ "@com_github_nvidia_cccl//:cccl",
+ "@com_github_nvidia_cuco//:cuco",
+ ],
+)
diff --git a/y2024/vision/apriltag_detector.cc b/y2024/vision/apriltag_detector.cc
new file mode 100644
index 0000000..96ff869
--- /dev/null
+++ b/y2024/vision/apriltag_detector.cc
@@ -0,0 +1,49 @@
+
+#include <string>
+
+#include "aos/init.h"
+#include "frc971/orin/gpu_apriltag.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/vision/vision_util.h"
+
+DEFINE_string(channel, "/camera", "Channel name");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+void GpuApriltagDetector() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ const frc971::constants::ConstantsFetcher<y2024::Constants> calibration_data(
+ &event_loop);
+
+ const frc971::vision::calibration::CameraCalibration *calibration =
+ y2024::vision::FindCameraCalibration(
+ calibration_data.constants(),
+ event_loop.node()->name()->string_view());
+
+ frc971::apriltag::ApriltagDetector detector(&event_loop, FLAGS_channel,
+ calibration);
+
+ // TODO(austin): Figure out our core pinning strategy.
+ // event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+
+ LOG(INFO) << "Setting scheduler priority";
+ struct sched_param param;
+ param.sched_priority = 21;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0);
+
+ LOG(INFO) << "Running event loop";
+ // TODO(austin): Pre-warm it...
+ event_loop.Run();
+} // namespace frc971::apriltag
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ GpuApriltagDetector();
+
+ return 0;
+}
diff --git a/y2024/vision/vision_util.cc b/y2024/vision/vision_util.cc
new file mode 100644
index 0000000..4c196ce
--- /dev/null
+++ b/y2024/vision/vision_util.cc
@@ -0,0 +1,21 @@
+#include "y2024/vision/vision_util.h"
+
+#include "glog/logging.h"
+
+namespace y2024::vision {
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024::Constants &calibration_data, std::string_view node_name) {
+ CHECK(calibration_data.has_cameras());
+ for (const y2024::CameraConfiguration *candidate :
+ *calibration_data.cameras()) {
+ CHECK(candidate->has_calibration());
+ if (candidate->calibration()->node_name()->string_view() != node_name) {
+ continue;
+ }
+ return candidate->calibration();
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
+}
+
+} // namespace y2024::vision
diff --git a/y2024/vision/vision_util.h b/y2024/vision/vision_util.h
new file mode 100644
index 0000000..d8fa562
--- /dev/null
+++ b/y2024/vision/vision_util.h
@@ -0,0 +1,16 @@
+#ifndef Y2024_VISION_VISION_UTIL_H_
+#define Y2024_VISION_VISION_UTIL_H_
+#include <string_view>
+
+#include "opencv2/imgproc.hpp"
+
+#include "y2024/constants/constants_generated.h"
+
+namespace y2024::vision {
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024::Constants &calibration_data, std::string_view node_name);
+
+} // namespace y2024::vision
+
+#endif // Y2024_VISION_VISION_UTIL_H_
diff --git a/y2024/y2024_orin_template.json b/y2024/y2024_orin_template.json
index 04a63f0..6d378f7 100644
--- a/y2024/y2024_orin_template.json
+++ b/y2024/y2024_orin_template.json
@@ -265,8 +265,8 @@
]
},
{
- "name": "gpu_apriltag",
- "executable_name": "gpu_apriltag",
+ "name": "apriltag_detector",
+ "executable_name": "apriltag_detector",
"user": "pi",
"nodes": [
"orin{{ NUM }}"