Prefix LOG and CHECK with AOS_
This prepares us for introducing glog more widely and transitioning
things over where they make sense.
Change-Id: Ic6c208882407bc2153fe875ffc736d66f5c8ade5
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 801135a..6c1f504 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -76,7 +76,7 @@
} else {
Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
#ifdef __linux__
- CHECK_GT(region_vertices.outerSize(), 0);
+ AOS_CHECK_GT(region_vertices.outerSize(), 0);
#else
assert(region_vertices.outerSize() > 0);
#endif
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index 1572526..cdb7fb9 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -23,16 +23,18 @@
const ::Eigen::Matrix<double, 2, 1> start1 = spline1.Point(0.0);
if (!end0.isApprox(start1, 1e-6)) {
- LOG(ERROR, "Splines %d and %d don't line up. [%f, %f] != [%f, %f]\n",
- static_cast<int>(i - 1), static_cast<int>(i), end0(0, 0),
- end0(1, 0), start1(0, 0), start1(1, 0));
+ AOS_LOG(ERROR,
+ "Splines %d and %d don't line up. [%f, %f] != [%f, %f]\n",
+ static_cast<int>(i - 1), static_cast<int>(i), end0(0, 0),
+ end0(1, 0), start1(0, 0), start1(1, 0));
}
const ::Eigen::Matrix<double, 2, 1> dend0 = spline0.DPoint(1.0);
const ::Eigen::Matrix<double, 2, 1> dstart1 = spline1.DPoint(0.0);
if (!dend0.isApprox(dstart1, 1e-6)) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"Splines %d and %d don't line up in the first derivitive. [%f, "
"%f] != [%f, %f]\n",
static_cast<int>(i - 1), static_cast<int>(i), dend0(0, 0),
@@ -43,7 +45,8 @@
const ::Eigen::Matrix<double, 2, 1> ddstart1 = spline1.DDPoint(0.0);
if (!ddend0.isApprox(ddstart1, 1e-6)) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"Splines %d and %d don't line up in the second derivitive. [%f, "
"%f] != [%f, %f]\n",
static_cast<int>(i - 1), static_cast<int>(i), ddend0(0, 0),
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b9d2bc4..4685ed6 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -162,7 +162,7 @@
}
if (accel_squared > 1.03 || accel_squared < 0.97) {
- LOG(DEBUG, "New IMU value, rejecting reading\n");
+ AOS_LOG(DEBUG, "New IMU value, rejecting reading\n");
} else {
// -y is our gyro.
// z accel is down
@@ -172,10 +172,10 @@
down_estimator_.Correct(Y);
}
- LOG(DEBUG,
- "New IMU value, rate is %f, angle %f, fused %f, bias "
- "%f\n",
- rate, angle, down_estimator_.X_hat(0), down_estimator_.X_hat(1));
+ AOS_LOG(DEBUG,
+ "New IMU value, rate is %f, angle %f, fused %f, bias "
+ "%f\n",
+ rate, angle, down_estimator_.X_hat(0), down_estimator_.X_hat(1));
down_U_(0, 0) = rate;
}
down_estimator_.UpdateObserver(down_U_, ::aos::controls::kLoopFrequency);
@@ -185,48 +185,48 @@
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
last_gyro_rate_ = imu_values_fetcher_->gyro_x;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Y_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
last_gyro_rate_ = imu_values_fetcher_->gyro_y;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Z_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
last_gyro_rate_ = imu_values_fetcher_->gyro_z;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
last_gyro_rate_ = -imu_values_fetcher_->gyro_z;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::SPARTAN_GYRO:
if (gyro_reading_fetcher_.Fetch()) {
- LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
last_gyro_rate_ = gyro_reading_fetcher_->velocity;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_SPARTAN_GYRO:
if (gyro_reading_fetcher_.Fetch()) {
- LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
last_gyro_rate_ = -gyro_reading_fetcher_->velocity;
last_gyro_time_ = monotonic_now;
}
break;
default:
- LOG(FATAL, "invalid gyro configured");
+ AOS_LOG(FATAL, "invalid gyro configured");
break;
}
@@ -244,7 +244,7 @@
// TODO(james): Use a watcher (instead of a fetcher) once we support it in
// simulation.
if (localizer_control_fetcher_.Fetch()) {
- LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
+ AOS_LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
localizer_->ResetPosition(
monotonic_now, localizer_control_fetcher_->x,
localizer_control_fetcher_->y, localizer_control_fetcher_->theta,
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 16b0b61..3f3a7f7 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -157,7 +157,7 @@
// Because the check below for have_zeroed_encoders_ will add an
// Observation, do a check here to ensure that initialization has been
// performed and so there is at least one observation.
- CHECK(!observations_.empty());
+ AOS_CHECK(!observations_.empty());
if (!have_zeroed_encoders_) {
// This logic handles ensuring that on the first encoder reading, we
// update the internal state for the encoders to match the reading.
@@ -310,7 +310,7 @@
PredictImpl(obs->U, dt, state, P);
}
if (!(obs->h && obs->dhdx)) {
- CHECK(obs->make_h);
+ AOS_CHECK(obs->make_h);
obs->make_h(*state, *P, &obs->h, &obs->dhdx);
}
CorrectImpl(obs->R, obs->z, obs->h(*state, obs->U), obs->dhdx(*state),
@@ -351,18 +351,19 @@
dhdx,
const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t) {
- CHECK(!observations_.empty());
+ AOS_CHECK(!observations_.empty());
if (!observations_.full() && t < observations_.begin()->t) {
- LOG(ERROR,
- "Dropped an observation that was received before we "
- "initialized.\n");
+ AOS_LOG(ERROR,
+ "Dropped an observation that was received before we "
+ "initialized.\n");
return;
}
auto cur_it =
observations_.PushFromBottom({t, t, State::Zero(), StateSquare::Zero(),
Input::Zero(), z, make_h, h, dhdx, R});
if (cur_it == observations_.end()) {
- LOG(DEBUG,
+ AOS_LOG(
+ DEBUG,
"Camera dropped off of end with time of %fs; earliest observation in "
"queue has time of %fs.\n",
::aos::time::DurationInSeconds(t.time_since_epoch()),
@@ -395,7 +396,7 @@
--prev_it;
cur_it->prev_t = prev_it->t;
// TODO(james): Figure out a saner way of handling this.
- CHECK(U != nullptr);
+ AOS_CHECK(U != nullptr);
cur_it->U = *U;
} else {
cur_it->X_hat = next_it->X_hat;
@@ -411,7 +412,7 @@
// We use X_hat_ and P_ to store the intermediate states, and then
// once we reach the end they will all be up-to-date.
ProcessObservation(&*cur_it, cur_it->t - cur_it->prev_t, &X_hat_, &P_);
- CHECK(X_hat_.allFinite());
+ AOS_CHECK(X_hat_.allFinite());
if (next_it != observations_.end()) {
next_it->X_hat = X_hat_;
next_it->P = P_;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 4c51569..8234b1c 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -75,8 +75,8 @@
if (info != 0) {
// We allow a FATAL error here since this should only be called during
// initialization.
- LOG(FATAL, "Failed to solve %d, controllability: %d\n", info,
- controls::Controllability(A, B));
+ AOS_LOG(FATAL, "Failed to solve %d, controllability: %d\n", info,
+ controls::Controllability(A, B));
}
return K;
}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 1f7e2b0..de59355 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -31,7 +31,7 @@
::aos::MutexLocker locker(&mutex_);
while (run_) {
while (goal_.spline.spline_idx == future_spline_idx_) {
- CHECK(!new_goal_.Wait());
+ AOS_CHECK(!new_goal_.Wait());
if (!run_) {
return;
}
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 5330414..5924eb1 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -60,7 +60,7 @@
const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
- LOG_MATRIX(DEBUG, "U_uncapped", *U);
+ AOS_LOG_MATRIX(DEBUG, "U_uncapped", *U);
Eigen::Matrix<double, 2, 2> position_K;
position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
@@ -75,8 +75,7 @@
const auto drive_error = T_inverse_ * position_error;
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(1, 0), error(3, 0);
- LOG_MATRIX(DEBUG, "error", error);
-
+ AOS_LOG_MATRIX(DEBUG, "error", error);
Eigen::Matrix<double, 2, 1> U_integral;
U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
@@ -139,7 +138,7 @@
if (!output_was_capped_) {
if ((*U - kf_->U_uncapped()).norm() > 0.0001) {
- LOG(FATAL, "U unnecessarily capped\n");
+ AOS_LOG(FATAL, "U unnecessarily capped\n");
}
}
}
@@ -228,7 +227,7 @@
// coordinates. Convert back...
linear_profile_.MoveCurrentState(dt_config_.LeftRightToLinear(kf_->R()));
- LOG(DEBUG, "Saturated while moving\n");
+ AOS_LOG(DEBUG, "Saturated while moving\n");
Eigen::Matrix<double, 2, 1> absolute_angular =
dt_config_.LeftRightToAngular(kf_->R());
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index ae51cb1..9d35a5e 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -86,8 +86,8 @@
// If we would end up with an imaginary number, cap us at 0 acceleration.
// TODO(austin): Investigate when this happens, why, and fix it.
if (squared < 0.0) {
- LOG(ERROR, "Imaginary %f, maxa: %f, fa(%f, %f) -> 0.0\n", squared, maxa, x,
- v);
+ AOS_LOG(ERROR, "Imaginary %f, maxa: %f, fa(%f, %f) -> 0.0\n", squared, maxa,
+ x, v);
return 0.0;
}
const double longitudal_acceleration =
@@ -148,8 +148,8 @@
// If we would end up with an imaginary number, cap us at 0 acceleration.
// TODO(austin): Investigate when this happens, why, and fix it.
if (squared < 0.0) {
- LOG(ERROR, "Imaginary %f, mina: %f, fa(%f, %f) -> 0.0\n", squared, mina, x,
- v);
+ AOS_LOG(ERROR, "Imaginary %f, mina: %f, fa(%f, %f) -> 0.0\n", squared, mina,
+ x, v);
return 0.0;
}
const double longitudal_acceleration =
@@ -221,7 +221,8 @@
acceleration = BackwardAcceleration(distance, velocity);
break;
default:
- LOG(FATAL, "Unknown segment type %d\n",
+ AOS_LOG(
+ FATAL, "Unknown segment type %d\n",
static_cast<int>(plan_segment_type_[DistanceToSegment(distance)]));
break;
}
@@ -229,7 +230,7 @@
if (vcurvature < velocity) {
velocity = vcurvature;
acceleration = 0.0;
- LOG(ERROR, "Curvature won\n");
+ AOS_LOG(ERROR, "Curvature won\n");
}
return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
.finished();
@@ -327,23 +328,23 @@
int info = ::frc971::controls::dlqr<5, 2>(A, B, Q, R, &K, &S);
if (info == 0) {
- LOG_MATRIX(INFO, "K", K);
+ AOS_LOG_MATRIX(INFO, "K", K);
} else {
- LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
- controls::Controllability(A, B));
+ AOS_LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
+ controls::Controllability(A, B));
// TODO(austin): Can we be more clever here? Use the last one? We should
// collect more info about when this breaks down from logs.
K = ::Eigen::Matrix<double, 2, 5>::Zero();
}
::Eigen::EigenSolver<::Eigen::Matrix<double, 5, 5>> eigensolver(A - B * K);
const auto eigenvalues = eigensolver.eigenvalues();
- LOG(DEBUG,
- "Eigenvalues: (%f + %fj), (%f + %fj), (%f + %fj), (%f + %fj), (%f + "
- "%fj)\n",
- eigenvalues(0).real(), eigenvalues(0).imag(), eigenvalues(1).real(),
- eigenvalues(1).imag(), eigenvalues(2).real(), eigenvalues(2).imag(),
- eigenvalues(3).real(), eigenvalues(3).imag(), eigenvalues(4).real(),
- eigenvalues(4).imag());
+ AOS_LOG(DEBUG,
+ "Eigenvalues: (%f + %fj), (%f + %fj), (%f + %fj), (%f + %fj), (%f + "
+ "%fj)\n",
+ eigenvalues(0).real(), eigenvalues(0).imag(), eigenvalues(1).real(),
+ eigenvalues(1).imag(), eigenvalues(2).real(), eigenvalues(2).imag(),
+ eigenvalues(3).real(), eigenvalues(3).imag(), eigenvalues(4).real(),
+ eigenvalues(4).imag());
return K;
}
@@ -397,8 +398,8 @@
const double min_length = length() / static_cast<double>(plan_.size() - 1);
if (starting_distance > ending_distance) {
- LOG(FATAL, "End before start: %f > %f\n", starting_distance,
- ending_distance);
+ AOS_LOG(FATAL, "End before start: %f > %f\n", starting_distance,
+ ending_distance);
}
starting_distance = ::std::min(length(), ::std::max(0.0, starting_distance));
ending_distance = ::std::min(length(), ::std::max(0.0, ending_distance));
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index d2dd2c8..0b35ee8 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -97,10 +97,10 @@
::std::vector<double> backward_plan = trajectory.plan();
- LOG(INFO, "Took %fms to plan\n",
- chrono::duration_cast<chrono::duration<double, ::std::milli>>(end_time -
- start_time)
- .count());
+ AOS_LOG(INFO, "Took %fms to plan\n",
+ chrono::duration_cast<chrono::duration<double, ::std::milli>>(
+ end_time - start_time)
+ .count());
// Now, compute the xva as a function of time.
::std::vector<double> length_plan_t;
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index d69f7db..a6160e7 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -147,7 +147,7 @@
for (int i = 0; i < kNumInputs; ++i) {
if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
- LOG(FATAL, "U out of range\n");
+ AOS_LOG(FATAL, "U out of range\n");
}
}
}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 53c729d..58058c7 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -234,7 +234,7 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateOffset(double offset) {
const double doffset = offset - offset_(0, 0);
- LOG(INFO, "Adjusting offset from %f to %f\n", offset_(0, 0), offset);
+ AOS_LOG(INFO, "Adjusting offset from %f to %f\n", offset_(0, 0), offset);
this->loop_->mutable_X_hat()(0, 0) += doffset;
this->Y_(0, 0) += doffset;
@@ -282,7 +282,7 @@
this->estimators_[0].UpdateEstimate(new_position);
if (this->estimators_[0].error()) {
- LOG(ERROR, "zeroing error\n");
+ AOS_LOG(ERROR, "zeroing error\n");
return;
}
@@ -309,12 +309,12 @@
const char *name, Eigen::Matrix<double, 3, 1> *goal) {
// Limit the goal to min/max allowable positions.
if ((*goal)(0, 0) > range_.upper) {
- LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- range_.upper);
+ AOS_LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ range_.upper);
(*goal)(0, 0) = range_.upper;
}
if ((*goal)(0, 0) < range_.lower) {
- LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ AOS_LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
range_.lower);
(*goal)(0, 0) = range_.lower;
}
@@ -373,7 +373,8 @@
// Returns whether hard limits have been exceeded.
if (position() > range_.upper_hard || position() < range_.lower_hard) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"SingleDOFProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
position(), range_.lower_hard, range_.upper_hard);
return true;
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d5861b7..81783cc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -147,7 +147,7 @@
if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
#if defined(__linux__)
- LOG(FATAL, "U out of range\n");
+ AOS_LOG(FATAL, "U out of range\n");
#else
abort();
#endif
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 7e8f4a7..748086f 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -185,11 +185,11 @@
double safe_goal = goal->unsafe_goal;
if (safe_goal < min_position_) {
- LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
+ AOS_LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
safe_goal = min_position_;
}
if (safe_goal > max_position_) {
- LOG(DEBUG, "Limiting to %f from %f\n", max_position_, safe_goal);
+ AOS_LOG(DEBUG, "Limiting to %f from %f\n", max_position_, safe_goal);
safe_goal = max_position_;
}
profiled_subsystem_.set_unprofiled_goal(safe_goal);
@@ -197,7 +197,7 @@
} break;
case State::ESTOP:
- LOG(ERROR, "Estop\n");
+ AOS_LOG(ERROR, "Estop\n");
disabled = true;
break;
}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 58096f3..96cb782 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -263,7 +263,7 @@
void RunIteration(const GoalType *unsafe_goal, const PositionType *position,
OutputType *output, StatusType *status) {
if (this->WasReset()) {
- LOG(ERROR, "WPILib reset, restarting\n");
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
subsystem_.Reset();
}