commit aca887895a450e11c086974baffe4fbdb9a119a4
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Apr 24 16:48:45 2021 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Jun 12 14:08:30 2021 -0700
tree 3c3bb23e98f268e4c4fb9f306919d5f9091c0392
parent 1686903051a89a961204cf9d94e7dde748dfafa7

Ignore camera-to-robot pitch/roll in y2020 localizer

Make it so that if camera pitch is significantly off, the lever arm from
the camera to the robot origin does not cause an offset in our position
estimates.

Also, add the superstructure to the drivetrain replay.

Change-Id: I7701e7f6a7cfc93045f76033effcd916e0c263df

y2020/control_loops/drivetrain/drivetrain_replay.cc

diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index fc50556..8f6ff63 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -5,8 +5,8 @@
 // in some other way. The original drivetrain status data will be on the
 // /original/drivetrain channel.
 #include "aos/configuration.h"
-#include "aos/events/logging/log_writer.h"
 #include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
 #include "aos/events/simulated_event_loop.h"
 #include "aos/init.h"
 #include "aos/json_to_flatbuffer.h"
@@ -16,6 +16,7 @@
 #include "gflags/gflags.h"
 #include "y2020/control_loops/drivetrain/drivetrain_base.h"
 #include "y2020/control_loops/drivetrain/localizer.h"
+#include "y2020/control_loops/superstructure/superstructure.h"
 
 DEFINE_string(config, "y2020/config.json",
               "Name of the config file to replay using.");
@@ -74,6 +75,10 @@
                             "frc971.control_loops.drivetrain.Status");
   reader.RemapLoggedChannel("/drivetrain",
                             "frc971.control_loops.drivetrain.Output");
+  reader.RemapLoggedChannel("/superstructure",
+                            "y2020.control_loops.superstructure.Status");
+  reader.RemapLoggedChannel("/superstructure",
+                            "y2020.control_loops.superstructure.Output");
   reader.Register();
 
   std::vector<std::unique_ptr<LoggerState>> loggers;
@@ -117,6 +122,13 @@
       y2020::control_loops::drivetrain::GetDrivetrainConfig(),
       drivetrain_event_loop.get(), &localizer);
 
+  std::unique_ptr<aos::EventLoop> superstructure_event_loop =
+      reader.event_loop_factory()->MakeEventLoop("superstructure", node);
+  superstructure_event_loop->SkipTimingReport();
+
+  y2020::control_loops::superstructure::Superstructure superstructure(
+      superstructure_event_loop.get());
+
   reader.event_loop_factory()->Run();
 
   return 0;