Make localizer listen to pi[1-3]

Also, adds pi3 things to the config.

Change-Id: I9d5d43bcddc2f2eee2ed3edf6c9ad723b7e23230
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 4b164b5..8330289 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -23,6 +23,9 @@
   return result;
 }
 
+// Indices of the pis to use.
+const std::array<std::string, 3> kPisToUse{"pi1", "pi2", "pi3"};
+
 }  // namespace
 
 Localizer::Localizer(
@@ -51,9 +54,11 @@
                            ekf_.P());
   });
 
-  image_fetchers_.emplace_back(
-      event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
-          "/pi1/camera"));
+  for (const auto &pi : kPisToUse) {
+    image_fetchers_.emplace_back(
+        event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
+            "/" + pi + "/camera"));
+  }
 
   target_selector_.set_has_target(false);
 }
@@ -89,9 +94,10 @@
                        aos::monotonic_clock::time_point now,
                        double left_encoder, double right_encoder,
                        double gyro_rate, const Eigen::Vector3d &accel) {
-  for (auto &image_fetcher : image_fetchers_) {
+  for (size_t ii = 0; ii < kPisToUse.size(); ++ii) {
+    auto &image_fetcher = image_fetchers_[ii];
     while (image_fetcher.FetchNext()) {
-      HandleImageMatch(*image_fetcher);
+      HandleImageMatch(kPisToUse[ii], *image_fetcher);
     }
   }
   ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, accel,
@@ -99,13 +105,13 @@
 }
 
 void Localizer::HandleImageMatch(
-    const frc971::vision::sift::ImageMatchResult &result) {
+    std::string_view pi, const frc971::vision::sift::ImageMatchResult &result) {
   std::chrono::nanoseconds monotonic_offset(0);
   clock_offset_fetcher_.Fetch();
   if (clock_offset_fetcher_.get() != nullptr) {
     for (const auto connection : *clock_offset_fetcher_->connections()) {
       if (connection->has_node() && connection->node()->has_name() &&
-          connection->node()->name()->string_view() == "pi1") {
+          connection->node()->name()->string_view() == pi) {
         monotonic_offset =
             std::chrono::nanoseconds(connection->monotonic_offset());
         break;
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 8636a7a..1d6f816 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -1,6 +1,8 @@
 #ifndef Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
 #define Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
 
+#include <string_view>
+
 #include "aos/containers/ring_buffer.h"
 #include "aos/events/event_loop.h"
 #include "aos/network/message_bridge_server_generated.h"
@@ -69,8 +71,9 @@
     double velocity = 0.0;  // rad/sec
   };
 
-  // Processes new image data and updates the EKF.
-  void HandleImageMatch(const frc971::vision::sift::ImageMatchResult &result);
+  // Processes new image data from the given pi and updates the EKF.
+  void HandleImageMatch(std::string_view pi,
+                        const frc971::vision::sift::ImageMatchResult &result);
 
   // Processes the most recent turret position and stores it in the turret_data_
   // buffer.