commit 7aa29e2769a0de6465f625ffea8f4c3df65fca2e
author milind-u <milind.upadhyay@gmail.com> Thu Feb 23 20:22:01 2023 -0800
committer milind-u <milind.upadhyay@gmail.com> Thu Feb 23 20:26:55 2023 -0800
tree 4b01644590114f65f7e3a630d2707ffb4764bcca
parent ee67abd01eb5bf1a1b94941379ec50fb33b9bee6

Send april corners for foxglove displaying

Deleted AprilDebug and switched to foxglove image annotations.
Also made the displayed points a more visible color (pink).

Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ifb89784d00c80406237587acf17c40424632069b

y2023/vision/aprilrobotics.cc

diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 07eee64..b132f85 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -27,8 +27,8 @@
           chrono::milliseconds(5)),
       target_map_sender_(
           event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
-      april_debug_sender_(
-          event_loop->MakeSender<y2023::vision::AprilDebug>("/camera")) {
+      image_annotations_sender_(
+          event_loop->MakeSender<foxglove::ImageAnnotations>("/camera")) {
   tag_family_ = tag16h5_create();
   tag_detector_ = apriltag_detector_create();
 
@@ -80,7 +80,7 @@
 void AprilRoboticsDetector::HandleImage(cv::Mat image_grayscale,
                                         aos::monotonic_clock::time_point eof) {
   std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
-      DetectTags(image_grayscale);
+      DetectTags(image_grayscale, eof);
 
   auto builder = target_map_sender_.MakeBuilder();
   std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
@@ -136,7 +136,8 @@
 }
 
 std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
-AprilRoboticsDetector::DetectTags(cv::Mat image) {
+AprilRoboticsDetector::DetectTags(cv::Mat image,
+                                  aos::monotonic_clock::time_point eof) {
   const aos::monotonic_clock::time_point start_time =
       aos::monotonic_clock::now();
 
@@ -153,9 +154,9 @@
 
   std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
 
-  std::vector<flatbuffers::Offset<AprilCorners>> corners_vector;
+  std::vector<std::vector<cv::Point2f>> corners_vector;
 
-  auto builder = april_debug_sender_.MakeBuilder();
+  auto builder = image_annotations_sender_.MakeBuilder();
 
   for (int i = 0; i < zarray_size(detections); i++) {
     apriltag_detection_t *det;
@@ -195,33 +196,19 @@
                      .count()
               << " seconds for pose estimation";
 
-      std::vector<Point> corner_points;
-
+      std::vector<cv::Point2f> 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());
+      corners_vector.emplace_back(corner_points);
     }
   }
 
-  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()));
+  const auto annotations_offset =
+      frc971::vision::BuildAnnotations(eof, corners_vector, 5.0, builder.fbb());
+  builder.CheckOk(builder.Send(annotations_offset));
 
   apriltag_detections_destroy(detections);