commit 68965cd7ceb6f32d41e2fcbebb50b308e4578a98
author Jim Ostrowski <yimmy13@gmail.com> Wed Mar 01 20:32:51 2023 -0800
committer Jim Ostrowski <yimmy13@gmail.com> Fri Mar 03 22:14:55 2023 -0800
tree d35f73c564846e818ab3407693ec490204ce3e60
parent aec5dd27328aaf4f8c520fd095261ee63286213a

Enable target_mapping to write out to mcap log

Change-Id: I36c1da7c27b75947324d84f950d2eb417a1eea49
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>

y2023/vision/BUILD

diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 9bd29fa..94803e1 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -75,6 +75,7 @@
         "//aos:init",
         "//aos/events:simulated_event_loop",
         "//aos/events/logging:log_reader",
+        "//aos/util:mcap_logger",
         "//frc971/constants:constants_sender_lib",
         "//frc971/control_loops:pose",
         "//frc971/vision:calibration_fbs",

y2023/vision/target_mapping.cc

diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 8861e2e..c59414b 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -2,6 +2,7 @@
 #include "aos/events/logging/log_reader.h"
 #include "aos/events/simulated_event_loop.h"
 #include "aos/init.h"
+#include "aos/util/mcap_logger.h"
 #include "frc971/control_loops/pose.h"
 #include "frc971/vision/calibration_generated.h"
 #include "frc971/vision/charuco_lib.h"
@@ -29,6 +30,8 @@
               "Field name, for the output json filename and flatbuffer field");
 DEFINE_int32(team_number, 7971,
              "Use the calibration for a node with this team number");
+DEFINE_string(mcap_output_path, "/tmp/log.mcap", "Log to output.");
+DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
 
 namespace y2023 {
 namespace vision {
@@ -178,6 +181,25 @@
   HandlePiCaptures(pi4_detection_event_loop.get(), pi4_mapping_event_loop.get(),
                    &reader, &timestamped_target_detections, &detectors);
 
+  std::unique_ptr<aos::EventLoop> mcap_event_loop;
+  std::unique_ptr<aos::McapLogger> relogger;
+  if (!FLAGS_mcap_output_path.empty()) {
+    LOG(INFO) << "Writing out mcap file to " << FLAGS_mcap_output_path;
+    // TODO: Should make this work for any pi
+    const aos::Node *node =
+        aos::configuration::GetNode(reader.configuration(), FLAGS_pi);
+    reader.event_loop_factory()->GetNodeEventLoopFactory(node)->OnStartup(
+        [&relogger, &mcap_event_loop, &reader, node]() {
+          mcap_event_loop =
+              reader.event_loop_factory()->MakeEventLoop("mcap", node);
+          relogger = std::make_unique<aos::McapLogger>(
+              mcap_event_loop.get(), FLAGS_mcap_output_path,
+              aos::McapLogger::Serialization::kFlatbuffer,
+              aos::McapLogger::CanonicalChannelNames::kShortened,
+              aos::McapLogger::Compression::kLz4);
+        });
+  }
+
   reader.event_loop_factory()->Run();
 
   auto target_constraints =