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.
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 0e1a6ba..3d966b8 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -380,6 +380,42 @@
"max_size": 2000000
},
{
+ "name": "/pi3/camera",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "pi3",
+ "frequency": 25,
+ "max_size": 620000,
+ "num_senders": 18
+ },
+ {
+ "name": "/pi3/camera",
+ "type": "frc971.vision.sift.ImageMatchResult",
+ "source_node": "pi3",
+ "frequency": 25,
+ "max_size": 10000,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi3/camera/detailed",
+ "type": "frc971.vision.sift.ImageMatchResult",
+ "source_node": "pi3",
+ "frequency": 25,
+ "max_size": 300000
+ },
+ {
+ "name": "/pi3/camera",
+ "type": "frc971.vision.sift.TrainingData",
+ "source_node": "pi3",
+ "frequency": 2,
+ "max_size": 2000000
+ },
+ {
"name": "/autonomous",
"type": "aos.common.actions.Status",
"source_node": "roborio"
@@ -491,6 +527,24 @@
},
{
"match": {
+ "name": "/camera",
+ "source_node": "pi3"
+ },
+ "rename": {
+ "name": "/pi3/camera"
+ }
+ },
+ {
+ "match": {
+ "name": "/camera/detailed",
+ "source_node": "pi3"
+ },
+ "rename": {
+ "name": "/pi3/camera/detailed"
+ }
+ },
+ {
+ "match": {
"name": "/aos",
"type": "aos.RobotState"
},