Merge "Sundry extrinsics/target mapping fixes."
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index dc71cbd..9d2bed0 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -19,7 +19,10 @@
export PATH="${PATH}:/home/pi/bin"
# Turn the fans up.
- echo 255 > /sys/devices/platform/pwm-fan/hwmon/hwmon1/pwm1
+ echo 255 > /sys/devices/platform/pwm-fan/hwmon/hwmon?/pwm1
+
+ # Give read permissions for INA3221 electrical readings sysfs
+ chmod -R a+r /sys/devices/platform/c240000.i2c/i2c-1/1-0040/hwmon/hwmon?/
exec starterd --user=pi --purge_shm_base
else
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index a876ae3..163311a 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -196,13 +196,13 @@
<< min_friction_accel << " max fric accel " << max_friction_accel;
}
if (best_accel < min_voltage_accel || best_accel > max_voltage_accel) {
- LOG(WARNING) << "Viable friction limits and viable voltage limits do not "
- "overlap (x: "
- << x << ", v: " << v << ", backwards: " << backwards
- << ") best_accel = " << best_accel << ", min voltage "
- << min_voltage_accel << ", max voltage " << max_voltage_accel
- << " min friction " << min_friction_accel << " max friction "
- << max_friction_accel << ".";
+ VLOG(1) << "Viable friction limits and viable voltage limits do not "
+ "overlap (x: "
+ << x << ", v: " << v << ", backwards: " << backwards
+ << ") best_accel = " << best_accel << ", min voltage "
+ << min_voltage_accel << ", max voltage " << max_voltage_accel
+ << " min friction " << min_friction_accel << " max friction "
+ << max_friction_accel << ".";
// Don't actually do anything--this will just result in attempting to drive
// higher voltages thatn we have available. In practice, that'll probably
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.cc b/frc971/control_loops/drivetrain/trajectory_generator.cc
index e531167..bb7a53b 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.cc
+++ b/frc971/control_loops/drivetrain/trajectory_generator.cc
@@ -7,10 +7,16 @@
: event_loop_(event_loop),
dt_config_(config),
trajectory_sender_(
- event_loop_->MakeSender<fb::Trajectory>("/drivetrain")) {
+ event_loop_->MakeSender<fb::Trajectory>("/drivetrain")),
+ spline_goal_fetcher_(event_loop->MakeFetcher<SplineGoal>("/drivetrain")) {
event_loop_->MakeWatcher("/drivetrain", [this](const SplineGoal &goal) {
HandleSplineGoal(goal);
});
+ event_loop_->OnRun([this]() {
+ if (spline_goal_fetcher_.Fetch()) {
+ HandleSplineGoal(*spline_goal_fetcher_.get());
+ }
+ });
}
void TrajectoryGenerator::HandleSplineGoal(const SplineGoal &goal) {
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.h b/frc971/control_loops/drivetrain/trajectory_generator.h
index 5d638d3..f3498bd 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.h
+++ b/frc971/control_loops/drivetrain/trajectory_generator.h
@@ -18,6 +18,7 @@
aos::EventLoop *const event_loop_;
const DrivetrainConfig<double> dt_config_;
aos::Sender<fb::Trajectory> trajectory_sender_;
+ aos::Fetcher<SplineGoal> spline_goal_fetcher_;
};
} // namespace frc971::control_loops::drivetrain
diff --git a/frc971/orin/BUILD b/frc971/orin/BUILD
index 3ce9040..8c4e9ac 100644
--- a/frc971/orin/BUILD
+++ b/frc971/orin/BUILD
@@ -1,3 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
load("//frc971:halide.bzl", "halide_library")
exports_files(["orin_irq_config.json"])
@@ -150,3 +152,27 @@
"//third_party:opencv",
],
)
+
+static_flatbuffer(
+ name = "hardware_stats_fbs",
+ srcs = ["hardware_stats.fbs"],
+ visibility = ["//visibility:public"],
+)
+
+cc_static_flatbuffer(
+ name = "hardware_stats_schema",
+ function = "aos::util::HardwareStatsSchema",
+ target = ":hardware_stats_fbs_reflection_out",
+)
+
+cc_binary(
+ name = "hardware_monitor",
+ srcs = ["hardware_monitor.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":hardware_stats_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "@com_google_absl//absl/strings",
+ ],
+)
diff --git a/frc971/orin/hardware_monitor.cc b/frc971/orin/hardware_monitor.cc
new file mode 100644
index 0000000..f09d0e6
--- /dev/null
+++ b/frc971/orin/hardware_monitor.cc
@@ -0,0 +1,174 @@
+#include <dirent.h>
+#include <sys/statvfs.h>
+
+#include "absl/strings/numbers.h"
+#include "absl/strings/str_format.h"
+#include "gflags/gflags.h"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/orin/hardware_stats_generated.h"
+
+DEFINE_string(config, "aos_config.json", "File path of aos configuration");
+
+namespace frc971::orin {
+namespace {
+std::optional<std::string> ReadFileFirstLine(std::string_view file_name) {
+ std::ifstream file(std::string(file_name), std::ios_base::in);
+ if (!file.good()) {
+ VLOG(1) << "Can't read " << file_name;
+ return std::nullopt;
+ }
+ std::string line;
+ std::getline(file, line);
+ return line;
+}
+
+std::string GetHwmonNumber(const char *dir_name) {
+ DIR *dirp = opendir(dir_name);
+ if (!dirp) {
+ VLOG(1) << "Can't open " << dir_name;
+ return "";
+ }
+ struct dirent *directory_entry;
+ while ((directory_entry = readdir(dirp)) != NULL) {
+ std::string entry_name(directory_entry->d_name);
+ if (entry_name.starts_with("hwmon")) {
+ closedir(dirp);
+ return entry_name;
+ }
+ }
+ closedir(dirp);
+ return "";
+}
+} // namespace
+
+// Periodically sends out the HardwareStats message with hardware statistics
+// info.
+class HardwareMonitor {
+ public:
+ HardwareMonitor(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ sender_(event_loop_->MakeSender<HardwareStats>("/hardware_monitor")),
+ fan_hwmon_(
+ GetHwmonNumber("/sys/devices/platform/39c0000.tachometer/hwmon/")),
+ electrical_hwmon_(GetHwmonNumber(
+ "/sys/devices/platform/c240000.i2c/i2c-1/1-0040/hwmon/")) {
+ periodic_timer_ =
+ event_loop_->AddTimer([this]() { PublishHardwareStats(); });
+ event_loop_->OnRun([this]() {
+ periodic_timer_->Schedule(event_loop_->monotonic_now(),
+ std::chrono::seconds(5));
+ });
+ }
+
+ private:
+ void PublishHardwareStats() {
+ aos::Sender<HardwareStats>::Builder builder = sender_.MakeBuilder();
+ // Iterate through all thermal zones
+ std::vector<flatbuffers::Offset<ThermalZone>> thermal_zones;
+ for (int zone_id = 0; zone_id < 9; zone_id++) {
+ ThermalZone::Builder thermal_zone_builder =
+ builder.MakeBuilder<ThermalZone>();
+ thermal_zone_builder.add_id(zone_id);
+
+ std::optional<std::string> zone_name = ReadFileFirstLine(absl::StrFormat(
+ "/sys/devices/virtual/thermal/thermal_zone%d/type", zone_id));
+ if (zone_name) {
+ thermal_zone_builder.add_name(builder.fbb()->CreateString(*zone_name));
+ }
+
+ std::optional<std::string> temperature_str =
+ ReadFileFirstLine(absl::StrFormat(
+ "/sys/devices/virtual/thermal/thermal_zone%d/temp", zone_id));
+ uint64_t temperature = 0;
+ if (temperature_str && absl::SimpleAtoi(*temperature_str, &temperature)) {
+ thermal_zone_builder.add_temperature(temperature);
+ }
+
+ thermal_zones.emplace_back(thermal_zone_builder.Finish());
+ }
+
+ // Get fan speed
+ std::optional<std::string> fan_speed_str = ReadFileFirstLine(
+ absl::StrFormat("/sys/class/hwmon/%s/rpm", fan_hwmon_));
+
+ // Iterate through INA3221 electrical reading channels
+ std::vector<flatbuffers::Offset<ElectricalReading>> electrical_readings;
+ for (int channel = 1; channel <= 3; channel++) {
+ ElectricalReading::Builder electrical_reading_builder =
+ builder.MakeBuilder<ElectricalReading>();
+ electrical_reading_builder.add_channel(channel);
+
+ std::optional<std::string> label = ReadFileFirstLine(absl::StrFormat(
+ "/sys/class/hwmon/%s/in%d_label", electrical_hwmon_, channel));
+ if (label) {
+ electrical_reading_builder.add_label(
+ builder.fbb()->CreateString(*label));
+ }
+
+ std::optional<std::string> voltage_str =
+ ReadFileFirstLine(absl::StrFormat("/sys/class/hwmon/%s/in%d_input",
+ electrical_hwmon_, channel));
+ uint64_t voltage = 0;
+ if (voltage_str && absl::SimpleAtoi(*voltage_str, &voltage)) {
+ electrical_reading_builder.add_voltage(voltage);
+ }
+
+ std::optional<std::string> current_str =
+ ReadFileFirstLine(absl::StrFormat("/sys/class/hwmon/%s/curr%d_input",
+ electrical_hwmon_, channel));
+ uint64_t current = 0;
+ if (current_str && absl::SimpleAtoi(*current_str, ¤t)) {
+ electrical_reading_builder.add_current(current);
+ }
+
+ uint64_t power = voltage * current / 1000;
+ if (power != 0) {
+ electrical_reading_builder.add_power(power);
+ }
+
+ electrical_readings.emplace_back(electrical_reading_builder.Finish());
+ }
+
+ HardwareStats::Builder hardware_stats_builder =
+ builder.MakeBuilder<HardwareStats>();
+ hardware_stats_builder.add_thermal_zones(
+ builder.fbb()->CreateVector(thermal_zones));
+ uint64_t fan_speed = 0;
+ if (fan_speed_str && absl::SimpleAtoi(*fan_speed_str, &fan_speed)) {
+ hardware_stats_builder.add_fan_speed(fan_speed);
+ }
+ hardware_stats_builder.add_electrical_readings(
+ builder.fbb()->CreateVector(electrical_readings));
+
+ builder.CheckOk(builder.Send(hardware_stats_builder.Finish()));
+ }
+
+ aos::EventLoop *event_loop_;
+
+ aos::Sender<HardwareStats> sender_;
+
+ aos::TimerHandler *periodic_timer_;
+
+ std::string fan_hwmon_;
+
+ std::string electrical_hwmon_;
+};
+
+} // namespace frc971::orin
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop shm_event_loop(&config.message());
+
+ frc971::orin::HardwareMonitor hardware_monitor(&shm_event_loop);
+
+ shm_event_loop.Run();
+
+ return 0;
+}
diff --git a/frc971/orin/hardware_stats.fbs b/frc971/orin/hardware_stats.fbs
new file mode 100644
index 0000000..9f0fa74
--- /dev/null
+++ b/frc971/orin/hardware_stats.fbs
@@ -0,0 +1,37 @@
+namespace frc971.orin;
+
+// Per-zone stats for a single thermal zone.
+table ThermalZone {
+ // Number of the thermal zone in question.
+ id: uint64 (id: 0);
+ // Name of the thermal zone.
+ name: string (id: 1);
+ // Temperature of the thermal zone in millidegrees C.
+ temperature: uint64 (id: 2);
+}
+
+// Electrical readings from the INA3221 monitoring chip.
+table ElectricalReading {
+ // Number of the INA3221 measurement channel.
+ channel: uint64 (id: 0);
+ // Label of the INA3221 measurement point.
+ label: string (id: 1);
+ // Voltage reading in mV at the measurement point.
+ voltage: uint64 (id: 2);
+ // Current reading in mA through the measurement point.
+ current: uint64 (id: 3);
+ // Power usage in mW at the measurement point.
+ power: uint64 (id: 4);
+}
+
+// Table to track the current state of a compute platform's hardware.
+table HardwareStats {
+ // Per-zone thermal stats
+ thermal_zones: [ThermalZone] (id: 0);
+ // RPM of the fan
+ fan_speed: uint64 (id: 1);
+ // Electrical readings
+ electrical_readings: [ElectricalReading] (id: 2);
+}
+
+root_type HardwareStats;
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index d1aa33c..de9e55a 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -233,6 +233,11 @@
T_frozen_actual_(Eigen::Vector3d::Zero()),
R_frozen_actual_(Eigen::Quaterniond::Identity()),
vis_robot_(cv::Size(kImageWidth_, kImageHeight_)) {
+ // Compute focal length so that image shows field with viewpoint at 10m above
+ // it (default for viewer)
+ const double focal_length = kImageWidth_ * 10.0 / kFieldWidth_;
+ vis_robot_.SetDefaultViewpoint(kImageWidth_, focal_length);
+
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
@@ -345,7 +350,7 @@
if (id_pair.second > max_constraint_id) {
max_constraint_id = id_pair.second;
}
- // Normalize constraint cost by occurances
+ // Normalize constraint cost by occurrences
size_t constraint_count = constraint_counts_[id_pair];
// Scale all costs so the total cost comes out to more reasonable numbers
constexpr double kGlobalWeight = 1000.0;
@@ -395,10 +400,6 @@
TargetMapper::BuildMapFittingOptimizationProblem(ceres::Problem *problem) {
// Set up robot visualization.
vis_robot_.ClearImage();
- // Compute focal length so that image shows field with viewpoint at 10m above
- // it (default for viewer)
- const double kFocalLength = kImageWidth_ * 10.0 / kFieldWidth_;
- vis_robot_.SetDefaultViewpoint(kImageWidth_, kFocalLength);
const size_t num_targets = FLAGS_max_target_id - FLAGS_min_target_id;
// Translation and rotation error for each target
@@ -421,6 +422,34 @@
return cost_function;
}
+void TargetMapper::DisplayConstraintGraph() {
+ vis_robot_.ClearImage();
+ for (auto constraint : constraint_counts_) {
+ Eigen::Vector3d start_line =
+ PoseUtils::Pose3dToAffine3d(
+ ideal_target_poses_.at(constraint.first.first))
+ .translation();
+ Eigen::Vector3d end_line =
+ PoseUtils::Pose3dToAffine3d(
+ ideal_target_poses_.at(constraint.first.second))
+ .translation();
+ // Weight the green intensity by # of constraints
+ // TODO: This could be improved
+ int color_scale =
+ 50 + std::min(155, static_cast<int>(constraint.second * 155.0 / 200.0));
+ vis_robot_.DrawLine(start_line, end_line, cv::Scalar(0, color_scale, 0));
+ }
+
+ for (const auto &[id, solved_pose] : target_poses_) {
+ Eigen::Affine3d H_world_ideal =
+ PoseUtils::Pose3dToAffine3d(ideal_target_poses_.at(id));
+ vis_robot_.DrawFrameAxes(H_world_ideal, std::to_string(id),
+ cv::Scalar(255, 255, 255));
+ }
+ cv::imshow("Constraint graph", vis_robot_.image_);
+ cv::waitKey(0);
+}
+
// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) {
CHECK_NOTNULL(problem);
@@ -448,6 +477,11 @@
RemoveOutlierConstraints();
+ if (FLAGS_visualize_solver) {
+ LOG(INFO) << "Displaying constraint graph after removing outliers";
+ DisplayConstraintGraph();
+ }
+
// Solve again once we've thrown out bad constraints
ceres::Problem target_pose_problem_2;
BuildTargetPoseOptimizationProblem(target_constraints_, &target_poses_,
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index dc4a85a..6cc2396 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -100,6 +100,10 @@
std::unique_ptr<ceres::CostFunction> BuildMapFittingOptimizationProblem(
ceres::Problem *problem);
+ // Create and display a visualization of the graph connectivity of the
+ // constraints
+ void DisplayConstraintGraph();
+
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem *problem);
diff --git a/y2024/BUILD b/y2024/BUILD
index 4e13aed..7bbf64f 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -55,6 +55,7 @@
"//aos/starter:irq_affinity",
"//aos/util:foxglove_websocket",
"//frc971/image_streamer:image_streamer",
+ "//frc971/orin:hardware_monitor",
"//frc971/vision:intrinsics_calibration",
"//aos/util:filesystem_monitor",
"//y2024/vision:viewer",
@@ -123,6 +124,7 @@
"//y2024/constants:constants_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/can_logger:can_logging_fbs",
+ "//frc971/orin:hardware_stats_fbs",
"//y2024/localizer:status_fbs",
"//y2024/localizer:visualization_fbs",
"//aos/network:timestamp_fbs",
@@ -182,6 +184,7 @@
"//aos/network:remote_message_fbs",
"//y2024/constants:constants_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/orin:hardware_stats_fbs",
"//frc971/vision:calibration_fbs",
"//y2024/localizer:visualization_fbs",
"//frc971/vision:target_map_fbs",
diff --git a/y2024/constants/simulated_constants_sender.cc b/y2024/constants/simulated_constants_sender.cc
index 28189f8..9b3ee5b 100644
--- a/y2024/constants/simulated_constants_sender.cc
+++ b/y2024/constants/simulated_constants_sender.cc
@@ -6,8 +6,13 @@
namespace y2024 {
bool SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
- std::string constants_path) {
+ std::string constants_path,
+ const std::set<std::string_view> &node_names) {
for (const aos::Node *node : factory->nodes()) {
+ if (!node_names.empty() &&
+ !node_names.contains(node->name()->string_view())) {
+ continue;
+ }
std::unique_ptr<aos::EventLoop> event_loop =
factory->MakeEventLoop("constants_sender", node);
frc971::constants::ConstantSender<Constants, ConstantsList> sender(
diff --git a/y2024/constants/simulated_constants_sender.h b/y2024/constants/simulated_constants_sender.h
index 5ecc4da..40a5bdd 100644
--- a/y2024/constants/simulated_constants_sender.h
+++ b/y2024/constants/simulated_constants_sender.h
@@ -1,16 +1,21 @@
#ifndef Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
#define Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+#include <set>
+
#include "aos/events/simulated_event_loop.h"
#include "aos/testing/path.h"
namespace y2024 {
// Returns true, to allow this to be easily called in the initializer list of a
// constructor.
+// If node_names is specified, we limit ourselves to sending constants on the
+// specified nodes.
bool SendSimulationConstants(
aos::SimulatedEventLoopFactory *factory, int team,
std::string constants_path =
- aos::testing::ArtifactPath("y2024/constants/test_constants.json"));
+ aos::testing::ArtifactPath("y2024/constants/test_constants.json"),
+ const std::set<std::string_view> &node_names = {});
} // namespace y2024
#endif // Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2024/control_loops/drivetrain/BUILD b/y2024/control_loops/drivetrain/BUILD
index e9f93ca..2a97a7b 100644
--- a/y2024/control_loops/drivetrain/BUILD
+++ b/y2024/control_loops/drivetrain/BUILD
@@ -140,3 +140,29 @@
parameters = {},
visibility = ["//visibility:public"],
)
+
+cc_binary(
+ name = "drivetrain_replay",
+ srcs = ["drivetrain_replay.cc"],
+ data = [
+ "//y2024:aos_config",
+ "//y2024/constants:constants.json",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/events/logging:log_writer",
+ "//aos/util:simulation_logger",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain:trajectory_generator",
+ "//frc971/control_loops/drivetrain/localization:puppet_localizer",
+ "//frc971/imu_fdcan:dual_imu_blender_lib",
+ "//y2024/constants:simulated_constants_sender",
+ "//y2024/control_loops/drivetrain:drivetrain_base",
+ ],
+)
diff --git a/y2024/control_loops/drivetrain/drivetrain_replay.cc b/y2024/control_loops/drivetrain/drivetrain_replay.cc
new file mode 100644
index 0000000..d43b2af
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_replay.cc
@@ -0,0 +1,102 @@
+#include "gflags/gflags.h"
+
+#include "aos/configuration.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"
+#include "aos/network/team_number.h"
+#include "aos/util/simulation_logger.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/constants/simulated_constants_sender.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(config, "y2024/aos_config.json",
+ "Name of the config file to replay using.");
+DEFINE_bool(override_config, false,
+ "If set, override the logged config with --config.");
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/replayed",
+ "Name of the folder to write replayed logs to.");
+DEFINE_string(constants_path, "y2024/constants/constants.json",
+ "Path to the constant file");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ // sort logfiles
+ const std::vector<aos::logger::LogFile> logfiles =
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv));
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ logfiles, FLAGS_override_config ? &config.message() : nullptr);
+
+ reader.RemapLoggedChannel("/imu/constants", "y2024.Constants");
+ reader.RemapLoggedChannel("/roborio/constants", "y2024.Constants");
+ reader.RemapLoggedChannel("/orin1/constants", "y2024.Constants");
+ reader.RemapLoggedChannel("/drivetrain",
+ "frc971.control_loops.drivetrain.Status");
+ reader.RemapLoggedChannel("/drivetrain",
+ "frc971.control_loops.drivetrain.Output");
+ reader.RemapLoggedChannel(
+ "/drivetrain", "frc971.control_loops.drivetrain.RioLocalizerInputs");
+
+ auto factory =
+ std::make_unique<aos::SimulatedEventLoopFactory>(reader.configuration());
+
+ reader.RegisterWithoutStarting(factory.get());
+
+ y2024::SendSimulationConstants(reader.event_loop_factory(), FLAGS_team,
+ FLAGS_constants_path, {"roborio"});
+
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), "roborio");
+ }
+ std::vector<std::unique_ptr<aos::util::LoggerState>> loggers;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_event_loop;
+ std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+ localizer;
+ std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop> drivetrain;
+ reader.OnStart(node, [&factory, node, &loggers, &drivetrain_event_loop,
+ &localizer, &drivetrain]() {
+ aos::NodeEventLoopFactory *node_factory =
+ factory->GetNodeEventLoopFactory(node);
+ drivetrain_event_loop = node_factory->MakeEventLoop("drivetrain");
+ const auto drivetrain_config =
+ ::y2024::control_loops::drivetrain::GetDrivetrainConfig(
+ drivetrain_event_loop.get());
+ localizer =
+ std::make_unique<::frc971::control_loops::drivetrain::PuppetLocalizer>(
+ drivetrain_event_loop.get(), drivetrain_config);
+ drivetrain =
+ std::make_unique<frc971::control_loops::drivetrain::DrivetrainLoop>(
+ drivetrain_config, drivetrain_event_loop.get(), localizer.get());
+ loggers.push_back(std::make_unique<aos::util::LoggerState>(
+ factory.get(), node, FLAGS_output_folder));
+ // The Trajectory information is NOT_LOGGED, so we need to rerun it against
+ // the SplineGoals that we have in the log.
+ node_factory
+ ->AlwaysStart<frc971::control_loops::drivetrain::TrajectoryGenerator>(
+ "trajectory_generator", drivetrain_config);
+ });
+
+ reader.event_loop_factory()->Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2024/localizer/localizer_plotter.ts b/y2024/localizer/localizer_plotter.ts
index b5e764b..4e64a68 100644
--- a/y2024/localizer/localizer_plotter.ts
+++ b/y2024/localizer/localizer_plotter.ts
@@ -21,6 +21,8 @@
'/drivetrain', 'frc971.control_loops.drivetrain.Output');
const localizer = aosPlotter.addMessageSource(
'/localizer', 'y2024.localizer.Status');
+ const rio_inputs = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.RioLocalizerInputs');
const imu = aosPlotter.addRawMessageSource(
'/localizer', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
@@ -43,6 +45,12 @@
positionPlot.addMessageLine(position, ['right_encoder'])
.setColor(CYAN)
.setDrawLine(false);
+ positionPlot.addMessageLine(rio_inputs, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(rio_inputs, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
// Drivetrain Velocities
@@ -98,6 +106,12 @@
voltagePlot.addMessageLine(output, ['right_voltage'])
.setColor(GREEN)
.setPointSize(0);
+ voltagePlot.addMessageLine(rio_inputs, ['left_voltage'])
+ .setColor(RED)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(rio_inputs, ['right_voltage'])
+ .setColor(GREEN)
+ .setDrawLine(false);
// Heading
const yawPlot = aosPlotter.addPlot(element);
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 189c96c..f7dd7c2 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -49,7 +49,7 @@
"type": "aos.logging.LogMessageFbs",
"source_node": "imu",
"frequency": 200,
- "num_senders": 20
+ "num_senders": 30
},
{
"name": "/imu/aos",
@@ -310,6 +310,12 @@
"num_senders": 2
},
{
+ "name": "/imu/hardware_monitor",
+ "type": "frc971.orin.HardwareStats",
+ "source_node": "imu",
+ "frequency": 2
+ },
+ {
"name": "/imu/constants",
"type": "y2024.Constants",
"source_node": "imu",
@@ -347,6 +353,13 @@
]
},
{
+ "name": "hardware_monitor",
+ "executable_name": "hardware_monitor",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
"name": "joystick_republish",
"executable_name": "joystick_republish",
"user": "pi",
@@ -563,6 +576,15 @@
"rename": {
"name": "/imu/camera"
}
+ },
+ {
+ "match": {
+ "name": "/hardware_monitor*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/hardware_monitor"
+ }
}
],
"nodes": [
diff --git a/y2024/y2024_orin1.json b/y2024/y2024_orin1.json
index 97c6b64..ecb9342 100644
--- a/y2024/y2024_orin1.json
+++ b/y2024/y2024_orin1.json
@@ -19,7 +19,7 @@
"type": "aos.logging.LogMessageFbs",
"source_node": "orin1",
"frequency": 200,
- "num_senders": 20
+ "num_senders": 30
},
{
"name": "/orin1/aos",
@@ -244,6 +244,12 @@
"max_size": 208
},
{
+ "name": "/orin1/hardware_monitor",
+ "type": "frc971.orin.HardwareStats",
+ "source_node": "orin1",
+ "frequency": 2
+ },
+ {
"name": "/orin1/constants",
"type": "y2024.Constants",
"source_node": "orin1",
@@ -283,6 +289,13 @@
]
},
{
+ "name": "hardware_monitor",
+ "executable_name": "hardware_monitor",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
"name": "message_bridge_server",
"executable_name": "message_bridge_server",
"args": [
@@ -431,6 +444,15 @@
"rename": {
"name": "/orin1/camera"
}
+ },
+ {
+ "match": {
+ "name": "/hardware_monitor*",
+ "source_node": "orin1"
+ },
+ "rename": {
+ "name": "/orin1/hardware_monitor"
+ }
}
],
"nodes": [