Create an enum for sender errors
Will replace usages of bools, and will now currently only be used
for indicating that messages were sent too fast
After we merge this commit we will replace this enum with a general
Status class for all of aos, similar to absl::Status.
Change-Id: I4b5b2e7685744b3c6826a241cd3c84190eaa96ee
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/frc971/control_loops/control_loop-tmpl.h b/frc971/control_loops/control_loop-tmpl.h
index 0db1786..7e20227 100644
--- a/frc971/control_loops/control_loop-tmpl.h
+++ b/frc971/control_loops/control_loop-tmpl.h
@@ -23,7 +23,7 @@
OutputType>::ZeroOutputs() {
typename ::aos::Sender<OutputType>::Builder builder =
output_sender_.MakeBuilder();
- builder.Send(Zero(&builder));
+ builder.CheckOk(builder.Send(Zero(&builder)));
}
template <class GoalType, class PositionType, class StatusType,
diff --git a/frc971/control_loops/control_loop_test.h b/frc971/control_loops/control_loop_test.h
index de9d4cd..a820215 100644
--- a/frc971/control_loops/control_loop_test.h
+++ b/frc971/control_loops/control_loop_test.h
@@ -11,6 +11,7 @@
#include "aos/time/time.h"
#include "frc971/input/joystick_state_generated.h"
#include "frc971/input/robot_state_generated.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
namespace frc971 {
@@ -123,7 +124,8 @@
builder.add_autonomous(false);
builder.add_team_id(team_id_);
- new_state.Send(builder.Finish());
+ CHECK_EQ(new_state.Send(builder.Finish()),
+ aos::RawSender::Error::kOk);
last_ds_time_ = monotonic_now();
last_enabled_ = enabled_;
@@ -150,7 +152,7 @@
builder.add_voltage_roborio_in(battery_voltage_);
builder.add_voltage_battery(battery_voltage_);
- new_state.Send(builder.Finish());
+ new_state.CheckOk(new_state.Send(builder.Finish()));
}
static constexpr ::std::chrono::microseconds kTimeTick{5000};
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 928d418..62b77c4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -526,7 +526,10 @@
builder.add_down_estimator(down_estimator_state_offset);
builder.add_localizer(localizer_offset);
builder.add_zeroing(zeroer_offset);
- status->Send(builder.Finish());
+
+ builder.add_send_failures(status_failure_counter_.failures());
+
+ status_failure_counter_.Count(status->Send(builder.Finish()));
}
// If the filters aren't ready/valid, then disable all outputs (currently,
@@ -568,7 +571,7 @@
}
if (output) {
- output->Send(Output::Pack(*output->fbb(), &output_struct));
+ output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index efaf53d..52ae605 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -2,9 +2,8 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
#include "Eigen/Dense"
-#include "frc971/control_loops/control_loop.h"
-#include "frc971/control_loops/polytope.h"
#include "aos/util/log_interval.h"
+#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
@@ -20,6 +19,7 @@
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/polytope.h"
#include "frc971/queues/gyro_generated.h"
#include "frc971/wpilib/imu_batch_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
@@ -185,6 +185,8 @@
LineFollowDrivetrain dt_line_follow_;
bool has_been_enabled_ = false;
+
+ aos::SendFailureCounter status_failure_counter_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d658e21..3ad4776 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -183,7 +183,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(2));
VerifyNearGoal();
@@ -199,7 +200,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Sanity check that the drivetrain is indeed commanding voltage while the IMU
@@ -235,7 +237,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
drivetrain_plant_.set_left_voltage_offset(1.0);
drivetrain_plant_.set_right_voltage_offset(1.0);
@@ -251,7 +254,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
if (i > 20 && i < 200) {
@@ -281,7 +285,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(4.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
@@ -305,7 +310,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(3.9);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
RunFor(dt());
@@ -366,7 +372,8 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -408,7 +415,8 @@
goal_builder.add_right_goal(1.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -450,7 +458,8 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -473,7 +482,8 @@
goal_builder.add_throttle(1.0);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(1));
@@ -486,7 +496,8 @@
goal_builder.add_throttle(-0.3);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(1));
@@ -499,7 +510,8 @@
goal_builder.add_throttle(0.0);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(10));
@@ -526,7 +538,8 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
const auto end_time = monotonic_now() + chrono::seconds(4);
@@ -566,7 +579,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -576,7 +590,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -611,7 +626,8 @@
spline_goal_builder.add_drive_spline_backwards(true);
spline_goal_builder.add_spline(multispline_offset);
- EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -620,7 +636,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Check that we are right on the spline at the start (otherwise the feedback
@@ -669,7 +686,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
@@ -677,7 +695,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -709,14 +728,16 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -728,7 +749,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 100; ++i) {
RunFor(dt());
@@ -769,7 +791,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
@@ -777,7 +800,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -789,7 +813,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -799,7 +824,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -828,7 +854,8 @@
localizer_control_builder.add_x(drivetrain_plant_.state()(0));
localizer_control_builder.add_y(drivetrain_plant_.state()(1));
localizer_control_builder.add_theta(drivetrain_plant_.state()(2));
- ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
+ ASSERT_EQ(builder.Send(localizer_control_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = trajectory_goal_sender_.MakeBuilder();
@@ -852,7 +879,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(GetParam());
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -861,7 +889,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -898,7 +927,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -907,7 +937,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -947,7 +978,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -956,7 +988,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -1006,7 +1039,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -1015,7 +1049,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(4000));
@@ -1047,7 +1082,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -1056,7 +1092,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1086,7 +1123,8 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -1096,7 +1134,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -1128,14 +1167,16 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -1153,7 +1194,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -1180,14 +1222,16 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1222,7 +1266,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(1000));
@@ -1250,7 +1295,8 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Now execute it.
@@ -1259,7 +1305,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1292,7 +1339,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Second spline goal
@@ -1318,14 +1366,16 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1335,7 +1385,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(4000));
@@ -1367,7 +1418,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 100; ++i) {
@@ -1402,7 +1454,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -1412,7 +1465,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1434,7 +1488,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(kRunSpline);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (size_t spline_index = 0;
spline_index < DrivetrainLoop::kNumSplineFetchers + kExtraSplines;
@@ -1460,7 +1515,8 @@
spline_goal_builder.add_spline_idx(spline_index);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
// Run for at least 2 iterations. Because of how the logic works, there will
// actually typically be a single iteration where we store kNumStoredSplines
// + 1.
@@ -1504,7 +1560,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
goal_builder.add_throttle(0.5);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(5));
@@ -1541,7 +1598,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
goal_builder.add_throttle(0.5);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(5));
@@ -1564,7 +1622,8 @@
localizer_control_builder.add_x(9.0);
localizer_control_builder.add_y(7.0);
localizer_control_builder.add_theta(1.0);
- ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
+ ASSERT_EQ(builder.Send(localizer_control_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -1586,7 +1645,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(4.0);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(2));
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index fd3ff0e..f886bb2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -259,6 +259,9 @@
localizer:LocalizerState (id: 26);
zeroing:ImuZeroerState (id: 27);
+
+ // Total number of status send failures.
+ send_failures:uint64 (id: 28);
}
root_type Status;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index aabd1c6..fef0d13 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -7,6 +7,7 @@
#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "gflags/gflags.h"
+#include "glog/logging.h"
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
@@ -168,7 +169,8 @@
status_builder.add_x(state_.x());
status_builder.add_y(state_.y());
status_builder.add_theta(state_(2));
- builder.Send(status_builder.Finish());
+ CHECK_EQ(builder.Send(status_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
void DrivetrainSimulation::SendPositionMessage() {
@@ -184,7 +186,8 @@
position_builder.add_right_encoder(right_encoder);
position_builder.add_left_shifter_position(left_gear_high_ ? 1.0 : 0.0);
position_builder.add_right_shifter_position(right_gear_high_ ? 1.0 : 0.0);
- builder.Send(position_builder.Finish());
+ CHECK_EQ(builder.Send(position_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
}
@@ -263,7 +266,8 @@
frc971::IMUValuesBatch::Builder imu_values_batch_builder =
builder.MakeBuilder<frc971::IMUValuesBatch>();
imu_values_batch_builder.add_readings(imu_values_offset);
- builder.Send(imu_values_batch_builder.Finish());
+ CHECK_EQ(builder.Send(imu_values_batch_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Simulates the drivetrain moving for one timestep.
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.cc b/frc971/control_loops/drivetrain/trajectory_generator.cc
index 16b6809..d022b34 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.cc
+++ b/frc971/control_loops/drivetrain/trajectory_generator.cc
@@ -22,7 +22,8 @@
aos::Sender<fb::Trajectory>::Builder builder =
trajectory_sender_.MakeBuilder();
- CHECK(builder.Send(trajectory.Serialize(builder.fbb())));
+ CHECK_EQ(builder.Send(trajectory.Serialize(builder.fbb())),
+ aos::RawSender::Error::kOk);
}
} // namespace drivetrain
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 cb0f651..62e3d08 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
@@ -14,6 +14,7 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_generated.h"
#include "frc971/zeroing/zeroing.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -171,7 +172,8 @@
&real_position_builder);
auto position_builder = position.template MakeBuilder<PositionType>();
position_builder.add_position(position_offset);
- position.Send(position_builder.Finish());
+ CHECK_EQ(position.Send(position_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
void set_peak_subsystem_acceleration(double value) {
@@ -331,13 +333,16 @@
status->fbb());
typename StatusType::Builder subsystem_status_builder =
status->template MakeBuilder<StatusType>();
+
subsystem_status_builder.add_status(status_offset);
- status->Send(subsystem_status_builder.Finish());
+ CHECK_EQ(status->Send(subsystem_status_builder.Finish()),
+ aos::RawSender::Error::kOk);
if (output != nullptr) {
typename OutputType::Builder output_builder =
output->template MakeBuilder<OutputType>();
output_builder.add_output(output_voltage);
- output->Send(output_builder.Finish());
+ CHECK_EQ(output->Send(output_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
}
@@ -419,8 +424,9 @@
// Intake system uses 0.05 to test for 0.
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(
- zeroing::testing::CreateSubsystemGoal(*message.fbb(), 0.05)));
+ EXPECT_EQ(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), 0.05)),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(5));
@@ -437,8 +443,9 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(1);
profile_builder.add_max_acceleration(0.5);
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), 0.10, profile_builder.Finish())));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 0.10, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
// Give it a lot of time to get there.
@@ -459,8 +466,9 @@
profile_builder.add_max_velocity(std::numeric_limits<double>::quiet_NaN());
profile_builder.add_max_acceleration(
std::numeric_limits<double>::quiet_NaN());
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), 0.10, profile_builder.Finish(), 0.0, true)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 0.10, profile_builder.Finish(), 0.0, true)),
+ aos::RawSender::Error::kOk);
}
// Give it a lot of time to get there.
@@ -483,12 +491,14 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(0);
profile_builder.add_max_acceleration(0);
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(),
- kStartingGoal + aos::time::DurationInSeconds(
- this->monotonic_now().time_since_epoch()) *
- kVelocity,
- profile_builder.Finish(), kVelocity, true)));
+ EXPECT_EQ(
+ message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(),
+ kStartingGoal + aos::time::DurationInSeconds(
+ this->monotonic_now().time_since_epoch()) *
+ kVelocity,
+ profile_builder.Finish(), kVelocity, true)),
+ aos::RawSender::Error::kOk);
},
this->dt());
@@ -514,8 +524,9 @@
// Zero it before we move.
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(
- zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(*message.fbb(),
+ kRange.upper)),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
@@ -528,8 +539,9 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(20.0);
profile_builder.add_max_acceleration(0.1);
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.lower, profile_builder.Finish())));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.lower, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
this->set_peak_subsystem_velocity(23.0);
this->set_peak_subsystem_acceleration(0.2);
@@ -544,8 +556,9 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(0.1);
profile_builder.add_max_acceleration(100.0);
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.upper, profile_builder.Finish())));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
this->set_peak_subsystem_velocity(0.2);
@@ -567,8 +580,9 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(1.0);
profile_builder.add_max_acceleration(0.5);
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), 100.0, profile_builder.Finish())));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 100.0, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -584,8 +598,9 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(1.0);
profile_builder.add_max_acceleration(0.5);
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), -100.0, profile_builder.Finish())));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), -100.0, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -606,8 +621,9 @@
message.template MakeBuilder<frc971::ProfileParameters>();
profile_builder.add_max_velocity(1.0);
profile_builder.add_max_acceleration(0.5);
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.upper, profile_builder.Finish())));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper, profile_builder.Finish())),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -628,8 +644,9 @@
this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(
- zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(*message.fbb(),
+ kRange.upper)),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -643,8 +660,9 @@
this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(
- zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(*message.fbb(),
+ kRange.upper)),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -658,8 +676,9 @@
this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.upper - 0.1)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper - 0.1)),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(10));
@@ -682,8 +701,9 @@
TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.lower + 0.03)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.lower + 0.03)),
+ aos::RawSender::Error::kOk);
}
// Checks that the subsystem has not moved from its starting position at 0
@@ -700,8 +720,9 @@
TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(
- zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.lower)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(*message.fbb(),
+ kRange.lower)),
+ aos::RawSender::Error::kOk);
}
// Run disabled for 2 seconds
@@ -719,8 +740,9 @@
this->SetEnabled(true);
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.lower_hard)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.lower_hard)),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(2));
@@ -753,8 +775,9 @@
{
auto message = this->subsystem_goal_sender_.MakeBuilder();
- EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
- *message.fbb(), kRange.upper_hard)));
+ EXPECT_EQ(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper_hard)),
+ aos::RawSender::Error::kOk);
}
this->RunFor(chrono::seconds(2));