Merge "scouting: Enable strict template checking"
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/analysis/foxglove.md b/frc971/analysis/foxglove.md
index 8276584..d241ec0 100644
--- a/frc971/analysis/foxglove.md
+++ b/frc971/analysis/foxglove.md
@@ -8,7 +8,7 @@
is convenient; it won't work when you do not have Internet access, and
has some limitations when it comes to accessing unsecured websockets.
2. Download the foxglove desktop application.
-3. Run our local copy by running `bazel run //frc971/analysis:local_foxglove`
+3. Run our local copy by running `bazel run //aos/analysis:local_foxglove`
This will work offline, and serves foxglove at http://localhost:8000 by
default.
@@ -20,6 +20,9 @@
This will create an MCAP file at the specified path, which you can then open
in any of the various foxglove options.
+Troubleshooting:
+* If you get the error `Check failed: output_`: Check whether `/tmp/log.mcap` already exists under another owner. If so, use a different filename, e.g. `/tmp/<your_name>_log.mcap`
+
# Live Visualization
On the pis, we run a `foxglove_websocket` application by default. This exposes
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 95fab0a2..81f1d8d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -28,7 +28,7 @@
namespace {
// Maximum variation to allow in the gyro when zeroing.
-constexpr double kMaxYawGyroZeroingRange = 0.08;
+constexpr double kMaxYawGyroZeroingRange = 0.15;
} // namespace
DrivetrainFilters::DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
@@ -355,7 +355,7 @@
event_loop->TryMakeSender<
frc971::control_loops::drivetrain::RioLocalizerInputsStatic>(
"/drivetrain")) {
- event_loop->SetRuntimeRealtimePriority(30);
+ event_loop->SetRuntimeRealtimePriority(37);
for (size_t ii = 0; ii < trajectory_fetchers_.size(); ++ii) {
trajectory_fetchers_[ii].fetcher =
event_loop->MakeFetcher<fb::Trajectory>("/drivetrain");
@@ -570,6 +570,7 @@
builder.add_down_estimator(down_estimator_state_offset);
builder.add_localizer(localizer_offset);
builder.add_zeroing(zeroer_offset);
+ builder.add_filters_ready(filters_.Ready());
builder.add_send_failures(status_failure_counter_.failures());
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 8e18915..9d02bb2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -52,6 +52,14 @@
PLANNED,
}
+table SplineState {
+ x:double (id: 0);
+ y:double (id: 1);
+ theta:double (id: 2);
+ left_velocity:double (id: 3);
+ right_velocity:double (id: 4);
+}
+
// For logging information about the state of the trajectory planning.
table TrajectoryLogging {
// state of planning the trajectory.
@@ -82,6 +90,12 @@
// Splines that we have full plans for.
available_splines:[int] (id: 12);
+
+ state_error:SplineState (id: 14);
+ left_voltage_components:SplineState (id: 15);
+ right_voltage_components:SplineState (id: 16);
+ left_ff_voltage:double (id: 17);
+ right_ff_voltage:double (id: 18);
}
enum RobotSide : ubyte {
@@ -279,6 +293,8 @@
send_failures:uint64 (id: 28);
encoder_faults:Faults (id: 29);
+
+ filters_ready:bool (id: 30);
}
root_type Status;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index f90e7e0..dbabfb1 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -175,6 +175,7 @@
Eigen::Matrix<double, 5, 1> state_error = relative_goal - relative_state;
state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+ last_U_components_ = K * Eigen::DiagonalMatrix<double, 5>(state_error);
if (backwards) {
Eigen::Matrix<double, 2, 1> swapU(U_fb(1), U_fb(0));
@@ -186,6 +187,8 @@
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
ScaleCapU(&next_U_);
+ last_state_error_ = state_error;
+ last_U_ff_ = U_ff;
}
}
@@ -215,6 +218,20 @@
}
}
+namespace {
+template <typename Matrix>
+flatbuffers::Offset<SplineState> MakeSplineState(
+ const Matrix &state, flatbuffers::FlatBufferBuilder *fbb) {
+ SplineState::Builder builder(*fbb);
+ builder.add_x(state(0));
+ builder.add_y(state(1));
+ builder.add_theta(::aos::math::NormalizeAngle(state(2)));
+ builder.add_left_velocity(state(3));
+ builder.add_right_velocity(state(4));
+ return builder.Finish();
+}
+} // namespace
+
flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
flatbuffers::FlatBufferBuilder *builder) const {
int *spline_handles;
@@ -225,6 +242,13 @@
spline_handles[ii] = trajectories_[ii].spline_handle();
}
+ const flatbuffers::Offset<SplineState> state_error_offset =
+ MakeSplineState(last_state_error_, builder);
+ const flatbuffers::Offset<SplineState> left_voltage_components_offset =
+ MakeSplineState(last_U_components_.row(0), builder);
+ const flatbuffers::Offset<SplineState> right_voltage_components_offset =
+ MakeSplineState(last_U_components_.row(1), builder);
+
drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
if (executing_spline_) {
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
@@ -241,6 +265,13 @@
trajectory_logging_builder.add_left_velocity(goal_state(3));
trajectory_logging_builder.add_right_velocity(goal_state(4));
}
+ trajectory_logging_builder.add_state_error(state_error_offset);
+ trajectory_logging_builder.add_left_voltage_components(
+ left_voltage_components_offset);
+ trajectory_logging_builder.add_right_voltage_components(
+ right_voltage_components_offset);
+ trajectory_logging_builder.add_left_ff_voltage(last_U_ff_(0));
+ trajectory_logging_builder.add_right_ff_voltage(last_U_ff_(1));
}
trajectory_logging_builder.add_is_executing(!IsAtEnd() && executing_spline_);
trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index f02a5b6..24fb66c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -105,6 +105,9 @@
// Information used for status message.
::Eigen::Matrix<double, 2, 1> uncapped_U_;
+ ::Eigen::Matrix<double, 5, 1> last_state_error_;
+ ::Eigen::Matrix<double, 2, 5> last_U_components_;
+ ::Eigen::Matrix<double, 2, 1> last_U_ff_;
bool enable_ = false;
bool output_was_capped_ = false;
};
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/contents/home/pi/.ssh/authorized_keys b/frc971/orin/contents/home/pi/.ssh/authorized_keys
index 1e7929a..0ba3066 100644
--- a/frc971/orin/contents/home/pi/.ssh/authorized_keys
+++ b/frc971/orin/contents/home/pi/.ssh/authorized_keys
@@ -21,3 +21,5 @@
ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDG+8q7VDZv1bI2s/LDLZvjX4HInQobPF9QfBi8sNJdu98drWzSIlTIRMfOFuN8Vbq2E/wr6eKXOUwq/Brxu4sZ6+po6+sT6IqyE5cd+FffNJXZUc+QxXC6maD20i7k8gNJuo+BRa6VbSR25WarRoGT+pYl03jbhR304Wn8wIYIS5cAfaP2wzwNPOvCZzzwJrba2jeoeVWRUTuqLRAw4NNq5eZskdpv7w/3qw901z7RKzuTfYiAnvsRm8o6E0uRncyujzvXoXD2jyWdKC/boIv/l1Is/XwsSJahq1NIK3y95jZHoUXpDQsq49U0wOByBpKEyQYcI54nhfwYagVsYRY9 jim.o@bluerivert.com
ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAID7tW6k8e+eF+TV4rTQi8B40C6dWXTXvVWgyCV9POypm tushar.s.pankaj@gmail.com
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQD2QRuJnT79E5cODCLttNTpj4EEUdRQg2wssr6xGdY0A0hHLYLgjpbxzeSGuYAu40syhtfEqtCsrLNI37JsonMFT5Pe0wjB7N3FBzFIpJSU3rMCyvn1fqVucDp9iYz3U5ahNir06LkDMgtxkIxvYjZDHNruwpI5Fe0nhsnw9TkfDHyB2Rl0948Y2AElb7yiJgUgQ5oPa+yHJIthAC1aPgM4xXQz9Q7Nnc0JpfxyCjvKwa2/Li0GQSzJ6Nf0PQiE7VaZHLCz7SVmHkC1qcQFRuqpjVl+C/ZFTXEPLXf2ktV9RhNi/YceCH35WtxQbE/GxJuEoKT9O6zpa8HTCAqP70zaIj9HMFr4Bvd3QBu+H0uLAWUA+i43auMwqUsnmZmLZpFmJqpk15USJ8v4IJ36N6+4TTRtHNSvtPflrXzxtOmkxkyKF94hJzAIindQ/pYNpkleUACeP7SPTOpRuA2cP/G9TZ5VZfsOCqHwWOlUXMbCAzqZheia0ypGali1wjzZTaE= sindytan@Sindys-MacBook-Pro.local
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/orin/set_orin_clock.sh b/frc971/orin/set_orin_clock.sh
index 32edb86..bc9dd8f 100755
--- a/frc971/orin/set_orin_clock.sh
+++ b/frc971/orin/set_orin_clock.sh
@@ -6,13 +6,13 @@
ROBOT_PREFIX="9" #71 (Should be one of 79, 89, 99, or 9)
-ORIN_LIST="1"
+ORIN_LIST="1 2"
echo "Setting hwclock on Orins"
for orin in $ORIN_LIST; do
echo "========================================================"
- echo "Setting clock for ${ROBOT_PREFIX}71.1${orin}"
+ echo "Setting clock for ${ROBOT_PREFIX}71.10${orin}"
echo "========================================================"
current_time=`sudo hwclock`
IFS="."
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index d96822b..46b7cea 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -373,6 +373,7 @@
"//frc971/vision:calibration_fbs",
"//third_party:opencv",
"@com_github_google_glog//:glog",
+ "@com_google_absl//absl/strings:str_format",
],
)
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index c48c12d..59a45ac 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -249,18 +249,12 @@
std::stringstream time_ss;
time_ss << realtime_now;
- std::string camera_number_optional = "";
std::optional<uint16_t> camera_number =
frc971::vision::CameraNumberFromChannel(camera_channel_);
- if (camera_number != std::nullopt) {
- camera_number_optional = "-" + std::to_string(camera_number.value());
- }
- const std::string calibration_filename =
- calibration_folder_ +
- absl::StrFormat("/calibration_%s-%d-%d%s_cam-%s_%s.json",
- cpu_type_.value(), team_number.value(),
- cpu_number_.value(), camera_number_optional, camera_id_,
- time_ss.str());
+ CHECK(camera_number.has_value());
+ std::string calibration_filename =
+ CalibrationFilename(calibration_folder_, hostname_, team_number.value(),
+ camera_number.value(), camera_id_, time_ss.str());
LOG(INFO) << calibration_filename << " -> "
<< aos::FlatbufferToJson(camera_calibration,
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index f5c8dab..de9e55a 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -121,23 +121,28 @@
ceres::examples::VectorOfConstraints target_constraints;
for (auto detection = timestamped_target_detections.begin() + 1;
detection < timestamped_target_detections.end(); detection++) {
- auto last_detection = detection - 1;
+ for (int past = 1;
+ past <=
+ std::min<int>(4, detection - timestamped_target_detections.begin());
+ ++past) {
+ auto last_detection = detection - past;
- // Skip two consecutive detections of the same target, because the solver
- // doesn't allow this
- if (detection->id == last_detection->id) {
- continue;
+ // Skip two consecutive detections of the same target, because the solver
+ // doesn't allow this
+ if (detection->id == last_detection->id) {
+ continue;
+ }
+
+ // Don't take into account constraints too far apart in time, because the
+ // recording device could have moved too much
+ if ((detection->time - last_detection->time) > max_dt) {
+ continue;
+ }
+
+ auto confidence = ComputeConfidence(*last_detection, *detection);
+ target_constraints.emplace_back(
+ ComputeTargetConstraint(*last_detection, *detection, confidence));
}
-
- // Don't take into account constraints too far apart in time, because the
- // recording device could have moved too much
- if ((detection->time - last_detection->time) > max_dt) {
- continue;
- }
-
- auto confidence = ComputeConfidence(*last_detection, *detection);
- target_constraints.emplace_back(
- ComputeTargetConstraint(*last_detection, *detection, confidence));
}
return target_constraints;
@@ -228,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()) {
@@ -340,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;
@@ -390,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
@@ -416,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);
@@ -443,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/frc971/vision/vision_util_lib.cc b/frc971/vision/vision_util_lib.cc
index bfd6209..45aa199 100644
--- a/frc971/vision/vision_util_lib.cc
+++ b/frc971/vision/vision_util_lib.cc
@@ -1,5 +1,6 @@
#include "frc971/vision/vision_util_lib.h"
+#include "absl/strings/str_format.h"
#include "glog/logging.h"
namespace frc971::vision {
@@ -58,4 +59,19 @@
return camera_number;
}
+std::string CalibrationFilename(std::string calibration_folder,
+ std::string node_name, int team_number,
+ int camera_number, std::string camera_id,
+ std::string timestamp) {
+ // Get rid of any fractional seconds-- we shouldn't need those and it makes
+ // the string unnecessarily longer
+ timestamp = timestamp.substr(0, timestamp.find("."));
+ std::string calibration_filename =
+ calibration_folder +
+ absl::StrFormat("/calibration_%s-%d-%d_cam-%s_%s.json", node_name.c_str(),
+ team_number, camera_number, camera_id.c_str(),
+ timestamp.c_str());
+ return calibration_filename;
+}
+
} // namespace frc971::vision
diff --git a/frc971/vision/vision_util_lib.h b/frc971/vision/vision_util_lib.h
index 8ce651c..a26af3b 100644
--- a/frc971/vision/vision_util_lib.h
+++ b/frc971/vision/vision_util_lib.h
@@ -25,6 +25,12 @@
// not have a number
std::optional<uint16_t> CameraNumberFromChannel(std::string camera_channel);
+// Return a calibration filename to save to based on the given data
+std::string CalibrationFilename(std::string calibration_folder,
+ std::string node_name, int team_number,
+ int camera_number, std::string camera_id,
+ std::string timestamp);
+
} // namespace frc971::vision
#endif // FRC971_VISION_VISION_UTIL_LIB_H_
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 447e3b4..1c8c5d3 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -120,11 +120,18 @@
AOS_LOG(INFO, "Defaulting to open loop pwm synchronization\n");
}
- // Now that we are configured, actually fill in the defaults.
- timer_handler_->Schedule(
- event_loop_->monotonic_now() +
- (pwm_trigger_ ? chrono::milliseconds(3) : chrono::milliseconds(4)),
- period_);
+ if (pwm_trigger_) {
+ // Now that we are configured, actually fill in the defaults.
+ timer_handler_->Schedule(
+ event_loop_->monotonic_now() +
+ (pwm_trigger_ ? chrono::milliseconds(3) : chrono::milliseconds(4)),
+ period_);
+ } else {
+ // Synchronous CAN wakes up at round multiples of the clock. Use a phased
+ // loop to calculate it.
+ aos::time::PhasedLoop phased_loop(period_, monotonic_clock::now());
+ timer_handler_->Schedule(phased_loop.sleep_time(), period_);
+ }
last_monotonic_now_ = monotonic_clock::now();
}
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/7971.json b/y2024/constants/7971.json
index be70754..3beaae0 100644
--- a/y2024/constants/7971.json
+++ b/y2024/constants/7971.json
@@ -11,13 +11,13 @@
"calibration": {% include 'y2024/constants/calib_files/calibration_imu-7971-0_cam-24-01_2024-03-02_19-44-12.098903651.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-02_20-09-18.022901293.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-03_00-43-52.104451759.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-02_20-09-18.016217514.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-03_00-43-52.104348086.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-02_20-09-18.016860291.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-03_00-43-52.104098137.json' %}
}
],
"robot": {
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 908467d..974fb60 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -48,7 +48,7 @@
"altitude_constants": {
{% set _ = altitude_zero.update(
{
- "measured_absolute_position" : 0.1725
+ "measured_absolute_position" : 0.1877
}
) %}
"zeroing_constants": {{ altitude_zero | tojson(indent=2)}},
@@ -66,11 +66,11 @@
"extend_constants": {
{% set _ = extend_zero.update(
{
- "measured_absolute_position" : 0.135593394632399
+ "measured_absolute_position" : 0.1547
}
) %}
"zeroing_constants": {{ extend_zero | tojson(indent=2)}},
- "potentiometer_offset": {{ -0.2574404033256 + 0.0170793439542 - 0.177097393974999 + 0.3473623911879 }}
+ "potentiometer_offset": {{ -0.2574404033256 + 0.0170793439542 - 0.177097393974999 + 0.3473623911879 - 0.1577}}
}
},
{% include 'y2024/constants/common.json' %}
diff --git a/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-02_20-09-18.016860291.json b/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-03_00-43-52.104451759.json
similarity index 62%
rename from y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-02_20-09-18.016860291.json
rename to y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-03_00-43-52.104451759.json
index d60cc2a..804e854 100755
--- a/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-02_20-09-18.016860291.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-03_00-43-52.104451759.json
@@ -14,18 +14,18 @@
],
"fixed_extrinsics": {
"data": [
- -0.845268,
- 0.031126,
- 0.533435,
- 0.494822,
- -0.525295,
- 0.134521,
- -0.84022,
- -1.212857,
- -0.097911,
- -0.990422,
- -0.097356,
- -0.319412,
+ -0.999409,
+ -0.033871,
+ -0.005938,
+ -0.054149,
+ 0.00624,
+ -0.008812,
+ -0.999942,
+ -0.455382,
+ 0.033816,
+ -0.999387,
+ 0.009018,
+ -0.038435,
0.0,
0.0,
0.0,
@@ -39,7 +39,7 @@
0.000099,
-0.005468
],
- "calibration_timestamp": 1709438958016860291,
+ "calibration_timestamp": 1709455432104451759,
"camera_id": "24-04",
"camera_number": 0
}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-02_20-09-18.022901293.json b/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-03_00-43-52.104348086.json
similarity index 62%
rename from y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-02_20-09-18.022901293.json
rename to y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-03_00-43-52.104348086.json
index ed7b2f3..5e9095f 100755
--- a/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-02_20-09-18.022901293.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-03_00-43-52.104348086.json
@@ -14,18 +14,18 @@
],
"fixed_extrinsics": {
"data": [
- -0.07336,
- 0.085699,
- 0.993617,
- 0.608349,
- -0.989433,
- 0.118683,
- -0.083288,
- -0.939084,
- -0.125063,
- -0.989227,
- 0.076087,
- -0.35071,
+ -0.465205,
+ -0.009078,
+ 0.885157,
+ -0.040515,
+ -0.885133,
+ -0.007831,
+ -0.465273,
+ -0.328485,
+ 0.011156,
+ -0.999928,
+ -0.004392,
+ -0.036284,
0.0,
0.0,
0.0,
@@ -39,7 +39,7 @@
0.000015,
-0.005636
],
- "calibration_timestamp": 1709438958022901293,
+ "calibration_timestamp": 1709455432104348086,
"camera_id": "24-02",
"camera_number": 1
}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-02_20-09-18.016217514.json b/y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-03_00-43-52.104098137.json
similarity index 62%
rename from y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-02_20-09-18.016217514.json
rename to y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-03_00-43-52.104098137.json
index c0847a8..874d282 100755
--- a/y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-02_20-09-18.016217514.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-03_00-43-52.104098137.json
@@ -14,18 +14,18 @@
],
"fixed_extrinsics": {
"data": [
- 0.646787,
- 0.04307,
- 0.761453,
- 0.280984,
- -0.762291,
- 0.067995,
- 0.643653,
- -0.238549,
- -0.024053,
- -0.996756,
- 0.07681,
- -0.200534,
+ 0.530963,
+ -0.006085,
+ 0.847373,
+ -0.047041,
+ -0.847373,
+ -0.011096,
+ 0.530883,
+ -0.127507,
+ 0.006172,
+ -0.99992,
+ -0.011048,
+ -0.04483,
0.0,
0.0,
0.0,
@@ -39,7 +39,7 @@
-0.000061,
-0.00879
],
- "calibration_timestamp": 1709438958016217514,
+ "calibration_timestamp": 1709455432104098137,
"camera_id": "24-03",
"camera_number": 1
}
\ No newline at end of file
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 25fae23..65fe20e 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -269,5 +269,6 @@
"ignore_targets": {
"red": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16],
"blue": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16]
- }
+ },
+ "disable_extend": false
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index c679128..1554f4c 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -197,6 +197,7 @@
altitude_avoid_extend_collision_position: double (id: 28);
autonomous_mode:AutonomousMode (id: 26);
ignore_targets:IgnoreTargets (id: 27);
+ disable_extend:bool (id: 29);
}
table Constants {
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/control_loops/python/altitude.py b/y2024/control_loops/python/altitude.py
index 544eddc..a06bda7 100644
--- a/y2024/control_loops/python/altitude.py
+++ b/y2024/control_loops/python/altitude.py
@@ -31,7 +31,8 @@
kalman_q_vel=2.0,
kalman_q_voltage=2.0,
kalman_r_position=0.05,
- radius=10.5 * 0.0254)
+ radius=10.5 * 0.0254,
+ dt=0.005)
def main(argv):
diff --git a/y2024/control_loops/python/catapult.py b/y2024/control_loops/python/catapult.py
index 49c6305..406f56e 100644
--- a/y2024/control_loops/python/catapult.py
+++ b/y2024/control_loops/python/catapult.py
@@ -40,7 +40,8 @@
kalman_q_voltage=0.7,
kalman_r_position=0.05,
radius=12 * 0.0254,
- delayed_u=1)
+ delayed_u=1,
+ dt=0.005)
kCatapultWithoutGamePiece = angular_system.AngularSystemParams(
name='Catapult',
@@ -57,7 +58,8 @@
kalman_q_voltage=0.7,
kalman_r_position=0.05,
radius=12 * 0.0254,
- delayed_u=1)
+ delayed_u=1,
+ dt=0.005)
def main(argv):
diff --git a/y2024/control_loops/python/climber.py b/y2024/control_loops/python/climber.py
index 997cf28..1b4e3d2 100644
--- a/y2024/control_loops/python/climber.py
+++ b/y2024/control_loops/python/climber.py
@@ -26,6 +26,7 @@
kalman_q_vel=2.00,
kalman_q_voltage=35.0,
kalman_r_position=0.05,
+ dt=0.005,
)
diff --git a/y2024/control_loops/python/drivetrain.py b/y2024/control_loops/python/drivetrain.py
index 282b146..b52f70d 100644
--- a/y2024/control_loops/python/drivetrain.py
+++ b/y2024/control_loops/python/drivetrain.py
@@ -28,6 +28,7 @@
force=True,
kf_q_voltage=1.0,
controller_poles=[0.82, 0.82],
+ dt=0.005,
)
diff --git a/y2024/control_loops/python/extend.py b/y2024/control_loops/python/extend.py
index f95c995..e8f066b 100644
--- a/y2024/control_loops/python/extend.py
+++ b/y2024/control_loops/python/extend.py
@@ -31,6 +31,7 @@
kalman_q_vel=2.0,
kalman_q_voltage=8.0,
kalman_r_position=0.05,
+ dt=0.005,
)
diff --git a/y2024/control_loops/python/intake_pivot.py b/y2024/control_loops/python/intake_pivot.py
index 5ea5c3f..c240d01 100644
--- a/y2024/control_loops/python/intake_pivot.py
+++ b/y2024/control_loops/python/intake_pivot.py
@@ -29,7 +29,8 @@
kalman_q_voltage=1.0,
kalman_r_position=0.05,
radius=6.85 * 0.0254,
- enable_voltage_error=False)
+ enable_voltage_error=False,
+ dt=0.005)
def main(argv):
diff --git a/y2024/control_loops/python/turret.py b/y2024/control_loops/python/turret.py
index 41cc48a..07c3d90 100644
--- a/y2024/control_loops/python/turret.py
+++ b/y2024/control_loops/python/turret.py
@@ -29,7 +29,8 @@
kalman_q_vel=2.0,
kalman_q_voltage=2.0,
kalman_r_position=0.05,
- radius=24 * 0.0254)
+ radius=24 * 0.0254,
+ dt=0.005)
def main(argv):
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 4421e15..79da980 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -181,18 +181,15 @@
const bool disabled = turret_.Correct(turret_goal, position->turret(),
turret_output == nullptr);
+ // Zero out extend goal and position if "disable_extend" is true
collision_avoidance->UpdateGoal(
{.intake_pivot_position = intake_pivot_position,
.turret_position = turret_.estimated_position(),
- .extend_position = extend_position},
- turret_goal->unsafe_goal(), extend_goal);
-
- if (!CatapultRetracted()) {
- altitude_.set_min_position(
- robot_constants_->common()->min_altitude_shooting_angle());
- } else {
- altitude_.clear_min_position();
- }
+ .extend_position =
+ ((!robot_constants_->common()->disable_extend()) ? extend_position
+ : 0.0)},
+ turret_goal->unsafe_goal(),
+ ((!robot_constants_->common()->disable_extend()) ? extend_goal : 0.0));
if (!CatapultRetracted()) {
altitude_.set_min_position(
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index a551993..5363288 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -134,9 +134,6 @@
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- aos::Fetcher<y2024::control_loops::superstructure::CANPosition>
- superstructure_can_position_fetcher_;
-
const Constants *robot_constants_;
CatapultSubsystem catapult_;
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index b30d530..5727546 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -52,7 +52,7 @@
robot_constants_->robot()->extend_constants()->zeroing_constants()),
extend_debouncer_(std::chrono::milliseconds(30),
std::chrono::milliseconds(8)) {
- event_loop->SetRuntimeRealtimePriority(30);
+ event_loop->SetRuntimeRealtimePriority(37);
}
bool PositionNear(double position, double goal, double threshold) {
@@ -159,9 +159,12 @@
robot_constants_->common()->extend_set_points();
// Checks if the extend is close enough to the retracted position to be
- // considered ready to accept note from the transfer rollers.
- const bool extend_at_retracted = PositionNear(
- extend_.position(), extend_set_points->retracted(), kExtendThreshold);
+ // considered ready to accept note from the transfer rollers. If disable
+ // extend is triggered, this will autoatically be false.
+ const bool extend_at_retracted =
+ (!robot_constants_->common()->disable_extend() &&
+ PositionNear(extend_.position(), extend_set_points->retracted(),
+ kExtendThreshold));
// Check if the turret is at the position to accept the note from extend
const bool turret_ready_for_load =
@@ -525,10 +528,13 @@
drivetrain_status_fetcher_.Fetch();
+ // Zero out extend position if "disable_extend" is true
const bool collided = collision_avoidance_.IsCollided({
.intake_pivot_position = intake_pivot_.estimated_position(),
.turret_position = shooter_.turret().estimated_position(),
- .extend_position = extend_.estimated_position(),
+ .extend_position = ((!robot_constants_->common()->disable_extend())
+ ? extend_.estimated_position()
+ : 0.0),
});
aos::FlatbufferFixedAllocatorArray<
@@ -635,6 +641,11 @@
output != nullptr ? &output_struct.extend_voltage : nullptr,
status->fbb());
+ // Zero out extend voltage if "disable_extend" is true
+ if (robot_constants_->common()->disable_extend()) {
+ output_struct.extend_voltage = 0.0;
+ }
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index e2c79d5..9dad4ec 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -328,7 +328,7 @@
SuperstructureTest()
: ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig("y2024/aos_config.json"),
- std::chrono::microseconds(5050)),
+ std::chrono::microseconds(5000)),
simulated_constants_dummy_(SendSimulationConstants(
event_loop_factory(), 7971, "y2024/constants/test_constants.json")),
roborio_(aos::configuration::GetNode(configuration(), "roborio")),
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/orin/can_logger_main.cc b/y2024/orin/can_logger_main.cc
index febed5f..dcfec69 100644
--- a/y2024/orin/can_logger_main.cc
+++ b/y2024/orin/can_logger_main.cc
@@ -11,8 +11,6 @@
::aos::ShmEventLoop event_loop(&config.message());
frc971::can_logger::CanLogger cana_logger(&event_loop, "/can/cana", "cana");
- frc971::can_logger::CanLogger canb_logger(&event_loop, "/can/canb", "canb");
- frc971::can_logger::CanLogger canc_logger(&event_loop, "/can/canc", "canc");
event_loop.Run();
diff --git a/y2024/vision/calibrate_multi_cameras.cc b/y2024/vision/calibrate_multi_cameras.cc
index 8a4bbaa..dee6646 100644
--- a/y2024/vision/calibrate_multi_cameras.cc
+++ b/y2024/vision/calibrate_multi_cameras.cc
@@ -11,6 +11,7 @@
#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/extrinsics_calibration.h"
#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_util_lib.h"
#include "frc971/vision/visualize_robot.h"
// clang-format off
// OpenCV eigen files must be included after Eigen includes
@@ -47,7 +48,6 @@
DEFINE_uint64(
wait_key, 1,
"Time in ms to wait between images (0 to wait indefinitely until click)");
-
DEFINE_bool(robot, false,
"If true we're calibrating extrinsics for the robot, use the "
"correct node path for the robot.");
@@ -364,7 +364,7 @@
void HandleTargetMap(const TargetMap &map,
aos::distributed_clock::time_point distributed_eof,
std::string node_name) {
- VLOG(1) << "Got april tag map call from node " << node_name;
+ VLOG(1) << "Got april tag map call from camera " << node_name;
// Create empty RGB image in this case
cv::Mat rgb_image;
std::vector<TargetMapper::TargetPose> target_poses;
@@ -485,8 +485,9 @@
detection_event_loops.emplace_back(
reader.event_loop_factory()->MakeEventLoop(
(camera_node.camera_name() + "_detection").c_str(), pi));
+ aos::EventLoop *const event_loop = detection_event_loops.back().get();
frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
- detection_event_loops.back().get());
+ event_loop);
// Get the calibration for this orin/camera pair
const calibration::CameraCalibration *calibration =
y2024::vision::FindCameraCalibration(constants_fetcher.constants(),
@@ -505,22 +506,21 @@
VLOG(1) << "Got extrinsics for " << camera_node.camera_name() << " as\n"
<< default_extrinsics.back().matrix();
- detection_event_loops.back()->MakeWatcher(
+ event_loop->MakeWatcher(
camera_node.camera_name(),
- [&reader, &detection_event_loops, camera_node](const TargetMap &map) {
+ [&reader, event_loop, camera_node](const TargetMap &map) {
aos::distributed_clock::time_point pi_distributed_time =
reader.event_loop_factory()
- ->GetNodeEventLoopFactory(
- detection_event_loops.back().get()->node())
+ ->GetNodeEventLoopFactory(event_loop->node())
->ToDistributedClock(aos::monotonic_clock::time_point(
aos::monotonic_clock::duration(
map.monotonic_timestamp_ns())));
HandleTargetMap(map, pi_distributed_time, camera_node.camera_name());
});
- LOG(INFO) << "Created watcher for using the detection event loop for "
- << camera_node.camera_name() << " and size "
- << detection_event_loops.size();
+ VLOG(1) << "Created watcher for using the detection event loop for "
+ << camera_node.camera_name() << " and size "
+ << detection_event_loops.size();
}
reader.event_loop_factory()->Run();
@@ -564,6 +564,8 @@
for (uint i = 0; i < node_list.size() - 1; i++) {
H_camera1_camera2_list.clear();
// Go through the list, and find successive pairs of cameras
+ // We'll be calculating and writing the second of the pair (the i+1'th
+ // camera)
for (auto [pose1, pose2] : detection_list) {
if ((pose1.pi_name == node_list.at(i).camera_name()) &&
(pose2.pi_name == node_list.at(i + 1).camera_name())) {
@@ -576,7 +578,8 @@
}
// Now, get the camera2->boardA map (notice it's boardA, same as
- // camera1, so we can compute the difference based both on boardA)
+ // camera1, so we can compute the difference between the cameras with
+ // both referenced to boardA)
Eigen::Affine3d H_camera2_boardA = pose2.H_camera_target;
// If pose2 isn't referenced to boardA (base_target_id), correct
// that
@@ -682,17 +685,12 @@
std::stringstream time_ss;
time_ss << realtime_now;
- // TODO: This breaks because we're naming orin1 and imu as nodes
- // Assumes camera_list name is of form "/orin#/cameraX" to create
- // calibration filename
+ CameraNode &camera_node = node_list[i + 1];
const std::string calibration_filename =
- FLAGS_output_folder +
- absl::StrFormat(
- "/calibration_orin-%d-%s-%d_cam-%s_%s.json", FLAGS_team_number,
- node_list.at(i + 1).camera_name().substr(5, 1),
- calibration_list[i + 1]->camera_number(),
- calibration_list[i + 1]->camera_id()->data(), time_ss.str());
-
+ frc971::vision::CalibrationFilename(
+ FLAGS_output_folder, camera_node.node_name, FLAGS_team_number,
+ camera_node.camera_number, cal_copy.message().camera_id()->data(),
+ time_ss.str());
LOG(INFO) << calibration_filename << " -> "
<< aos::FlatbufferToJson(merged_calibration,
{.multi_line = true});
diff --git a/y2024/vision/modify_extrinsics.cc b/y2024/vision/modify_extrinsics.cc
index 9122191..d021744 100644
--- a/y2024/vision/modify_extrinsics.cc
+++ b/y2024/vision/modify_extrinsics.cc
@@ -14,6 +14,7 @@
#include "aos/time/time.h"
#include "aos/util/file.h"
#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/vision_util_lib.h"
// This is a helper program to build and rename calibration files
// You can:
@@ -160,11 +161,9 @@
(FLAGS_calibration_folder == ""
? std::filesystem::path(orig_calib_filename).parent_path().string()
: FLAGS_calibration_folder);
- const std::string new_calib_filename =
- dirname + "/" +
- absl::StrFormat("calibration_%s-%d-%d_cam-%s_%s.json", node_name.c_str(),
- team_number, camera_number, camera_id.c_str(),
- time_ss.str());
+ const std::string new_calib_filename = frc971::vision::CalibrationFilename(
+ dirname, node_name.c_str(), team_number, camera_number, camera_id.c_str(),
+ time_ss.str());
VLOG(1) << "From: " << orig_calib_filename << " -> "
<< aos::FlatbufferToJson(base_calibration, {.multi_line = true});
diff --git a/y2024/vision/target_mapping.cc b/y2024/vision/target_mapping.cc
index 7548445..fac7e81 100644
--- a/y2024/vision/target_mapping.cc
+++ b/y2024/vision/target_mapping.cc
@@ -32,7 +32,7 @@
"Write the target constraints to this path");
DEFINE_string(dump_stats_to, "/tmp/mapping_stats.txt",
"Write the mapping stats to this path");
-DEFINE_string(field_name, "charged_up",
+DEFINE_string(field_name, "crescendo",
"Field name, for the output json filename and flatbuffer field");
DEFINE_string(json_path, "y2024/vision/maps/target_map.json",
"Specify path for json with initial pose guesses.");
@@ -52,6 +52,8 @@
DEFINE_uint64(skip_to, 1,
"Start at combined image of this number (1 is the first image)");
DEFINE_bool(solve, true, "Whether to solve for the field's target map.");
+DEFINE_bool(split_field, false,
+ "Whether to break solve into two sides of field");
DEFINE_int32(team_number, 0,
"Required: Use the calibration for a node with this team number");
DEFINE_uint64(wait_key, 1,
@@ -89,6 +91,9 @@
// Contains fixed target poses without solving, for use with visualization
static const TargetMapper kFixedTargetMapper;
+ // Map of TargetId to alliance "color" for splitting field
+ static std::map<uint, std::string> kIdAllianceMap;
+
// Change reference frame from camera to robot
static Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camera_target,
Eigen::Affine3d extrinsics);
@@ -110,8 +115,8 @@
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections_;
VisualizeRobot vis_robot_;
- // Set of node names which are currently drawn on the display
- std::set<std::string> drawn_nodes_;
+ // Set of camera names which are currently drawn on the display
+ std::set<std::string> drawn_cameras_;
// Number of frames displayed
size_t display_count_;
// Last time we drew onto the display image.
@@ -124,6 +129,9 @@
// used to determine if we need to pause for the user to see this frame
// clearly
double max_delta_T_world_robot_;
+ double ignore_count_;
+
+ std::map<std::string, int> camera_numbering_map_;
std::vector<std::unique_ptr<aos::EventLoop>> mapping_event_loops_;
@@ -138,6 +146,12 @@
{"/imu/camera1", cv::Scalar(255, 165, 0)},
};
+std::map<uint, std::string> TargetMapperReplay::kIdAllianceMap = {
+ {1, "red"}, {2, "red"}, {3, "red"}, {4, "red"},
+ {5, "red"}, {6, "blue"}, {7, "blue"}, {8, "blue"},
+ {9, "blue"}, {10, "blue"}, {11, "red"}, {12, "red"},
+ {13, "red"}, {14, "blue"}, {15, "blue"}, {16, "blue"}};
+
const auto TargetMapperReplay::kFixedTargetMapper =
TargetMapper(FLAGS_json_path, ceres::examples::VectorOfConstraints{});
@@ -152,13 +166,17 @@
: reader_(reader),
timestamped_target_detections_(),
vis_robot_(cv::Size(1280, 1000)),
- drawn_nodes_(),
+ drawn_cameras_(),
display_count_(0),
last_draw_time_(aos::distributed_clock::min_time),
last_H_world_robot_(Eigen::Matrix4d::Identity()),
max_delta_T_world_robot_(0.0) {
reader_->RemapLoggedChannel("/orin1/constants", "y2024.Constants");
reader_->RemapLoggedChannel("/imu/constants", "y2024.Constants");
+ // If it's Box of Orins, don't remap roborio constants
+ if (FLAGS_team_number == 7971) {
+ reader_->RemapLoggedChannel("/roborio/constants", "y2024.Constants");
+ }
reader_->Register();
SendSimulationConstants(reader_->event_loop_factory(), FLAGS_team_number,
@@ -168,6 +186,7 @@
node_list.push_back("imu");
node_list.push_back("orin1");
+ int camera_count = 0;
for (std::string node : node_list) {
const aos::Node *pi =
aos::configuration::GetNode(reader->configuration(), node);
@@ -186,6 +205,10 @@
HandleNodeCaptures(
mapping_event_loops_[mapping_event_loops_.size() - 1].get(),
&constants_fetcher, 1);
+ std::string camera0_name = "/" + node + "/camera0";
+ camera_numbering_map_[camera0_name] = camera_count++;
+ std::string camera1_name = "/" + node + "/camera1";
+ camera_numbering_map_[camera1_name] = camera_count++;
}
if (FLAGS_visualize_solver) {
@@ -205,6 +228,11 @@
std::stringstream label;
label << camera_name << " - ";
+ if (map.target_poses()->size() == 0) {
+ VLOG(2) << "Got 0 AprilTags for camera " << camera_name;
+ return;
+ }
+
for (const auto *target_pose_fbs : *map.target_poses()) {
// Skip detections with invalid ids
if (static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) <
@@ -242,13 +270,18 @@
double distance_from_camera = target_pose_camera.p.norm();
double distortion_factor = target_pose_fbs->distortion_factor();
- if (distance_from_camera > 5.0) {
+ double distance_threshold = 5.0;
+ if (distance_from_camera > distance_threshold) {
+ ignore_count_++;
+ LOG(INFO) << "Ignored " << ignore_count_ << " AprilTags with distance "
+ << distance_from_camera << " > " << distance_threshold;
continue;
}
CHECK(map.has_monotonic_timestamp_ns())
<< "Need detection timestamps for mapping";
+ // Detection is usable, so store it
timestamped_target_detections_.emplace_back(
DataAdapter::TimestampedDetection{
.time = node_distributed_time,
@@ -260,7 +293,7 @@
if (FLAGS_visualize_solver) {
// If we've already drawn this camera_name in the current image,
// display the image before clearing and adding the new poses
- if (drawn_nodes_.count(camera_name) != 0) {
+ if (drawn_cameras_.count(camera_name) != 0) {
display_count_++;
cv::putText(vis_robot_.image_,
"Poses #" + std::to_string(display_count_),
@@ -268,7 +301,7 @@
cv::Scalar(255, 255, 255));
if (display_count_ >= FLAGS_skip_to) {
- VLOG(1) << "Showing image for node " << camera_name
+ VLOG(1) << "Showing image for camera " << camera_name
<< " since we've drawn it already";
cv::imshow("View", vis_robot_.image_);
// Pause if delta_T is too large, but only after first image (to make
@@ -284,10 +317,10 @@
}
max_delta_T_world_robot_ = 0.0;
} else {
- VLOG(1) << "At poses #" << std::to_string(display_count_);
+ VLOG(2) << "At poses #" << std::to_string(display_count_);
}
vis_robot_.ClearImage();
- drawn_nodes_.clear();
+ drawn_cameras_.clear();
}
Eigen::Affine3d H_world_target = PoseUtils::Pose3dToAffine3d(
@@ -316,7 +349,7 @@
max_delta_T_world_robot_ =
std::max(delta_T_world_robot, max_delta_T_world_robot_);
- VLOG(1) << "Drew in info for robot " << camera_name << " and target #"
+ VLOG(1) << "Drew in info for camera " << camera_name << " and target #"
<< target_pose_fbs->id();
drew = true;
last_draw_time_ = node_distributed_time;
@@ -325,26 +358,30 @@
}
if (FLAGS_visualize_solver) {
if (drew) {
- // Collect all the labels from a given node, and add the text
- size_t pi_number =
- static_cast<size_t>(camera_name[camera_name.size() - 1] - '0');
+ // Collect all the labels from a given camera, and add the text
+ // TODO: Need to fix this one
+ int position_number = camera_numbering_map_[camera_name];
cv::putText(vis_robot_.image_, label.str(),
- cv::Point(10, 10 + 20 * pi_number), cv::FONT_HERSHEY_PLAIN,
- 1.0, kOrinColors.at(camera_name));
+ cv::Point(10, 10 + 20 * position_number),
+ cv::FONT_HERSHEY_PLAIN, 1.0, kOrinColors.at(camera_name));
- drawn_nodes_.emplace(camera_name);
+ drawn_cameras_.emplace(camera_name);
} else if (node_distributed_time - last_draw_time_ >
std::chrono::milliseconds(30) &&
- display_count_ >= FLAGS_skip_to) {
- cv::putText(vis_robot_.image_, "No detections", cv::Point(10, 0),
- cv::FONT_HERSHEY_PLAIN, 1.0, kOrinColors.at(camera_name));
+ display_count_ >= FLAGS_skip_to && drew) {
+ // TODO: Check on 30ms value-- does this make sense?
+ double delta_t = (node_distributed_time - last_draw_time_).count() / 1e6;
+ VLOG(1) << "Last result was " << delta_t << "ms ago";
+ cv::putText(vis_robot_.image_, "No detections in last 30ms",
+ cv::Point(10, 0), cv::FONT_HERSHEY_PLAIN, 1.0,
+ kOrinColors.at(camera_name));
// Display and clear the image if we haven't draw in a while
VLOG(1) << "Displaying image due to time lapse";
cv::imshow("View", vis_robot_.image_);
cv::waitKey(FLAGS_wait_key);
vis_robot_.ClearImage();
max_delta_T_world_robot_ = 0.0;
- drawn_nodes_.clear();
+ drawn_cameras_.clear();
}
}
}
@@ -383,20 +420,20 @@
auto target_constraints =
DataAdapter::MatchTargetDetections(timestamped_target_detections_);
- // Remove constraints between the two sides of the field - these are
- // basically garbage because of how far the camera is. We will use seeding
- // below to connect the two sides
- target_constraints.erase(
- std::remove_if(target_constraints.begin(), target_constraints.end(),
- [](const auto &constraint) {
- constexpr TargetMapper::TargetId kMaxRedId = 4;
- TargetMapper::TargetId min_id =
- std::min(constraint.id_begin, constraint.id_end);
- TargetMapper::TargetId max_id =
- std::max(constraint.id_begin, constraint.id_end);
- return (min_id <= kMaxRedId && max_id > kMaxRedId);
- }),
- target_constraints.end());
+ if (FLAGS_split_field) {
+ // Remove constraints between the two sides of the field - these are
+ // basically garbage because of how far the camera is. We will use seeding
+ // below to connect the two sides
+ target_constraints.erase(
+ std::remove_if(
+ target_constraints.begin(), target_constraints.end(),
+ [](const auto &constraint) {
+ return (
+ kIdAllianceMap[static_cast<uint>(constraint.id_begin)] !=
+ kIdAllianceMap[static_cast<uint>(constraint.id_end)]);
+ }),
+ target_constraints.end());
+ }
LOG(INFO) << "Solving for locations of tags with "
<< target_constraints.size() << " constraints";
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 37486d7..6b2acc4 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -419,7 +419,7 @@
// Thread 3.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop, robot_constants);
- sensor_reader.set_pwm_trigger(true);
+ sensor_reader.set_pwm_trigger(false);
sensor_reader.set_drivetrain_left_encoder(
std::make_unique<frc::Encoder>(8, 9));
sensor_reader.set_drivetrain_right_encoder(
@@ -492,10 +492,13 @@
7, true, "rio", &rio_signal_registry,
current_limits->climber_stator_current_limit(),
current_limits->climber_supply_current_limit());
- std::shared_ptr<TalonFX> extend = std::make_shared<TalonFX>(
- 12, false, "Drivetrain Bus", &canivore_signal_registry,
- current_limits->extend_stator_current_limit(),
- current_limits->extend_supply_current_limit());
+ std::shared_ptr<TalonFX> extend =
+ (robot_constants->common()->disable_extend())
+ ? nullptr
+ : std::make_shared<TalonFX>(
+ 12, false, "Drivetrain Bus", &canivore_signal_registry,
+ current_limits->extend_stator_current_limit(),
+ current_limits->extend_supply_current_limit());
std::shared_ptr<TalonFX> intake_roller = std::make_shared<TalonFX>(
8, false, "rio", &rio_signal_registry,
current_limits->intake_roller_stator_current_limit(),
@@ -529,7 +532,7 @@
ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
- constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+ constants::Values::kDrivetrainWriterPriority, true, "Drivetrain Bus");
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
can_sensor_reader_event_loop.set_name("CANSensorReader");
@@ -549,7 +552,9 @@
for (auto talonfx :
{intake_pivot, turret, altitude, catapult_one, catapult_two, extend}) {
- canivore_talonfxs.push_back(talonfx);
+ if (talonfx != nullptr) {
+ canivore_talonfxs.push_back(talonfx);
+ }
}
for (auto talonfx : {intake_roller, transfer_roller, climber, extend_roller,
@@ -612,8 +617,10 @@
catapult_two->SerializePosition(
superstructure_can_builder->add_catapult_two(),
control_loops::superstructure::catapult::kOutputRatio);
- extend->SerializePosition(superstructure_can_builder->add_extend(),
- superstructure::extend::kOutputRatio);
+ if (extend != nullptr) {
+ extend->SerializePosition(superstructure_can_builder->add_extend(),
+ superstructure::extend::kOutputRatio);
+ }
superstructure_can_builder->set_status(static_cast<int>(status));
superstructure_can_builder.CheckOk(superstructure_can_builder.Send());
@@ -682,8 +689,10 @@
output.turret_voltage());
talonfx_map.find("climber")->second->WriteVoltage(
output.climber_voltage());
- talonfx_map.find("extend")->second->WriteVoltage(
- output.extend_voltage());
+ if (talonfx_map.find("extend") != talonfx_map.end()) {
+ talonfx_map.find("extend")->second->WriteVoltage(
+ output.extend_voltage());
+ }
talonfx_map.find("intake_roller")
->second->WriteVoltage(output.intake_roller_voltage());
talonfx_map.find("transfer_roller")
@@ -705,7 +714,9 @@
can_superstructure_writer.add_talonfx("catapult_two", catapult_two);
can_superstructure_writer.add_talonfx("turret", turret);
can_superstructure_writer.add_talonfx("climber", climber);
- can_superstructure_writer.add_talonfx("extend", extend);
+ if (!robot_constants->common()->disable_extend()) {
+ can_superstructure_writer.add_talonfx("extend", extend);
+ }
can_superstructure_writer.add_talonfx("intake_roller", intake_roller);
can_superstructure_writer.add_talonfx("transfer_roller", transfer_roller);
can_superstructure_writer.add_talonfx("extend_roller", extend_roller);
@@ -720,16 +731,6 @@
AddLoop(&can_output_event_loop);
- ::aos::ShmEventLoop pwm_event_loop(&config.message());
- SuperstructurePWMWriter superstructure_pwm_writer(&pwm_event_loop);
- superstructure_pwm_writer.set_catapult_kraken_one(
- make_unique<frc::TalonFX>(0));
- superstructure_pwm_writer.set_catapult_kraken_two(
- make_unique<frc::TalonFX>(1));
-
- AddLoop(&pwm_event_loop);
- // Thread 6
-
RunLoops();
}
};
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 5baae1c..ed43e10 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",
@@ -181,7 +181,7 @@
"type": "frc971.vision.CameraImage",
"source_node": "imu",
"channel_storage_duration": 1000000000,
- "frequency": 65,
+ "frequency": 70,
"max_size": 4752384,
"num_readers": 6,
"read_method": "PIN",
@@ -192,7 +192,7 @@
"type": "frc971.vision.CameraImage",
"source_node": "imu",
"channel_storage_duration": 1000000000,
- "frequency": 65,
+ "frequency": 70,
"max_size": 4752384,
"num_readers": 6,
"read_method": "PIN",
@@ -204,7 +204,7 @@
"source_node": "imu",
"logger": "NOT_LOGGED",
"channel_storage_duration": 1000000000,
- "frequency": 65,
+ "frequency": 70,
"max_size": 622384
},
{
@@ -213,42 +213,42 @@
"source_node": "imu",
"logger": "NOT_LOGGED",
"channel_storage_duration": 1000000000,
- "frequency": 65,
+ "frequency": 70,
"max_size": 622384
},
{
"name": "/imu/camera0",
"type": "foxglove.ImageAnnotations",
"source_node": "imu",
- "frequency": 65,
+ "frequency": 70,
"max_size": 50000
},
{
"name": "/imu/camera1",
"type": "foxglove.ImageAnnotations",
"source_node": "imu",
- "frequency": 65,
+ "frequency": 70,
"max_size": 50000
},
{
"name": "/imu/camera0",
"type": "y2024.localizer.Visualization",
"source_node": "imu",
- "frequency": 65,
+ "frequency": 70,
"max_size": 50000
},
{
"name": "/imu/camera1",
"type": "y2024.localizer.Visualization",
"source_node": "imu",
- "frequency": 65,
+ "frequency": 70,
"max_size": 50000
},
{
"name": "/imu/camera0",
"type": "frc971.vision.TargetMap",
"source_node": "imu",
- "frequency": 65,
+ "frequency": 70,
"num_senders": 2,
"max_size": 1024
},
@@ -256,7 +256,7 @@
"name": "/imu/camera1",
"type": "frc971.vision.TargetMap",
"source_node": "imu",
- "frequency": 65,
+ "frequency": 70,
"num_senders": 2,
"max_size": 1024
},
@@ -286,22 +286,6 @@
"max_size": 200
},
{
- "name": "/can/canb",
- "type": "frc971.can_logger.CanFrame",
- "source_node": "imu",
- "frequency": 6000,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/can/canc",
- "type": "frc971.can_logger.CanFrame",
- "source_node": "imu",
- "frequency": 6000,
- "num_senders": 2,
- "max_size": 200
- },
- {
"name": "/localizer",
"type": "frc971.IMUValuesBatch",
"source_node": "imu",
@@ -326,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",
@@ -339,7 +329,8 @@
"name": "message_bridge_client",
"args": [
"--rt_priority=16",
- "--sinit_max_init_timeout=5000"
+ "--sinit_max_init_timeout=5000",
+ "--rmem=8388608"
],
"nodes": [
"imu"
@@ -362,6 +353,13 @@
]
},
{
+ "name": "hardware_monitor",
+ "executable_name": "hardware_monitor",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
"name": "joystick_republish",
"executable_name": "joystick_republish",
"user": "pi",
@@ -374,7 +372,8 @@
"executable_name": "message_bridge_server",
"user": "pi",
"args": [
- "--rt_priority=16"
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
],
"nodes": [
"imu"
@@ -577,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 0b2c6af..6072da8 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",
@@ -120,7 +120,7 @@
"type": "frc971.vision.CameraImage",
"source_node": "orin1",
"channel_storage_duration": 1000000000,
- "frequency": 65,
+ "frequency": 70,
"max_size": 4752384,
"num_readers": 6,
"read_method": "PIN",
@@ -131,7 +131,7 @@
"type": "frc971.vision.CameraImage",
"source_node": "orin1",
"channel_storage_duration": 1000000000,
- "frequency": 65,
+ "frequency": 70,
"max_size": 4752384,
"num_readers": 6,
"read_method": "PIN",
@@ -141,51 +141,53 @@
"name": "/orin1/camera0",
"type": "foxglove.CompressedImage",
"source_node": "orin1",
+ "logger": "NOT_LOGGED",
"channel_storage_duration": 1000000000,
- "frequency": 65,
+ "frequency": 70,
"max_size": 622384
},
{
"name": "/orin1/camera1",
"type": "foxglove.CompressedImage",
"source_node": "orin1",
+ "logger": "NOT_LOGGED",
"channel_storage_duration": 1000000000,
- "frequency": 65,
+ "frequency": 70,
"max_size": 622384
},
{
"name": "/orin1/camera0",
"type": "foxglove.ImageAnnotations",
"source_node": "orin1",
- "frequency": 65,
+ "frequency": 70,
"max_size": 50000
},
{
"name": "/orin1/camera1",
"type": "foxglove.ImageAnnotations",
"source_node": "orin1",
- "frequency": 65,
+ "frequency": 70,
"max_size": 50000
},
{
"name": "/orin1/camera0",
"type": "y2024.localizer.Visualization",
"source_node": "imu",
- "frequency": 65,
+ "frequency": 70,
"max_size": 50000
},
{
"name": "/orin1/camera1",
"type": "y2024.localizer.Visualization",
"source_node": "imu",
- "frequency": 65,
+ "frequency": 70,
"max_size": 50000
},
{
"name": "/orin1/camera0",
"type": "frc971.vision.TargetMap",
"source_node": "orin1",
- "frequency": 65,
+ "frequency": 70,
"num_senders": 2,
"max_size": 1024,
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -208,7 +210,7 @@
"name": "/orin1/camera1",
"type": "frc971.vision.TargetMap",
"source_node": "orin1",
- "frequency": 65,
+ "frequency": 70,
"num_senders": 2,
"max_size": 1024,
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -242,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",
@@ -256,7 +264,8 @@
"executable_name": "message_bridge_client",
"args": [
"--rt_priority=16",
- "--sinit_max_init_timeout=5000"
+ "--sinit_max_init_timeout=5000",
+ "--rmem=8388608"
],
"user": "pi",
"nodes": [
@@ -280,10 +289,18 @@
]
},
{
+ "name": "hardware_monitor",
+ "executable_name": "hardware_monitor",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
"name": "message_bridge_server",
"executable_name": "message_bridge_server",
"args": [
- "--rt_priority=16"
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
],
"user": "pi",
"nodes": [
@@ -427,6 +444,15 @@
"rename": {
"name": "/orin1/camera"
}
+ },
+ {
+ "match": {
+ "name": "/hardware_monitor*",
+ "source_node": "orin1"
+ },
+ "rename": {
+ "name": "/orin1/hardware_monitor"
+ }
}
],
"nodes": [
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 628e9d1..e0e1131 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -401,7 +401,8 @@
"executable_name": "message_bridge_client",
"args": [
"--rt_priority=16",
- "--sinit_max_init_timeout=5000"
+ "--sinit_max_init_timeout=5000",
+ "--rmem=2097152"
],
"nodes": [
"roborio"
@@ -411,7 +412,8 @@
"name": "roborio_message_bridge_server",
"executable_name": "message_bridge_server",
"args": [
- "--rt_priority=16"
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
],
"nodes": [
"roborio"