Update y2020 configs with 5th pi + timestamp channels
Also, update the drivetrain_replay_test to go with it.
Change-Id: Ic88b2f79b14b260e345cfec4f3e5ce3e16a5bd2f
diff --git a/WORKSPACE b/WORKSPACE
index e7de0e2..285b1f0 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -723,11 +723,17 @@
urls = ["https://www.frc971.org/Build-Dependencies/small_sample_logfile.fbs"],
)
-http_file(
+http_archive(
name = "drivetrain_replay",
- downloaded_file_path = "spinning_wheels_while_still.bfbs",
- sha256 = "8abe3bbf7ac7a3ab37ad8a313ec22fc244899d916f5e9037100b02e242f5fb45",
- urls = ["https://www.frc971.org/Build-Dependencies/spinning_wheels_while_still4.bfbs"],
+ build_file_content = """
+filegroup(
+ name = "drivetrain_replay",
+ srcs = glob(["**/*.bfbs"]),
+ visibility = ["//visibility:public"],
+)
+ """,
+ sha256 = "115dcd2fe005cb9cad3325707aa7f4466390c43a08555edf331c06c108bdf692",
+ url = "https://www.frc971.org/Build-Dependencies/2021-03-20_drivetrain_spin_wheels.tar.gz",
)
# OpenCV armhf (for raspberry pi)
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
index 9e406e9..e188bf2 100644
--- a/aos/events/logging/logfile_sorting.cc
+++ b/aos/events/logging/logfile_sorting.cc
@@ -80,6 +80,12 @@
closedir(directory);
}
+std::vector<std::string> FindLogs(std::string filename) {
+ std::vector<std::string> files;
+ FindLogs(&files, filename);
+ return files;
+}
+
std::vector<std::string> FindLogs(int argc, char **argv) {
std::vector<std::string> found_logfiles;
diff --git a/aos/events/logging/logfile_sorting.h b/aos/events/logging/logfile_sorting.h
index 95a6431..3b4422f 100644
--- a/aos/events/logging/logfile_sorting.h
+++ b/aos/events/logging/logfile_sorting.h
@@ -93,6 +93,10 @@
// them to the vector.
void FindLogs(std::vector<std::string> *files, std::string filename);
+// Recursively searches the file/folder for .bfbs and .bfbs.xz files and returns
+// them in a vector.
+std::vector<std::string> FindLogs(std::string filename);
+
// Recursively searches for logfiles in argv[1] and onward.
std::vector<std::string> FindLogs(int argc, char **argv);
diff --git a/aos/network/timestamp_channel.cc b/aos/network/timestamp_channel.cc
index d022b60..bc8c1d9 100644
--- a/aos/network/timestamp_channel.cc
+++ b/aos/network/timestamp_channel.cc
@@ -42,7 +42,7 @@
RemoteMessage::GetFullyQualifiedName(), name_, node_, true);
if (shared_timestamp_channel != nullptr) {
LOG(WARNING) << "Failed to find timestamp channel {\"name\": \""
- << split_timestamp_channel << "\", \"type\": \""
+ << split_timestamp_channel_name << "\", \"type\": \""
<< RemoteMessage::GetFullyQualifiedName()
<< "\"}, falling back to old version.";
return shared_timestamp_channel;
diff --git a/y2020/BUILD b/y2020/BUILD
index cd001a1..024b6ac 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -152,6 +152,7 @@
":config_pi2",
":config_pi3",
":config_pi4",
+ ":config_pi5",
":config_roborio",
],
flatbuffers = [
@@ -191,6 +192,7 @@
"pi2",
"pi3",
"pi4",
+ "pi5",
]
]
@@ -289,5 +291,5 @@
parameters = {"NUM": str(num)},
target_compatible_with = ["@platforms//os:linux"],
)
- for num in range(1, 5)
+ for num in range(1, 6)
]
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 1b98592..75a8475 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -117,16 +117,6 @@
],
)
-aos_config(
- name = "replay_config",
- src = "drivetrain_replay_config.json",
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//y2020:config",
- ],
-)
-
cc_test(
name = "localizer_test",
srcs = ["localizer_test.cc"],
@@ -150,8 +140,8 @@
name = "drivetrain_replay_test",
srcs = ["drivetrain_replay_test.cc"],
data = [
- ":replay_config",
- "@drivetrain_replay//file:spinning_wheels_while_still.bfbs",
+ "//y2020:config",
+ "@drivetrain_replay",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_config.json b/y2020/control_loops/drivetrain/drivetrain_replay_config.json
deleted file mode 100644
index 987e55b..0000000
--- a/y2020/control_loops/drivetrain/drivetrain_replay_config.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
- "channels": [
- {
- "name": "/drivetrain",
- "type": "frc971.IMUValues",
- "frequency": 2000,
- "source_node": "roborio"
- }
- ],
- "imports": [
- "../../y2020.json"
- ]
-}
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index 2e6b327..e6a3f6e 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -22,9 +22,10 @@
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
DEFINE_string(
- logfile, "external/drivetrain_replay/file/spinning_wheels_while_still.bfbs",
+ logfile,
+ "external/drivetrain_replay/",
"Name of the logfile to read from.");
-DEFINE_string(config, "y2020/control_loops/drivetrain/replay_config.json",
+DEFINE_string(config, "y2020/config.json",
"Name of the config file to replay using.");
namespace y2020 {
@@ -36,7 +37,8 @@
public:
DrivetrainReplayTest()
: config_(aos::configuration::ReadConfig(FLAGS_config)),
- reader_(FLAGS_logfile, &config_.message()) {
+ reader_(aos::logger::SortParts(aos::logger::FindLogs(FLAGS_logfile)),
+ &config_.message()) {
aos::network::OverrideTeamNumber(971);
// TODO(james): Actually enforce not sending on the same buses as the
@@ -55,10 +57,6 @@
frc971::control_loops::drivetrain::DrivetrainConfig<double> config =
GetDrivetrainConfig();
- // Make the modification required to the imu transform to work with the 2016
- // logs...
- config.imu_transform << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
- config.gyro_type = frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
localizer_ =
std::make_unique<frc971::control_loops::drivetrain::DeadReckonEkf>(
@@ -70,25 +68,6 @@
test_event_loop_ =
reader_.event_loop_factory()->MakeEventLoop("drivetrain_test", roborio_);
- // IMU readings used to be published out one at a time, but we now expect
- // batches. Batch them up to upgrade the data.
- imu_sender_ =
- test_event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain");
- test_event_loop_->MakeWatcher(
- "/drivetrain", [this](const frc971::IMUValues &values) {
- aos::Sender<frc971::IMUValuesBatch>::Builder builder =
- imu_sender_.MakeBuilder();
- flatbuffers::Offset<frc971::IMUValues> values_offsets =
- aos::CopyFlatBuffer(&values, builder.fbb());
- flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
- values_offset = builder.fbb()->CreateVector(&values_offsets, 1);
- frc971::IMUValuesBatch::Builder imu_values_batch_builder =
- builder.MakeBuilder<frc971::IMUValuesBatch>();
- imu_values_batch_builder.add_readings(values_offset);
- builder.Send(imu_values_batch_builder.Finish());
- });
-
status_fetcher_ = test_event_loop_->MakeFetcher<
frc971::control_loops::drivetrain::Status>("/drivetrain");
}
@@ -102,7 +81,6 @@
std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop>
drivetrain_;
std::unique_ptr<aos::EventLoop> test_event_loop_;
- aos::Sender<frc971::IMUValuesBatch> imu_sender_;
aos::Fetcher<frc971::control_loops::drivetrain::Status> status_fetcher_;
};
@@ -116,7 +94,10 @@
ASSERT_TRUE(status_fetcher_->has_x());
ASSERT_TRUE(status_fetcher_->has_y());
ASSERT_TRUE(status_fetcher_->has_theta());
- EXPECT_LT(std::abs(status_fetcher_->x()), 0.25);
+ EXPECT_NEAR(status_fetcher_->estimated_left_position(),
+ status_fetcher_->estimated_right_position(), 0.1);
+ EXPECT_LT(std::abs(status_fetcher_->x()),
+ std::abs(status_fetcher_->estimated_left_position()) / 2.0);
// Because the encoders should not be affecting the y or yaw axes, expect a
// reasonably precise result (although, since this is a real worl dtest, the
// robot probably did actually move be some non-zero amount).
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 3059bf1..32c8834 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -24,7 +24,7 @@
}
// Indices of the pis to use.
-const std::array<std::string, 3> kPisToUse{"pi1", "pi2", "pi3"};
+const std::array<std::string, 5> kPisToUse{"pi1", "pi2", "pi3", "pi4", "pi5"};
// Calculates the pose implied by the camera target, just based on
// distance/heading components.
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 5dbad02..c3b6464 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -65,7 +65,7 @@
right_encoder, 0, 0, 0, 0, 0, 0)
.finished(),
ekf_.P());
- };
+ }
private:
// Storage for a single turret position data point.
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index d195b5d..a3e26b5 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -131,13 +131,14 @@
drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
last_frame_(monotonic_now()) {
event_loop_factory()->SetTimeConverter(&time_converter_);
- CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 5);
+ CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 6);
CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), pi1_), 1);
time_converter_.AddMonotonic({monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
+ monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch()});
set_team_id(frc971::control_loops::testing::kTeamNumber);
set_battery_voltage(12.0);
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 59f3fc0..70e51b0 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -17,6 +17,7 @@
"y2020_pi2.json",
"y2020_pi3.json",
"y2020_pi4.json",
+ "y2020_pi5.json",
"y2020_laptop.json"
]
}
diff --git a/y2020/y2020_laptop.json b/y2020/y2020_laptop.json
index 9ebac58..936f2cd 100644
--- a/y2020/y2020_laptop.json
+++ b/y2020/y2020_laptop.json
@@ -101,6 +101,20 @@
]
},
{
+ "name": "/pi5/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi5",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["laptop"],
+ "destination_nodes": [
+ {
+ "name": "laptop",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
"name": "/laptop/aos",
"type": "aos.timing.Report",
"source_node": "laptop",
@@ -119,7 +133,7 @@
"name": "/laptop/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "laptop",
- "frequency": 2,
+ "frequency": 10,
"num_senders": 2
},
{
@@ -166,6 +180,13 @@
"timestamp_logger_nodes": ["laptop"]
},
{
+ "name": "pi5",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["laptop"]
+ },
+ {
"name": "roborio",
"priority": 1,
"time_to_live": 5000000,
@@ -175,7 +196,7 @@
]
},
{
- "name": "/laptop/aos/remote_timestamps/roborio",
+ "name": "/laptop/aos/remote_timestamps/roborio/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -184,7 +205,7 @@
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi1",
+ "name": "/laptop/aos/remote_timestamps/pi1/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -193,7 +214,7 @@
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi2",
+ "name": "/laptop/aos/remote_timestamps/pi2/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -202,7 +223,7 @@
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi3",
+ "name": "/laptop/aos/remote_timestamps/pi3/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -211,7 +232,16 @@
"max_size": 200
},
{
- "name": "/laptop/aos/remote_timestamps/pi4",
+ "name": "/laptop/aos/remote_timestamps/pi4/laptop/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "laptop",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/laptop/aos/remote_timestamps/pi5/laptop/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "laptop",
"logger": "NOT_LOGGED",
@@ -330,6 +360,34 @@
"time_to_live": 5000000
}
]
+ },
+ {
+ "name": "/pi5/camera",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "pi5",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["laptop"],
+ "destination_nodes": [
+ {
+ "name": "laptop",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi5/camera",
+ "type": "frc971.vision.sift.ImageMatchResult",
+ "source_node": "pi5",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["laptop"],
+ "destination_nodes": [
+ {
+ "name": "laptop",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
}
],
"maps": [
@@ -367,6 +425,9 @@
},
{
"name": "pi4"
+ },
+ {
+ "name": "pi5"
}
]
}
diff --git a/y2020/y2020_pi_template.json b/y2020/y2020_pi_template.json
index d5c97d3..4ed10b3 100644
--- a/y2020/y2020_pi_template.json
+++ b/y2020/y2020_pi_template.json
@@ -34,7 +34,7 @@
"name": "/pi{{ NUM }}/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "pi{{ NUM }}",
- "frequency": 2,
+ "frequency": 10,
"num_senders": 2
},
{
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 2e29eee..2a204ae 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -46,7 +46,7 @@
"name": "/roborio/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "roborio",
- "frequency": 2,
+ "frequency": 10,
"num_senders": 2
},
{
@@ -57,28 +57,42 @@
"num_senders": 2
},
{
- "name": "/roborio/aos/remote_timestamps/pi1",
+ "name": "/roborio/aos/remote_timestamps/laptop/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
- "name": "/roborio/aos/remote_timestamps/pi2",
+ "name": "/roborio/aos/remote_timestamps/pi1/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
- "name": "/roborio/aos/remote_timestamps/pi3",
+ "name": "/roborio/aos/remote_timestamps/pi2/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
- "name": "/roborio/aos/remote_timestamps/pi4",
+ "name": "/roborio/aos/remote_timestamps/pi3/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 200,
+ "logger": "NOT_LOGGED",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/pi4/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 200,
+ "logger": "NOT_LOGGED",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/pi5/roborio/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 200,
"logger": "NOT_LOGGED",
@@ -119,6 +133,13 @@
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": ["roborio"],
"time_to_live": 5000000
+ },
+ {
+ "name": "pi5",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["roborio"],
+ "time_to_live": 5000000
}
]
},
@@ -192,47 +213,17 @@
},
{
"name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Output",
+ "source_node": "roborio",
+ "frequency": 200,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Status",
"source_node": "roborio",
"frequency": 200,
"max_size": 2000,
- "num_senders": 2,
- "destination_nodes": [
- {
- "name": "pi1",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": ["roborio"],
- "time_to_live": 5000000
- },
- {
- "name": "pi2",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": ["roborio"],
- "time_to_live": 5000000
- },
- {
- "name": "pi3",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": ["roborio"],
- "time_to_live": 5000000
- },
- {
- "name": "pi4",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": ["roborio"],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/drivetrain",
- "type": "frc971.control_loops.drivetrain.Output",
- "source_node": "roborio",
- "frequency": 200,
"num_senders": 2
},
{
@@ -352,6 +343,9 @@
},
{
"name": "pi4"
+ },
+ {
+ "name": "pi5"
}
]
}