Setup standalone replay for localizer
This is what I used for a bit of debugging during the season.
Further improvements to the replay would probably require more
substantial changes to the replay infrastructure overall.
Change-Id: Id0429af78bab58f55344cc05031f793ae1d6097f
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 845575f..2e16e1d 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -31,6 +31,39 @@
],
)
+cc_binary(
+ name = "replay_localizer",
+ srcs = [
+ "replay_localizer.cc",
+ ],
+ defines =
+ cpu_select({
+ "amd64": [
+ "SUPPORT_PLOT=1",
+ ],
+ "arm": [],
+ }),
+ linkstatic = True,
+ deps = [
+ "//frc971/control_loops/drivetrain:localizer_queue",
+ ":localizer",
+ ":event_loop_localizer",
+ ":drivetrain_base",
+ "@com_github_gflags_gflags//:gflags",
+ "//y2019:constants",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//aos:init",
+ "//aos/controls:replay_control_loop",
+ "//frc971/queues:gyro",
+ "//frc971/wpilib:imu_queue",
+ ] + cpu_select({
+ "amd64": [
+ "//third_party/matplotlib-cpp",
+ ],
+ "arm": [],
+ }),
+)
+
cc_library(
name = "polydrivetrain_plants",
srcs = [
diff --git a/y2019/control_loops/drivetrain/replay_localizer.cc b/y2019/control_loops/drivetrain/replay_localizer.cc
new file mode 100644
index 0000000..dd025f8
--- /dev/null
+++ b/y2019/control_loops/drivetrain/replay_localizer.cc
@@ -0,0 +1,415 @@
+#include <fcntl.h>
+
+#include "aos/init.h"
+#include "aos/logging/implementations.h"
+#include "aos/logging/replay.h"
+#include "aos/network/team_number.h"
+#include "aos/queue.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/wpilib/imu.q.h"
+#include "gflags/gflags.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
+#if defined(SUPPORT_PLOT)
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
+
+DEFINE_string(logfile, "", "Path to the logfile to replay.");
+// TODO(james): Figure out how to reliably source team number from logfile.
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
+DEFINE_int32(plot_duration, 30,
+ "Duration, in seconds, to plot after the start time.");
+DEFINE_int32(start_offset, 0,
+ "Time, in seconds, to start replay plot after the first enable.");
+
+namespace y2019 {
+namespace control_loops {
+namespace drivetrain {
+using ::y2019::constants::Field;
+
+typedef TypedLocalizer<
+ constants::Values::kNumCameras, Field::kNumTargets, Field::kNumObstacles,
+ EventLoopLocalizer::kMaxTargetsPerFrame, double> TestLocalizer;
+typedef typename TestLocalizer::Camera TestCamera;
+typedef typename TestCamera::Pose Pose;
+typedef typename TestCamera::LineSegment Obstacle;
+
+#if defined(SUPPORT_PLOT)
+// Plots a line from a vector of Pose's.
+void PlotPlotPts(const ::std::vector<Pose> &poses,
+ const ::std::map<::std::string, ::std::string> &kwargs) {
+ ::std::vector<double> x;
+ ::std::vector<double> y;
+ for (const Pose & p : poses) {
+ x.push_back(p.abs_pos().x());
+ y.push_back(p.abs_pos().y());
+ }
+ matplotlibcpp::plot(x, y, kwargs);
+}
+#endif
+
+class LocalizerReplayer {
+ // This class is for replaying logfiles from the 2019 robot and seeing how the
+ // localizer performs withi any new changes versus how it performed during the
+ // real match. This starts running replay on the first enable and then will
+ // actually plot a subset of the run based on the --plot_duration and
+ // --start_offset flags.
+ public:
+ typedef ::frc971::control_loops::DrivetrainQueue::Goal DrivetrainGoal;
+ typedef ::frc971::control_loops::DrivetrainQueue::Position DrivetrainPosition;
+ typedef ::frc971::control_loops::DrivetrainQueue::Status DrivetrainStatus;
+ typedef ::frc971::control_loops::DrivetrainQueue::Output DrivetrainOutput;
+ typedef ::frc971::IMUValues IMUValues;
+ typedef ::aos::JoystickState JoystickState;
+
+ LocalizerReplayer() : localizer_(GetDrivetrainConfig(), &event_loop_) {
+ replayer_.AddDirectQueueSender<CameraFrame>(
+ "wpilib_interface.stripped.IMU", "camera_frames",
+ ".y2019.control_loops.drivetrain.camera_frames");
+
+ const char *drivetrain = "drivetrain.stripped";
+
+ replayer_.AddHandler<DrivetrainPosition>(
+ drivetrain, "position", [this](const DrivetrainPosition &msg) {
+ if (has_been_enabled_) {
+ localizer_.Update(last_last_U_ * battery_voltage_ / 12.0,
+ msg.sent_time, msg.left_encoder,
+ msg.right_encoder, latest_gyro_, 0.0);
+ if (start_time_ <= msg.sent_time &&
+ start_time_ + plot_duration_ >= msg.sent_time) {
+ t_pos_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ msg.sent_time.time_since_epoch())
+ .count());
+ new_x_.push_back(localizer_.x());
+ new_y_.push_back(localizer_.y());
+ new_theta_.push_back(localizer_.theta());
+ new_left_encoder_.push_back(localizer_.left_encoder());
+ new_right_encoder_.push_back(localizer_.right_encoder());
+ new_left_velocity_.push_back(localizer_.left_velocity());
+ new_right_velocity_.push_back(localizer_.right_velocity());
+
+ left_voltage_.push_back(last_last_U_(0, 0));
+ right_voltage_.push_back(last_last_U_(1, 0));
+ }
+ }
+ });
+
+ replayer_.AddHandler<DrivetrainGoal>(
+ drivetrain, "goal", [this](const DrivetrainGoal &msg) {
+ if (has_been_enabled_ && start_time_ <= msg.sent_time &&
+ (start_time_ + plot_duration_ >= msg.sent_time)) {
+ t_goal_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ msg.sent_time.time_since_epoch())
+ .count());
+ drivetrain_mode_.push_back(msg.controller_type);
+ throttle_.push_back(msg.throttle);
+ wheel_.push_back(msg.wheel);
+ }
+ });
+
+ replayer_.AddHandler<DrivetrainStatus>(
+ drivetrain, "status", [this](const DrivetrainStatus &msg) {
+ if (has_been_enabled_ && start_time_ <= msg.sent_time &&
+ (start_time_ + plot_duration_ >= msg.sent_time)) {
+ t_status_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ msg.sent_time.time_since_epoch())
+ .count());
+ original_x_.push_back(msg.x);
+ original_y_.push_back(msg.y);
+ original_theta_.push_back(msg.theta);
+ original_line_theta_.push_back(msg.line_follow_logging.rel_theta);
+ original_line_goal_theta_.push_back(
+ msg.line_follow_logging.goal_theta);
+ original_line_distance_.push_back(
+ msg.line_follow_logging.distance_to_target);
+ }
+ });
+
+ replayer_.AddHandler<DrivetrainOutput>(
+ drivetrain, "output", [this](const DrivetrainOutput &msg) {
+ last_last_U_ = last_U_;
+ last_U_ << msg.left_voltage, msg.right_voltage;
+ });
+
+ replayer_.AddHandler<frc971::control_loops::drivetrain::LocalizerControl>(
+ drivetrain, "localizer_control",
+ [this](const frc971::control_loops::drivetrain::LocalizerControl &msg) {
+ LOG_STRUCT(DEBUG, "localizer_control", msg);
+ localizer_.ResetPosition(msg.sent_time, msg.x, msg.y, msg.theta,
+ msg.theta_uncertainty,
+ !msg.keep_current_theta);
+ });
+
+ replayer_.AddHandler<JoystickState>(
+ "wpilib_interface.stripped.DSReader", "joystick_state",
+ [this](const JoystickState &msg) {
+ if (msg.enabled && !has_been_enabled_) {
+ has_been_enabled_ = true;
+ start_time_ =
+ msg.sent_time + ::std::chrono::seconds(FLAGS_start_offset);
+ }
+ });
+
+ replayer_.AddHandler<IMUValues>(
+ "wpilib_interface.stripped.IMU", "sending",
+ [this](const IMUValues &msg) {
+ latest_gyro_ = msg.gyro_z;
+ if (has_been_enabled_ && start_time_ <= msg.sent_time &&
+ (start_time_ + plot_duration_ >= msg.sent_time)) {
+ t_imu_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ msg.sent_time.time_since_epoch())
+ .count());
+ lat_accel_.push_back(msg.accelerometer_y);
+ long_accel_.push_back(msg.accelerometer_x);
+ z_accel_.push_back(msg.accelerometer_z);
+ }
+ });
+
+ replayer_.AddHandler<::aos::RobotState>(
+ "wpilib_interface.stripped.SensorReader", "robot_state",
+ [this](const ::aos::RobotState &msg) {
+ battery_voltage_ = msg.voltage_battery;
+ });
+ }
+
+ void ProcessFile(const char *filename) {
+ int fd;
+ if (strcmp(filename, "-") == 0) {
+ fd = STDIN_FILENO;
+ } else {
+ fd = open(filename, O_RDONLY);
+ }
+ if (fd == -1) {
+ PLOG(FATAL, "couldn't open file '%s' for reading", filename);
+ }
+
+ replayer_.OpenFile(fd);
+ ::aos::RawQueue *queue = ::aos::logging::GetLoggingQueue();
+ while (!replayer_.ProcessMessage()) {
+ const ::aos::logging::LogMessage *const msg =
+ static_cast<const ::aos::logging::LogMessage *>(
+ queue->ReadMessage(::aos::RawQueue::kNonBlock));
+ if (msg != NULL) {
+ ::aos::logging::internal::PrintMessage(stdout, *msg);
+
+ queue->FreeMessage(msg);
+ }
+ continue;
+ }
+ replayer_.CloseCurrentFile();
+
+ PCHECK(close(fd));
+
+ Plot();
+ }
+
+ void Plot() {
+#if defined(SUPPORT_PLOT)
+ int start_idx = 0;
+ for (const float t : t_pos_) {
+ if (t < ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ start_time_.time_since_epoch())
+ .count() +
+ 0.0) {
+ ++start_idx;
+ } else {
+ break;
+ }
+ }
+ ::std::vector<float> trunc_orig_x(original_x_.begin() + start_idx,
+ original_x_.end());
+ ::std::vector<float> trunc_orig_y(original_y_.begin() + start_idx,
+ original_y_.end());
+ ::std::vector<float> trunc_new_x(new_x_.begin() + start_idx, new_x_.end());
+ ::std::vector<float> trunc_new_y(new_y_.begin() + start_idx, new_y_.end());
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(trunc_orig_x, trunc_orig_y,
+ {{"label", "original"}, {"marker", "o"}});
+ matplotlibcpp::plot(trunc_new_x, trunc_new_y,
+ {{"label", "new"}, {"marker", "o"}});
+ matplotlibcpp::xlim(-0.1, 19.0);
+ matplotlibcpp::ylim(-5.0, 5.0);
+ Field field;
+ for (const Target &target : field.targets()) {
+ PlotPlotPts(target.PlotPoints(), {{"c", "g"}});
+ }
+ for (const Obstacle &obstacle : field.obstacles()) {
+ PlotPlotPts(obstacle.PlotPoints(), {{"c", "k"}});
+ }
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_status_, original_x_,
+ {{"label", "original x"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_y_,
+ {{"label", "original y"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_theta_,
+ {{"label", "original theta"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_x_, {{"label", "new x"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_y_, {{"label", "new y"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_theta_,
+ {{"label", "new theta"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_pos_, left_voltage_,
+ {{"label", "left voltage"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, right_voltage_,
+ {{"label", "right voltage"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_pos_, new_left_encoder_,
+ {{"label", "left encoder"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_right_encoder_,
+ {{"label", "right encoder"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_left_velocity_,
+ {{"label", "left velocity"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_right_velocity_,
+ {{"label", "right velocity"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_goal_, drivetrain_mode_,
+ {{"label", "mode"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_goal_, throttle_,
+ {{"label", "throttle"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_goal_, wheel_, {{"label", "wheel"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_line_theta_,
+ {{"label", "relative theta"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_line_goal_theta_,
+ {{"label", "goal theta"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_line_distance_,
+ {{"label", "dist to target"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ ::std::vector<double> filt_lat_accel_ = FilterValues(lat_accel_, 10);
+ ::std::vector<double> filt_long_accel_ = FilterValues(long_accel_, 10);
+ ::std::vector<double> filt_z_accel_ = FilterValues(z_accel_, 10);
+ matplotlibcpp::plot(t_imu_, filt_lat_accel_,
+ {{"label", "lateral accel"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_imu_, filt_long_accel_,
+ {{"label", "longitudinal accel"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_imu_, filt_z_accel_,
+ {{"label", "z accel"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+#endif
+ }
+
+ private:
+ // Do a simple moving average with a width of N over the given raw data and
+ // return the results. For calculating values with N/2 of either edge, we
+ // assume that the space past the edges is filled with zeroes.
+ ::std::vector<double> FilterValues(const ::std::vector<double> &raw, int N) {
+ ::std::vector<double> filt;
+ for (int ii = 0; ii < static_cast<int>(raw.size()); ++ii) {
+ double sum = 0;
+ for (int jj = ::std::max(0, ii - N / 2);
+ jj < ::std::min(ii + N / 2, static_cast<int>(raw.size()) - 1);
+ ++jj) {
+ sum += raw[jj];
+ }
+ filt.push_back(sum / static_cast<double>(N));
+ }
+ return filt;
+ }
+
+ // TODO(james): Move to some sort of virtual event loop.
+ ::aos::ShmEventLoop event_loop_;
+
+ EventLoopLocalizer localizer_;
+ // Whether the robot has been enabled yet.
+ bool has_been_enabled_ = false;
+ // Cache of last gyro value to forward to the localizer.
+ double latest_gyro_ = 0.0; // rad/sec
+ double battery_voltage_ = 12.0; // volts
+ ::Eigen::Matrix<double, 2, 1> last_U_{0, 0};
+ ::Eigen::Matrix<double, 2, 1> last_last_U_{0, 0};
+
+ ::aos::logging::linux_code::LogReplayer replayer_;
+
+ // Time at which to start the plot.
+ ::aos::monotonic_clock::time_point start_time_;
+ // Length of time to plot.
+ ::std::chrono::seconds plot_duration_{FLAGS_plot_duration};
+
+ // All the values to plot, organized in blocks by vectors of data that come in
+ // at the same time (e.g., status messages come in offset from position
+ // messages and so while they are all generally the same frequency, we can end
+ // receiving slightly different numbers of messages on different queues).
+
+ // TODO(james): Improve plotting support.
+ ::std::vector<double> t_status_;
+ ::std::vector<double> original_x_;
+ ::std::vector<double> original_y_;
+ ::std::vector<double> original_theta_;
+ ::std::vector<double> original_line_theta_;
+ ::std::vector<double> original_line_goal_theta_;
+ ::std::vector<double> original_line_distance_;
+
+ ::std::vector<double> t_pos_;
+ ::std::vector<double> new_x_;
+ ::std::vector<double> new_y_;
+ ::std::vector<double> new_left_encoder_;
+ ::std::vector<double> new_right_encoder_;
+ ::std::vector<double> new_left_velocity_;
+ ::std::vector<double> new_right_velocity_;
+ ::std::vector<double> new_theta_;
+ ::std::vector<double> left_voltage_;
+ ::std::vector<double> right_voltage_;
+
+ ::std::vector<double> t_goal_;
+ ::std::vector<double> drivetrain_mode_;
+ ::std::vector<double> throttle_;
+ ::std::vector<double> wheel_;
+
+ ::std::vector<double> t_imu_;
+ ::std::vector<double> lat_accel_;
+ ::std::vector<double> z_accel_;
+ ::std::vector<double> long_accel_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2019
+
+int main(int argc, char *argv[]) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+
+ if (FLAGS_logfile.empty()) {
+ fprintf(stderr, "Need a file to replay!\n");
+ return EXIT_FAILURE;
+ }
+
+ ::aos::network::OverrideTeamNumber(FLAGS_team);
+
+ ::aos::InitCreate();
+
+ ::y2019::control_loops::drivetrain::LocalizerReplayer replay;
+ replay.ProcessFile(FLAGS_logfile.c_str());
+
+ // TODO(james): No clue if the Clear() functions are actually accomplishing
+ // things. Mostly just paranoia left over from earlier on.
+ ::frc971::imu_values.Clear();
+ ::frc971::control_loops::drivetrain_queue.goal.Clear();
+ ::frc971::control_loops::drivetrain_queue.status.Clear();
+ ::frc971::control_loops::drivetrain_queue.position.Clear();
+ ::frc971::control_loops::drivetrain_queue.output.Clear();
+
+ ::aos::Cleanup();
+}