Write april tag image coordinates to flatbuffer
Signed-off-by: Yash Chainani <yashchainani28@gmail.com>
Change-Id: I073e362b58dc05fb917d9ed5ad0d923cbdf7da6e
diff --git a/y2023/BUILD b/y2023/BUILD
index 0c34ce8..20092eb 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -69,6 +69,7 @@
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
+ "//y2023/vision:april_debug_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 2ddb735..96ea632 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,3 +1,5 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
py_binary(
name = "create_calib_file",
srcs = [
@@ -101,6 +103,14 @@
],
)
+flatbuffer_cc_library(
+ name = "april_debug_fbs",
+ srcs = ["april_debug.fbs"],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "aprilrobotics_lib",
srcs = [
@@ -110,6 +120,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2023:__subpackages__"],
deps = [
+ ":april_debug_fbs",
":calibration_data",
"//aos:init",
"//aos/events:shm_event_loop",
diff --git a/y2023/vision/april_debug.fbs b/y2023/vision/april_debug.fbs
new file mode 100644
index 0000000..4442448
--- /dev/null
+++ b/y2023/vision/april_debug.fbs
@@ -0,0 +1,21 @@
+namespace y2023.vision;
+
+// Stores image xy pixel coordinates of apriltag corners
+struct Point {
+ x:double (id: 0);
+ y:double (id: 1);
+}
+
+// Corner locations for each apriltag
+table AprilCorners {
+ id:uint64 (id: 0);
+ // Will always have 4 values, one for each corner
+ points: [Point] (id: 1);
+}
+
+// List of positions of all apriltags detected in current frame
+table AprilDebug {
+ corners: [AprilCorners] (id: 0);
+}
+
+root_type AprilDebug;
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index d20a247..fbbc01d 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -19,7 +19,9 @@
HandleImage(image_color_mat, eof);
}),
target_map_sender_(
- event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
+ event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
+ april_debug_sender_(
+ event_loop->MakeSender<y2023::vision::AprilDebug>("/camera")) {
tag_family_ = tag16h5_create();
tag_detector_ = apriltag_detector_create();
@@ -120,6 +122,10 @@
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
+ std::vector<flatbuffers::Offset<AprilCorners>> corners_vector;
+
+ auto builder = april_debug_sender_.MakeBuilder();
+
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
@@ -155,9 +161,35 @@
before_pose_estimation)
.count()
<< " seconds for pose estimation";
+
+ std::vector<Point> corner_points;
+
+ corner_points.emplace_back(det->p[0][0], det->p[0][1]);
+ corner_points.emplace_back(det->p[1][0], det->p[1][1]);
+ corner_points.emplace_back(det->p[2][0], det->p[2][1]);
+ corner_points.emplace_back(det->p[3][0], det->p[3][1]);
+
+ auto corner_points_fbs =
+ builder.fbb()->CreateVectorOfStructs(corner_points);
+
+ AprilCorners::Builder april_corners_builder =
+ builder.MakeBuilder<AprilCorners>();
+
+ april_corners_builder.add_id(det->id);
+ april_corners_builder.add_points(corner_points_fbs);
+
+ corners_vector.emplace_back(april_corners_builder.Finish());
}
}
+ auto corners_vector_fbs = builder.fbb()->CreateVector(corners_vector);
+
+ AprilDebug::Builder april_debug_builder = builder.MakeBuilder<AprilDebug>();
+
+ april_debug_builder.add_corners(corners_vector_fbs);
+
+ builder.CheckOk(builder.Send(april_debug_builder.Finish()));
+
apriltag_detections_destroy(detections);
const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index a68b1d9..d5172c7 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -15,6 +15,7 @@
#include "third_party/apriltag/apriltag.h"
#include "third_party/apriltag/apriltag_pose.h"
#include "third_party/apriltag/tag16h5.h"
+#include "y2023/vision/april_debug_generated.h"
#include "y2023/vision/calibration_data.h"
DECLARE_int32(team_number);
@@ -95,6 +96,7 @@
frc971::vision::ImageCallback image_callback_;
aos::Sender<frc971::vision::TargetMap> target_map_sender_;
+ aos::Sender<y2023::vision::AprilDebug> april_debug_sender_;
};
} // namespace vision
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index fac37b2..881af55 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -203,6 +203,15 @@
]
},
{
+ "name": "/pi{{ NUM }}/camera",
+ "type": "y2023.vision.AprilDebug",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 40,
+ "num_senders": 2,
+ "max_size": 40000,
+ "logger": "LOCAL_LOGGER"
+ },
+ {
"name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/frc971-vision-TargetMap",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 80,