Remove usage of CHECK_NOTNULL
We want to switch to absl logging instead of glog. gtest and ceres are
going there, and we already have absl as a dependency. ABSL doesn't
have CHECK_NOTNULL, and we can move things over in an easier to review
fashion.
Change-Id: Ifd9a11ec34a2357cec43f88dba015db9c28ed2cf
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index bcd9d4c..80ef4c4 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -77,7 +77,7 @@
}
std::vector<Spline> FlatbufferToSplines(const MultiSpline *fb) {
- CHECK_NOTNULL(fb);
+ CHECK(fb != nullptr);
const size_t spline_count = fb->spline_count();
CHECK_EQ(fb->spline_x()->size(), static_cast<size_t>(spline_count * 5 + 1));
CHECK_EQ(fb->spline_y()->size(), static_cast<size_t>(spline_count * 5 + 1));
@@ -95,7 +95,7 @@
aos::SizedArray<Spline, FinishedDistanceSpline::kMaxSplines>
SizedFlatbufferToSplines(const MultiSpline *fb) {
- CHECK_NOTNULL(fb);
+ CHECK(fb != nullptr);
const size_t spline_count = fb->spline_count();
CHECK_EQ(fb->spline_x()->size(), static_cast<size_t>(spline_count * 5 + 1));
CHECK_EQ(fb->spline_y()->size(), static_cast<size_t>(spline_count * 5 + 1));
@@ -141,8 +141,8 @@
fbb->CreateUninitializedVector(num_points, &spline_x_vector);
const flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
fbb->CreateUninitializedVector(num_points, &spline_y_vector);
- CHECK_NOTNULL(spline_x_vector);
- CHECK_NOTNULL(spline_y_vector);
+ CHECK(spline_x_vector != nullptr);
+ CHECK(spline_y_vector != nullptr);
spline_x_vector[0] = splines()[0].control_points()(0, 0);
spline_y_vector[0] = splines()[0].control_points()(1, 0);
for (size_t spline_index = 0; spline_index < splines().size();
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 4c35b6a..ae1a31a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -43,9 +43,11 @@
if (fbs == nullptr) {
return {};
}
+ CHECK(fbs->q() != nullptr);
+ CHECK(fbs->r() != nullptr);
return LineFollowConfig{
- .Q = ToEigenOrDie<3, 3>(*CHECK_NOTNULL(fbs->q())),
- .R = ToEigenOrDie<2, 2>(*CHECK_NOTNULL(fbs->r())),
+ .Q = ToEigenOrDie<3, 3>(*fbs->q()),
+ .R = ToEigenOrDie<2, 2>(*fbs->r()),
.max_controllable_offset = fbs->max_controllable_offset()};
}
};
@@ -71,9 +73,10 @@
if (fbs == nullptr) {
return {};
}
- return SplineFollowerConfig{
- .Q = ToEigenOrDie<5, 5>(*CHECK_NOTNULL(fbs->q())),
- .R = ToEigenOrDie<2, 2>(*CHECK_NOTNULL(fbs->r()))};
+ CHECK(fbs->q() != nullptr);
+ CHECK(fbs->r() != nullptr);
+ return SplineFollowerConfig{.Q = ToEigenOrDie<5, 5>(*fbs->q()),
+ .R = ToEigenOrDie<2, 2>(*fbs->r())};
}
};
@@ -205,36 +208,45 @@
fbs_copy = std::make_shared<
aos::FlatbufferDetachedBuffer<fbs::DrivetrainConfig>>(
aos::RecursiveCopyFlatBuffer(&fbs));
+ CHECK(fbs_copy->message().loop_config()->drivetrain_loop() != nullptr);
+ CHECK(fbs_copy->message().loop_config()->velocity_drivetrain_loop() !=
+ nullptr);
+ CHECK(fbs_copy->message().loop_config()->kalman_drivetrain_loop() !=
+ nullptr);
+ CHECK(
+ fbs_copy->message().loop_config()->hybrid_velocity_drivetrain_loop() !=
+ nullptr);
+ CHECK(fbs.imu_transform() != nullptr);
return {
#define ASSIGN(field) .field = fbs.field()
ASSIGN(shifter_type), ASSIGN(loop_type), ASSIGN(gyro_type),
ASSIGN(imu_type),
.make_drivetrain_loop =
[fbs_copy]() {
- return MakeStateFeedbackLoop<4, 2, 2>(*CHECK_NOTNULL(
- fbs_copy->message().loop_config()->drivetrain_loop()));
+ return MakeStateFeedbackLoop<4, 2, 2>(
+ *fbs_copy->message().loop_config()->drivetrain_loop());
},
.make_v_drivetrain_loop =
[fbs_copy]() {
return MakeStateFeedbackLoop<2, 2, 2>(
- *CHECK_NOTNULL(fbs_copy->message()
- .loop_config()
- ->velocity_drivetrain_loop()));
+ *fbs_copy->message()
+ .loop_config()
+ ->velocity_drivetrain_loop());
},
.make_kf_drivetrain_loop =
[fbs_copy]() {
return MakeStateFeedbackLoop<7, 2, 4>(
- *CHECK_NOTNULL(fbs_copy->message()
- .loop_config()
- ->kalman_drivetrain_loop()));
+ *fbs_copy->message()
+ .loop_config()
+ ->kalman_drivetrain_loop());
},
#if defined(__linux__)
.make_hybrid_drivetrain_velocity_loop =
[fbs_copy]() {
return MakeHybridStateFeedbackLoop<2, 2, 2>(
- *CHECK_NOTNULL(fbs_copy->message()
- .loop_config()
- ->hybrid_velocity_drivetrain_loop()));
+ *fbs_copy->message()
+ .loop_config()
+ ->hybrid_velocity_drivetrain_loop());
},
#endif
.dt = std::chrono::nanoseconds(fbs.loop_config()->dt()),
@@ -253,8 +265,7 @@
ASSIGN(wheel_non_linearity), ASSIGN(quickturn_wheel_multiplier),
ASSIGN(wheel_multiplier),
ASSIGN(pistol_grip_shift_enables_line_follow),
- .imu_transform =
- ToEigenOrDie<3, 3>(*CHECK_NOTNULL(fbs.imu_transform())),
+ .imu_transform = ToEigenOrDie<3, 3>(*fbs.imu_transform()),
ASSIGN(is_simulated),
.down_estimator_config =
aos::UnpackFlatbuffer(fbs.down_estimator_config()),
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 5082f39..c1aa2a5 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -75,7 +75,8 @@
// Run for enough time to allow the gyro/imu zeroing code to run.
RunFor(std::chrono::seconds(15));
CHECK(drivetrain_status_fetcher_.Fetch());
- EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->zeroing())->zeroed());
+ CHECK(drivetrain_status_fetcher_->zeroing() != nullptr);
+ EXPECT_TRUE(drivetrain_status_fetcher_->zeroing()->zeroed());
}
virtual ~DrivetrainTest() {}
@@ -97,10 +98,11 @@
void VerifyNearSplineGoal() {
drivetrain_status_fetcher_.Fetch();
+ CHECK(drivetrain_status_fetcher_->trajectory_logging() != nullptr);
const double expected_x =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ drivetrain_status_fetcher_->trajectory_logging()->x();
const double expected_y =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
+ drivetrain_status_fetcher_->trajectory_logging()->y();
const double estimated_x = drivetrain_status_fetcher_->x();
const double estimated_y = drivetrain_status_fetcher_->y();
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
@@ -114,15 +116,16 @@
do {
RunFor(dt());
EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
- } while (!CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
- ->is_executed());
+ CHECK(drivetrain_status_fetcher_->trajectory_logging() != nullptr);
+ } while (!drivetrain_status_fetcher_->trajectory_logging()->is_executed());
}
void VerifyDownEstimator() {
EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
// TODO(james): Handle Euler angle singularities...
+ CHECK(drivetrain_status_fetcher_->down_estimator() != nullptr);
const double down_estimator_yaw =
- CHECK_NOTNULL(drivetrain_status_fetcher_->down_estimator())->yaw();
+ drivetrain_status_fetcher_->down_estimator()->yaw();
const double localizer_yaw = drivetrain_status_fetcher_->theta();
EXPECT_LT(std::abs(aos::math::DiffAngle(down_estimator_yaw, localizer_yaw)),
1e-2);
@@ -636,8 +639,9 @@
// Check that we are pointed the right direction:
drivetrain_status_fetcher_.Fetch();
auto actual = drivetrain_plant_.state();
+ CHECK(drivetrain_status_fetcher_->trajectory_logging() != nullptr);
const double expected_theta =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->theta();
+ drivetrain_status_fetcher_->trajectory_logging()->theta();
// As a sanity check, compare both against absolute angle and the spline's
// goal angle.
EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 5e-2);
@@ -739,10 +743,9 @@
EXPECT_EQ(0.0, drivetrain_output_fetcher_->right_voltage());
// The goal should be null after stopping.
ASSERT_TRUE(drivetrain_status_fetcher_.Fetch());
- EXPECT_FALSE(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
- ->has_x());
- EXPECT_FALSE(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
- ->has_y());
+ CHECK(drivetrain_status_fetcher_->trajectory_logging() != nullptr);
+ EXPECT_FALSE(drivetrain_status_fetcher_->trajectory_logging()->has_x());
+ EXPECT_FALSE(drivetrain_status_fetcher_->trajectory_logging()->has_y());
}
}
@@ -808,10 +811,9 @@
// The goal should be empty.
drivetrain_status_fetcher_.Fetch();
- EXPECT_FALSE(
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->has_x());
- EXPECT_FALSE(
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->has_y());
+ CHECK(drivetrain_status_fetcher_->trajectory_logging() != nullptr);
+ EXPECT_FALSE(drivetrain_status_fetcher_->trajectory_logging()->has_x());
+ EXPECT_FALSE(drivetrain_status_fetcher_->trajectory_logging()->has_y());
}
class DrivetrainBackwardsParamTest
@@ -970,10 +972,11 @@
// Since the voltage error compensation is disabled, expect that we will have
// *failed* to reach our goal.
drivetrain_status_fetcher_.Fetch();
+ CHECK(drivetrain_status_fetcher_->trajectory_logging() != nullptr);
const double expected_x =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ drivetrain_status_fetcher_->trajectory_logging()->x();
const double expected_y =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
+ drivetrain_status_fetcher_->trajectory_logging()->y();
const double estimated_x = drivetrain_status_fetcher_->x();
const double estimated_y = drivetrain_status_fetcher_->y();
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
@@ -1497,17 +1500,18 @@
// We should always just have the past kNumStoredSplines available.
drivetrain_status_fetcher_.Fetch();
- ASSERT_EQ(expected_splines.size(),
- CHECK_NOTNULL(drivetrain_status_fetcher_.get()
- ->trajectory_logging()
- ->available_splines())
- ->size());
+ CHECK(drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->available_splines() != nullptr);
+ ASSERT_EQ(expected_splines.size(), drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->available_splines()
+ ->size());
for (size_t ii = 0; ii < expected_splines.size(); ++ii) {
- EXPECT_EQ(expected_splines[ii],
- CHECK_NOTNULL(drivetrain_status_fetcher_.get()
- ->trajectory_logging()
- ->available_splines())
- ->Get(ii));
+ EXPECT_EQ(expected_splines[ii], drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->available_splines()
+ ->Get(ii));
}
}
}
@@ -1529,19 +1533,13 @@
RunFor(chrono::seconds(5));
drivetrain_status_fetcher_.Fetch();
- EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
- ->frozen());
- EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
- ->have_target());
- EXPECT_EQ(
- 1.0,
- CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())->x());
- EXPECT_EQ(
- 1.0,
- CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())->y());
- EXPECT_FLOAT_EQ(
- M_PI_4, CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
- ->theta());
+ ASSERT_TRUE(drivetrain_status_fetcher_->line_follow_logging() != nullptr);
+ EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging()->frozen());
+ EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging()->have_target());
+ EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging()->x());
+ EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging()->y());
+ EXPECT_FLOAT_EQ(M_PI_4,
+ drivetrain_status_fetcher_->line_follow_logging()->theta());
// Should have run off the end of the target, running along the y=x line.
EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 737c179..d3de332 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -741,7 +741,8 @@
}
if (obs->h == nullptr) {
CHECK(obs->make_h != nullptr);
- obs->h = CHECK_NOTNULL(obs->make_h->MakeExpectedObservations(*state, *P));
+ obs->h = obs->make_h->MakeExpectedObservations(*state, *P);
+ CHECK(obs->h != nullptr);
}
CorrectImpl(obs, state, P);
}
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index c1adb9a..ff6f6c5 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -109,7 +109,8 @@
// verbosity here seems appropriate.
Eigen::Matrix<double, 4, 4> FlatbufferToTransformationMatrix(
const frc971::vision::calibration::TransformationMatrix &flatbuffer) {
- CHECK_EQ(16u, CHECK_NOTNULL(flatbuffer.data())->size());
+ CHECK(flatbuffer.data() != nullptr);
+ CHECK_EQ(16u, flatbuffer.data()->size());
Eigen::Matrix<double, 4, 4> result;
result.setIdentity();
for (int row = 0; row < 4; ++row) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index f052a9e..ebe0f64 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -79,7 +79,9 @@
}
void SplineDrivetrain::DeleteCurrentSpline() {
- DeleteTrajectory(&CHECK_NOTNULL(current_trajectory())->trajectory());
+ const FinishedTrajectory *const trajectory = current_trajectory();
+ CHECK(trajectory != nullptr);
+ DeleteTrajectory(&trajectory->trajectory());
executing_spline_ = false;
commanded_spline_.reset();
current_xva_.setZero();
@@ -95,12 +97,15 @@
DeleteCurrentSpline();
return;
} else {
- if (executing_spline_ &&
- CHECK_NOTNULL(current_trajectory())->spline_handle() !=
- *commanded_spline) {
- // If we are executing a spline, and the handle has changed, garbage
- // collect the old spline.
- DeleteCurrentSpline();
+ if (executing_spline_) {
+ const FinishedTrajectory *const trajectory = current_trajectory();
+ CHECK(trajectory != nullptr);
+
+ if (trajectory->spline_handle() != *commanded_spline) {
+ // If we are executing a spline, and the handle has changed, garbage
+ // collect the old spline.
+ DeleteCurrentSpline();
+ }
}
}
}
@@ -144,8 +149,8 @@
enable_ = enable;
if (enable && executing_spline_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
- const FinishedTrajectory *const trajectory =
- CHECK_NOTNULL(current_trajectory());
+ const FinishedTrajectory *const trajectory = current_trajectory();
+ CHECK(trajectory != nullptr);
if (!IsAtEnd() && executing_spline_) {
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
@@ -258,7 +263,9 @@
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
trajectory_logging_builder.add_x(goal_state(0));
trajectory_logging_builder.add_y(goal_state(1));
- if (CHECK_NOTNULL(current_trajectory())->drive_spline_backwards()) {
+ const FinishedTrajectory *const trajectory = current_trajectory();
+ CHECK(trajectory != nullptr);
+ if (trajectory->drive_spline_backwards()) {
trajectory_logging_builder.add_left_velocity(-goal_state(4));
trajectory_logging_builder.add_right_velocity(-goal_state(3));
trajectory_logging_builder.add_theta(
@@ -285,10 +292,14 @@
trajectory_logging_builder.add_current_spline_idx(*commanded_spline_);
}
}
- trajectory_logging_builder.add_distance_remaining(
- executing_spline_
- ? CHECK_NOTNULL(current_trajectory())->length() - current_xva_.x()
- : 0.0);
+ if (executing_spline_) {
+ const FinishedTrajectory *const trajectory = current_trajectory();
+ CHECK(trajectory != nullptr);
+ trajectory_logging_builder.add_distance_remaining(trajectory->length() -
+ current_xva_.x());
+ } else {
+ trajectory_logging_builder.add_distance_remaining(0.0);
+ }
trajectory_logging_builder.add_available_splines(handles_vector);
trajectory_logging_builder.add_distance_traveled(
executing_spline_ ? current_xva_.x() : 0.0);
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 24fb66c..09bdc71 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -48,15 +48,23 @@
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
- return executing_spline_ ? CHECK_NOTNULL(current_trajectory())
- ->GoalState(current_xva_(0), current_xva_(1))
- : ::Eigen::Matrix<double, 5, 1>::Zero();
+ if (executing_spline_) {
+ const FinishedTrajectory *finished = current_trajectory();
+ CHECK(finished != nullptr);
+ return finished->GoalState(current_xva_(0), current_xva_(1));
+ } else {
+ return ::Eigen::Matrix<double, 5, 1>::Zero();
+ }
}
bool IsAtEnd() const {
- return executing_spline_ ? CHECK_NOTNULL(current_trajectory())
- ->is_at_end(current_xva_.block<2, 1>(0, 0))
- : true;
+ if (!executing_spline_) {
+ return true;
+ }
+
+ const FinishedTrajectory *finished = current_trajectory();
+ CHECK(finished != nullptr);
+ return finished->is_at_end(current_xva_.block<2, 1>(0, 0));
}
size_t trajectory_count() const { return trajectories_.size(); }
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index cf9929f..4db41ee 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -38,9 +38,13 @@
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
velocity_drivetrain)
- : BaseTrajectory(CHECK_NOTNULL(CHECK_NOTNULL(buffer->spline())->spline())
- ->constraints(),
- config, std::move(velocity_drivetrain)),
+ : BaseTrajectory(
+ [&]() {
+ CHECK(buffer->spline() != nullptr);
+ CHECK(buffer->spline()->spline() != nullptr);
+ return buffer->spline()->spline()->constraints();
+ }(),
+ config, std::move(velocity_drivetrain)),
buffer_(buffer),
spline_(*buffer_->spline()) {}
@@ -819,8 +823,9 @@
Eigen::Matrix<double, 2, 5> FinishedTrajectory::GainForDistance(
double distance) const {
+ CHECK(trajectory().gains() != nullptr);
const flatbuffers::Vector<flatbuffers::Offset<fb::GainPoint>> &gains =
- *CHECK_NOTNULL(trajectory().gains());
+ *trajectory().gains();
CHECK_LT(0u, gains.size());
size_t index = 0;
for (index = 0; index < gains.size() - 1; ++index) {