commit 52db336330a700da6c1712eab4a0c209f13b714f
author milind-u <milind.upadhyay@gmail.com> Sat Mar 25 20:09:22 2023 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sat Mar 25 22:01:03 2023 -0700
tree f7569b58588882a75ca66d133b47a1ca366c73d7
parent 42b0865f39d3880f3fac278643e0e0e066e5b10d

Reset target mapping image when there aren't poses

If we haven't gotten anything for a while, clear the display so we
aren't looking at stale estimates.

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

y2023/vision/target_mapping.cc

diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index e9ef662..4da972a 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -81,7 +81,8 @@
                      Eigen::Affine3d extrinsics, std::string node_name,
                      frc971::vision::TargetMapper target_loc_mapper,
                      std::set<std::string> *drawn_nodes,
-                     VisualizeRobot *vis_robot, size_t *display_count) {
+                     VisualizeRobot *vis_robot, size_t *display_count,
+                     aos::distributed_clock::time_point *last_draw_time) {
   bool drew = false;
   std::stringstream label;
   label << node_name << " - ";
@@ -167,6 +168,7 @@
                                std::to_string(target_pose_fbs->id()),
                                kPiColors.at(node_name));
       drew = true;
+      *last_draw_time = pi_distributed_time;
     }
   }
   if (drew) {
@@ -177,6 +179,13 @@
                 kPiColors.at(node_name));
 
     drawn_nodes->emplace(node_name);
+  } else if (pi_distributed_time - *last_draw_time >
+                 std::chrono::milliseconds(30) &&
+             *display_count >= FLAGS_skip_to) {
+    // Clear the image if we haven't draw in a while
+    vis_robot->ClearImage();
+    cv::imshow("View", vis_robot->image_);
+    cv::waitKey(FLAGS_wait_key);
   }
 }
 
@@ -188,7 +197,7 @@
         *timestamped_target_detections,
     std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors,
     std::set<std::string> *drawn_nodes, VisualizeRobot *vis_robot,
-    size_t *display_count) {
+    size_t *display_count, aos::distributed_clock::time_point *last_draw_time) {
   static constexpr std::string_view kImageChannel = "/camera";
   // For the robots, we need to flip the image since the cameras are rolled by
   // 180 degrees
@@ -217,7 +226,7 @@
     std::string node_name = detection_event_loop->node()->name()->str();
     HandleAprilTags(map, pi_distributed_time, timestamped_target_detections,
                     extrinsics, node_name, target_loc_mapper, drawn_nodes,
-                    vis_robot, display_count);
+                    vis_robot, display_count, last_draw_time);
   });
 }
 
@@ -260,6 +269,8 @@
   VisualizeRobot vis_robot;
   std::set<std::string> drawn_nodes;
   size_t display_count = 0;
+  aos::distributed_clock::time_point last_draw_time =
+      aos::distributed_clock::min_time;
 
   const aos::Node *pi1 =
       aos::configuration::GetNode(reader.configuration(), "pi1");
@@ -269,7 +280,7 @@
       reader.event_loop_factory()->MakeEventLoop("pi1_mapping", pi1);
   HandlePiCaptures(pi1_detection_event_loop.get(), pi1_mapping_event_loop.get(),
                    &reader, &timestamped_target_detections, &detectors,
-                   &drawn_nodes, &vis_robot, &display_count);
+                   &drawn_nodes, &vis_robot, &display_count, &last_draw_time);
 
   const aos::Node *pi2 =
       aos::configuration::GetNode(reader.configuration(), "pi2");
@@ -279,7 +290,7 @@
       reader.event_loop_factory()->MakeEventLoop("pi2_mapping", pi2);
   HandlePiCaptures(pi2_detection_event_loop.get(), pi2_mapping_event_loop.get(),
                    &reader, &timestamped_target_detections, &detectors,
-                   &drawn_nodes, &vis_robot, &display_count);
+                   &drawn_nodes, &vis_robot, &display_count, &last_draw_time);
 
   const aos::Node *pi3 =
       aos::configuration::GetNode(reader.configuration(), "pi3");
@@ -289,7 +300,7 @@
       reader.event_loop_factory()->MakeEventLoop("pi3_mapping", pi3);
   HandlePiCaptures(pi3_detection_event_loop.get(), pi3_mapping_event_loop.get(),
                    &reader, &timestamped_target_detections, &detectors,
-                   &drawn_nodes, &vis_robot, &display_count);
+                   &drawn_nodes, &vis_robot, &display_count, &last_draw_time);
 
   const aos::Node *pi4 =
       aos::configuration::GetNode(reader.configuration(), "pi4");
@@ -299,7 +310,7 @@
       reader.event_loop_factory()->MakeEventLoop("pi4_mapping", pi4);
   HandlePiCaptures(pi4_detection_event_loop.get(), pi4_mapping_event_loop.get(),
                    &reader, &timestamped_target_detections, &detectors,
-                   &drawn_nodes, &vis_robot, &display_count);
+                   &drawn_nodes, &vis_robot, &display_count, &last_draw_time);
 
   std::unique_ptr<aos::EventLoop> mcap_event_loop;
   std::unique_ptr<aos::McapLogger> relogger;