commit 09fb125c71ed65262ab142f49a860f27eaec4325
author milind-u <milind.upadhyay@gmail.com> Sat Jan 28 19:21:41 2023 -0800
committer milind-u <milind.upadhyay@gmail.com> Sun Jan 29 14:03:17 2023 -0800
tree 905fd4386ba71260e21850d9d3487e0ee93958bf
parent 60706abf801a72379e3c7b0a8db699f08f1ccb08

Use aprilrobotics for target mapping

And delete april detection from charuco lib.

Signed-off-by: Yash Chainani <yashchainani28@gmail.com>
Change-Id: I98e6b84d99b23683f0c77c959b8bcc9af9ebc5d4

y2023/vision/aprilrobotics.cc

diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 3bf0adf..fc9d799 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -15,8 +15,8 @@
       ftrace_(),
       image_callback_(event_loop, channel_name,
                       [&](cv::Mat image_color_mat,
-                          const aos::monotonic_clock::time_point /*eof*/) {
-                        HandleImage(image_color_mat);
+                          const aos::monotonic_clock::time_point eof) {
+                        HandleImage(image_color_mat, eof);
                       }),
       target_map_sender_(
           event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
@@ -66,7 +66,8 @@
   }
 }
 
-void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat) {
+void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat,
+                                        aos::monotonic_clock::time_point eof) {
   std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
       DetectTags(image_color_mat);
 
@@ -78,8 +79,8 @@
   }
   const auto target_poses_offset = builder.fbb()->CreateVector(target_poses);
   auto target_map_builder = builder.MakeBuilder<frc971::vision::TargetMap>();
-
   target_map_builder.add_target_poses(target_poses_offset);
+  target_map_builder.add_monotonic_timestamp_ns(eof.time_since_epoch().count());
   builder.CheckOk(builder.Send(target_map_builder.Finish()));
 }
 
@@ -102,10 +103,10 @@
       aos::monotonic_clock::now();
 
   image_u8_t im = {
-    .width = image.cols,
-    .height = image.rows,
-    .stride = image.cols,
-    .buf = image.data,
+      .width = image.cols,
+      .height = image.rows,
+      .stride = image.cols,
+      .buf = image.data,
   };
 
   ftrace_.FormatMessage("Starting detect\n");