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/catapult/catapult.cc b/frc971/control_loops/catapult/catapult.cc
index a827dc9..153d35c 100644
--- a/frc971/control_loops/catapult/catapult.cc
+++ b/frc971/control_loops/catapult/catapult.cc
@@ -64,7 +64,7 @@
std::optional<double> solution = catapult_mpc_.Next();
if (!solution.has_value()) {
- CHECK_NOTNULL(catapult_voltage);
+ CHECK(catapult_voltage != nullptr);
*catapult_voltage = 0.0;
if (catapult_mpc_.started()) {
++shot_count_;
@@ -73,7 +73,7 @@
}
} else {
// TODO(austin): Voltage error?
- CHECK_NOTNULL(catapult_voltage);
+ CHECK(catapult_voltage != nullptr);
if (current_horizon_ == 1) {
battery_voltage = 12.0;
}
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) {
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_converters.h b/frc971/control_loops/hybrid_state_feedback_loop_converters.h
index 588a4d5..ffd579c 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_converters.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop_converters.h
@@ -15,22 +15,28 @@
number_of_states, number_of_inputs, number_of_outputs>>
MakeStateFeedbackHybridPlantCoefficients(
const fbs::StateFeedbackHybridPlantCoefficients &coefficients) {
+ CHECK(coefficients.a_continuous() != nullptr);
+ CHECK(coefficients.b_continuous() != nullptr);
+ CHECK(coefficients.c() != nullptr);
+ CHECK(coefficients.d() != nullptr);
+ CHECK(coefficients.u_max() != nullptr);
+ CHECK(coefficients.u_min() != nullptr);
+ CHECK(coefficients.u_limit_coefficient() != nullptr);
+ CHECK(coefficients.u_limit_constant() != nullptr);
+
return std::make_unique<StateFeedbackHybridPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>(
ToEigenOrDie<number_of_states, number_of_states>(
- *CHECK_NOTNULL(coefficients.a_continuous())),
+ *coefficients.a_continuous()),
ToEigenOrDie<number_of_states, number_of_inputs>(
- *CHECK_NOTNULL(coefficients.b_continuous())),
- ToEigenOrDie<number_of_outputs, number_of_states>(
- *CHECK_NOTNULL(coefficients.c())),
- ToEigenOrDie<number_of_outputs, number_of_inputs>(
- *CHECK_NOTNULL(coefficients.d())),
- ToEigenOrDie<number_of_inputs, 1>(*CHECK_NOTNULL(coefficients.u_max())),
- ToEigenOrDie<number_of_inputs, 1>(*CHECK_NOTNULL(coefficients.u_min())),
+ *coefficients.b_continuous()),
+ ToEigenOrDie<number_of_outputs, number_of_states>(*coefficients.c()),
+ ToEigenOrDie<number_of_outputs, number_of_inputs>(*coefficients.d()),
+ ToEigenOrDie<number_of_inputs, 1>(*coefficients.u_max()),
+ ToEigenOrDie<number_of_inputs, 1>(*coefficients.u_min()),
ToEigenOrDie<number_of_inputs, number_of_states>(
- *CHECK_NOTNULL(coefficients.u_limit_coefficient())),
- ToEigenOrDie<number_of_inputs, 1>(
- *CHECK_NOTNULL(coefficients.u_limit_constant())),
+ *coefficients.u_limit_coefficient()),
+ ToEigenOrDie<number_of_inputs, 1>(*coefficients.u_limit_constant()),
coefficients.delayed_u());
}
@@ -39,14 +45,17 @@
number_of_outputs>>
MakeHybridKalmanCoefficients(
const fbs::HybridKalmanCoefficients &coefficients) {
+ CHECK(coefficients.q_continuous() != nullptr);
+ CHECK(coefficients.r_continuous() != nullptr);
+ CHECK(coefficients.p_steady_state() != nullptr);
return std::make_unique<HybridKalmanCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>(
ToEigenOrDie<number_of_states, number_of_states>(
- *CHECK_NOTNULL(coefficients.q_continuous())),
+ *coefficients.q_continuous()),
ToEigenOrDie<number_of_outputs, number_of_outputs>(
- *CHECK_NOTNULL(coefficients.r_continuous())),
+ *coefficients.r_continuous()),
ToEigenOrDie<number_of_states, number_of_states>(
- *CHECK_NOTNULL(coefficients.p_steady_state())),
+ *coefficients.p_steady_state()),
coefficients.delayed_u());
}
template <int number_of_states, int number_of_inputs, int number_of_outputs>
diff --git a/frc971/control_loops/python/test_drivetrain/drivetrain_json_test.cc b/frc971/control_loops/python/test_drivetrain/drivetrain_json_test.cc
index 8c4d95f..c4a4ebe 100644
--- a/frc971/control_loops/python/test_drivetrain/drivetrain_json_test.cc
+++ b/frc971/control_loops/python/test_drivetrain/drivetrain_json_test.cc
@@ -33,8 +33,9 @@
"frc971/control_loops/python/test_drivetrain/"
"drivetrain_dog_motor_plant.json");
- StateFeedbackLoop<4, 2, 2> json_loop = MakeStateFeedbackLoop<4, 2, 2>(
- *CHECK_NOTNULL(coeffs.message().drivetrain_loop()));
+ CHECK(coeffs.message().drivetrain_loop() != nullptr);
+ StateFeedbackLoop<4, 2, 2> json_loop =
+ MakeStateFeedbackLoop<4, 2, 2>(*coeffs.message().drivetrain_loop());
for (size_t index = 0; index < 4; ++index) {
ASSERT_TRUE(coeffs.span().size() > 0u);
made_loop.set_index(index);
@@ -80,8 +81,9 @@
"frc971/control_loops/python/test_drivetrain/"
"hybrid_velocity_drivetrain.json");
+ CHECK(coeffs.message().hybrid_velocity_drivetrain_loop() != nullptr);
HybridLoop json_loop = MakeHybridStateFeedbackLoop<2, 2, 2>(
- *CHECK_NOTNULL(coeffs.message().hybrid_velocity_drivetrain_loop()));
+ *coeffs.message().hybrid_velocity_drivetrain_loop());
for (size_t index = 0; index < 4; ++index) {
ASSERT_TRUE(coeffs.span().size() > 0u);
made_loop.set_index(index);
diff --git a/frc971/control_loops/state_feedback_loop_converters.h b/frc971/control_loops/state_feedback_loop_converters.h
index 062257d..5e0117a 100644
--- a/frc971/control_loops/state_feedback_loop_converters.h
+++ b/frc971/control_loops/state_feedback_loop_converters.h
@@ -17,22 +17,26 @@
number_of_states, number_of_inputs, number_of_outputs>>
MakeStateFeedbackPlantCoefficients(
const fbs::StateFeedbackPlantCoefficients &coefficients) {
+ CHECK(coefficients.a() != nullptr);
+ CHECK(coefficients.b() != nullptr);
+ CHECK(coefficients.c() != nullptr);
+ CHECK(coefficients.d() != nullptr);
+ CHECK(coefficients.u_max() != nullptr);
+ CHECK(coefficients.u_min() != nullptr);
+ CHECK(coefficients.u_limit_coefficient() != nullptr);
+ CHECK(coefficients.u_limit_constant() != nullptr);
+
return std::make_unique<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>(
- ToEigenOrDie<number_of_states, number_of_states>(
- *CHECK_NOTNULL(coefficients.a())),
- ToEigenOrDie<number_of_states, number_of_inputs>(
- *CHECK_NOTNULL(coefficients.b())),
- ToEigenOrDie<number_of_outputs, number_of_states>(
- *CHECK_NOTNULL(coefficients.c())),
- ToEigenOrDie<number_of_outputs, number_of_inputs>(
- *CHECK_NOTNULL(coefficients.d())),
- ToEigenOrDie<number_of_inputs, 1>(*CHECK_NOTNULL(coefficients.u_max())),
- ToEigenOrDie<number_of_inputs, 1>(*CHECK_NOTNULL(coefficients.u_min())),
+ ToEigenOrDie<number_of_states, number_of_states>(*coefficients.a()),
+ ToEigenOrDie<number_of_states, number_of_inputs>(*coefficients.b()),
+ ToEigenOrDie<number_of_outputs, number_of_states>(*coefficients.c()),
+ ToEigenOrDie<number_of_outputs, number_of_inputs>(*coefficients.d()),
+ ToEigenOrDie<number_of_inputs, 1>(*coefficients.u_max()),
+ ToEigenOrDie<number_of_inputs, 1>(*coefficients.u_min()),
ToEigenOrDie<number_of_inputs, number_of_states>(
- *CHECK_NOTNULL(coefficients.u_limit_coefficient())),
- ToEigenOrDie<number_of_inputs, 1>(
- *CHECK_NOTNULL(coefficients.u_limit_constant())),
+ *coefficients.u_limit_coefficient()),
+ ToEigenOrDie<number_of_inputs, 1>(*coefficients.u_limit_constant()),
std::chrono::nanoseconds(coefficients.dt()), coefficients.delayed_u());
}
@@ -41,12 +45,12 @@
number_of_states, number_of_inputs, number_of_outputs>>
MakeStateFeedbackControllerCoefficients(
const fbs::StateFeedbackControllerCoefficients &coefficients) {
+ CHECK(coefficients.k() != nullptr);
+ CHECK(coefficients.kff() != nullptr);
return std::make_unique<StateFeedbackControllerCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>(
- ToEigenOrDie<number_of_inputs, number_of_states>(
- *CHECK_NOTNULL(coefficients.k())),
- ToEigenOrDie<number_of_inputs, number_of_states>(
- *CHECK_NOTNULL(coefficients.kff())));
+ ToEigenOrDie<number_of_inputs, number_of_states>(*coefficients.k()),
+ ToEigenOrDie<number_of_inputs, number_of_states>(*coefficients.kff()));
}
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -54,14 +58,15 @@
number_of_states, number_of_inputs, number_of_outputs>>
MakeStateFeedbackObserverCoefficients(
const fbs::StateFeedbackObserverCoefficients &coefficients) {
+ CHECK(coefficients.kalman_gain() != nullptr);
+ CHECK(coefficients.q() != nullptr);
+ CHECK(coefficients.r() != nullptr);
return std::make_unique<StateFeedbackObserverCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>(
ToEigenOrDie<number_of_states, number_of_outputs>(
- *CHECK_NOTNULL(coefficients.kalman_gain())),
- ToEigenOrDie<number_of_states, number_of_states>(
- *CHECK_NOTNULL(coefficients.q())),
- ToEigenOrDie<number_of_outputs, number_of_outputs>(
- *CHECK_NOTNULL(coefficients.r())),
+ *coefficients.kalman_gain()),
+ ToEigenOrDie<number_of_states, number_of_states>(*coefficients.q()),
+ ToEigenOrDie<number_of_outputs, number_of_outputs>(*coefficients.r()),
coefficients.delayed_u());
}
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 c084047..811e8e5 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -80,8 +80,8 @@
range(frc971::constants::Range::FromFlatbuffer(common->range())),
zeroing_constants(aos::UnpackFlatbuffer(zeroing)),
make_integral_loop([this]() {
- return MakeStateFeedbackLoop<3, 1, 1>(
- *CHECK_NOTNULL(loop_params->message().loop()));
+ CHECK(loop_params->message().loop() != nullptr);
+ return MakeStateFeedbackLoop<3, 1, 1>(*loop_params->message().loop());
}),
loop_params(std::make_shared<aos::FlatbufferDetachedBuffer<
StaticZeroingSingleDOFProfiledSubsystemCommonParams>>(
@@ -280,7 +280,7 @@
Profile>::Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position,
bool disabled) {
- CHECK_NOTNULL(position);
+ CHECK(position != nullptr);
profiled_subsystem_.Correct(*position);
if (profiled_subsystem_.error()) {
@@ -444,7 +444,7 @@
StaticZeroingSingleDOFProfiledSubsystem<
ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
Profile>::MakeStatus(flatbuffers::FlatBufferBuilder *status_fbb) {
- CHECK_NOTNULL(status_fbb);
+ CHECK(status_fbb != nullptr);
typename ProfiledJointStatus::Builder status_builder =
profiled_subsystem_