Manage monotonic offsets correctly in y2020 localizer
Currently only supports a single pi, but expanding to pi[2-5] shouldn't
be difficult.
Change-Id: Ibed8bd5a59e1a5a9971e9b60c81da413c21b4f7e
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 03d2dc2..1bd760f 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -59,6 +59,7 @@
hdrs = ["localizer.h"],
deps = [
"//aos/containers:ring_buffer",
+ "//aos/network:message_bridge_server_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:hybrid_ekf",
"//frc971/control_loops/drivetrain:localizer",
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 4b11f6f..b8463d6 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -22,13 +22,19 @@
}
return result;
}
+
} // namespace
Localizer::Localizer(
aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
&dt_config)
- : event_loop_(event_loop), dt_config_(dt_config), ekf_(dt_config) {
+ : event_loop_(event_loop),
+ dt_config_(dt_config),
+ ekf_(dt_config),
+ clock_offset_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/aos")) {
// TODO(james): This doesn't really need to be a watcher; we could just use a
// fetcher for the superstructure status.
// This probably should be a Fetcher instead of a Watcher, but this
@@ -89,9 +95,21 @@
void Localizer::HandleImageMatch(
const frc971::vision::sift::ImageMatchResult &result) {
- // TODO(james): compensate for capture time across multiple nodes correctly.
+ 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") {
+ monotonic_offset =
+ std::chrono::nanoseconds(connection->monotonic_offset());
+ break;
+ }
+ }
+ }
aos::monotonic_clock::time_point capture_time(
- std::chrono::nanoseconds(result.image_monotonic_timestamp_ns()));
+ std::chrono::nanoseconds(result.image_monotonic_timestamp_ns()) -
+ monotonic_offset);
CHECK(result.has_camera_calibration());
// Per the ImageMatchResult specification, we can actually determine whether
// the camera is the turret camera just from the presence of the
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 0b79361..8636a7a 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -3,6 +3,7 @@
#include "aos/containers/ring_buffer.h"
#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
@@ -86,6 +87,8 @@
std::vector<aos::Fetcher<frc971::vision::sift::ImageMatchResult>>
image_fetchers_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
+
// Buffer of recent turret data--this is used so that when we receive a camera
// frame from the turret, we can back out what the turret angle was at that
// time.
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 1e72c39..e05a810 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -4,6 +4,7 @@
#include "aos/controls/control_loop_test.h"
#include "aos/events/logging/logger.h"
+#include "aos/network/message_bridge_server_generated.h"
#include "aos/network/team_number.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
@@ -84,6 +85,8 @@
locations.push_back(H);
return locations;
}
+
+constexpr std::chrono::seconds kPiTimeOffset(10);
} // namespace
namespace chrono = std::chrono;
@@ -113,6 +116,9 @@
superstructure_status_sender_(
test_event_loop_->MakeSender<superstructure::Status>(
"/superstructure")),
+ server_statistics_sender_(
+ test_event_loop_->MakeSender<aos::message_bridge::ServerStatistics>(
+ "/aos")),
drivetrain_event_loop_(MakeEventLoop("drivetrain", roborio_)),
dt_config_(GetTest2020DrivetrainConfig()),
pi1_event_loop_(MakeEventLoop("test", pi1_)),
@@ -123,6 +129,9 @@
drivetrain_plant_event_loop_(MakeEventLoop("plant", roborio_)),
drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
last_frame_(monotonic_now()) {
+ event_loop_factory()->GetNodeEventLoopFactory(pi1_)->SetDistributedOffset(
+ kPiTimeOffset);
+
set_team_id(frc971::control_loops::testing::kTeamNumber);
SetStartingPosition({3.0, 2.0, 0.0});
set_battery_voltage(12.0);
@@ -151,6 +160,29 @@
test_event_loop_->AddPhasedLoop(
[this](int) {
+ auto builder = server_statistics_sender_.MakeBuilder();
+ auto name_offset = builder.fbb()->CreateString("pi1");
+ auto node_builder = builder.MakeBuilder<aos::Node>();
+ node_builder.add_name(name_offset);
+ auto node_offset = node_builder.Finish();
+ auto connection_builder =
+ builder.MakeBuilder<aos::message_bridge::ServerConnection>();
+ connection_builder.add_node(node_offset);
+ connection_builder.add_monotonic_offset(
+ chrono::duration_cast<chrono::nanoseconds>(-kPiTimeOffset)
+ .count());
+ auto connection_offset = connection_builder.Finish();
+ auto connections_offset =
+ builder.fbb()->CreateVector(&connection_offset, 1);
+ auto statistics_builder =
+ builder.MakeBuilder<aos::message_bridge::ServerStatistics>();
+ statistics_builder.add_connections(connections_offset);
+ builder.Send(statistics_builder.Finish());
+ },
+ chrono::milliseconds(500));
+
+ test_event_loop_->AddPhasedLoop(
+ [this](int) {
// Also use the opportunity to send out turret messages.
UpdateTurretPosition();
auto builder = superstructure_status_sender_.MakeBuilder();
@@ -258,7 +290,10 @@
frame->image_monotonic_timestamp_ns =
chrono::duration_cast<chrono::nanoseconds>(
- monotonic_now().time_since_epoch())
+ event_loop_factory()
+ ->GetNodeEventLoopFactory(pi1_)
+ ->monotonic_now()
+ .time_since_epoch())
.count();
frame->camera_calibration.reset(new CameraCalibrationT());
{
@@ -301,6 +336,7 @@
aos::Fetcher<Goal> drivetrain_goal_fetcher_;
aos::Sender<LocalizerControl> localizer_control_sender_;
aos::Sender<superstructure::Status> superstructure_status_sender_;
+ aos::Sender<aos::message_bridge::ServerStatistics> server_statistics_sender_;
std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double>