Add boilerplate localizer code for LED control
Will use this to turn LEDs facing away from the target off
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I4e03778f6b06db87b26ab3986dfebab7840d6de1
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 8f9ffa1..1aa2458 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -102,6 +102,8 @@
DiscretizeQAFast(Q_continuous_accel_, A_continuous_accel_, kNominalDt,
&Q_discrete_accel_, &A_discrete_accel_);
P_accel_ = Q_discrete_accel_;
+
+ led_outputs_.fill(LedOutput::ON);
}
Eigen::Matrix<double, ModelBasedLocalizer::kNModelStates,
@@ -487,7 +489,8 @@
}
// Node names of the pis to listen for cameras from.
-const std::array<std::string_view, 4> kPisToUse{"pi1", "pi2", "pi3", "pi4"};
+constexpr std::array<std::string_view, ModelBasedLocalizer::kNumPis> kPisToUse{
+ "pi1", "pi2", "pi3", "pi4"};
} // namespace
const Eigen::Matrix<double, 4, 4> ModelBasedLocalizer::CameraTransform(
@@ -667,6 +670,9 @@
const double camera_yaw =
std::atan2(camera_z_in_field.y(), camera_z_in_field.x());
+ // TODO(milind): actually control this
+ led_outputs_[camera_index] = LedOutput::ON;
+
TargetEstimateDebugT debug;
debug.camera = static_cast<uint8_t>(camera_index);
debug.camera_x = H_field_camera_measured(0, 3);
@@ -1010,6 +1016,11 @@
if (last_output_send_ + std::chrono::milliseconds(5) <
event_loop_->context().monotonic_event_time) {
auto builder = output_sender_.MakeBuilder();
+
+ const auto led_outputs_offset =
+ builder.fbb()->CreateVector(model_based_.led_outputs().data(),
+ model_based_.led_outputs().size());
+
LocalizerOutput::Builder output_builder =
builder.MakeBuilder<LocalizerOutput>();
// TODO(james): Should we bother to try to estimate time offsets for
@@ -1027,6 +1038,7 @@
quaternion.mutate_z(orientation.z());
quaternion.mutate_w(orientation.w());
output_builder.add_orientation(&quaternion);
+ output_builder.add_led_outputs(led_outputs_offset);
builder.CheckOk(builder.Send(output_builder.Finish()));
last_output_send_ = event_loop_->monotonic_now();
}
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index c19bf05..e9a61e1 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -63,6 +63,8 @@
// * Tune for ADIS16505/real robot.
class ModelBasedLocalizer {
public:
+ static constexpr size_t kNumPis = 4;
+
static constexpr size_t kX = 0;
static constexpr size_t kY = 1;
static constexpr size_t kTheta = 2;
@@ -141,6 +143,8 @@
size_t NumQueuedImageDebugs() const { return image_debugs_.size(); }
+ std::array<LedOutput, kNumPis> led_outputs() const { return led_outputs_; }
+
private:
struct CombinedState {
AccelState accel_state = AccelState::Zero();
@@ -298,6 +302,7 @@
Statistics statistics_;
aos::SizedArray<TargetEstimateDebugT, kDebugBufferSize> image_debugs_;
+ std::array<LedOutput, kNumPis> led_outputs_;
friend class testing::LocalizerTest;
};
@@ -322,7 +327,8 @@
aos::Sender<LocalizerVisualization> visualization_sender_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
- std::array<aos::Fetcher<y2022::vision::TargetEstimate>, 4>
+ std::array<aos::Fetcher<y2022::vision::TargetEstimate>,
+ ModelBasedLocalizer::kNumPis>
target_estimate_fetchers_;
aos::Fetcher<y2022::control_loops::superstructure::Status>
superstructure_fetcher_;
diff --git a/y2022/localizer/localizer_output.fbs b/y2022/localizer/localizer_output.fbs
index b07278b..ec3302a 100644
--- a/y2022/localizer/localizer_output.fbs
+++ b/y2022/localizer/localizer_output.fbs
@@ -10,6 +10,12 @@
z:double (id: 3);
}
+// Used to tell different LEDs to be on or off
+enum LedOutput : byte {
+ ON,
+ OFF
+}
+
table LocalizerOutput {
// Timestamp (on the source node) that this sample corresponds with. This
// may be older than the sent time to account for delays in sensor readings.
@@ -24,6 +30,10 @@
// Whether we have zeroed the IMU (may go false if we observe a fault with the
// IMU).
zeroed:bool (id: 5);
+
+ // Whether each led should be on.
+ // Indices correspond to pi number.
+ led_outputs:[LedOutput] (id: 6);
}
root_type LocalizerOutput;
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
index bf75446..6e9cd1e 100644
--- a/y2022/localizer/localizer_test.cc
+++ b/y2022/localizer/localizer_test.cc
@@ -2,12 +2,12 @@
#include "aos/events/logging/log_writer.h"
#include "aos/events/simulated_event_loop.h"
-#include "gtest/gtest.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/pose.h"
-#include "y2022/vision/target_estimate_generated.h"
-#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+#include "gtest/gtest.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2022/vision/target_estimate_generated.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
@@ -18,11 +18,11 @@
typedef ModelBasedLocalizer::ModelInput ModelInput;
typedef ModelBasedLocalizer::AccelInput AccelInput;
-using frc971::vision::calibration::CameraCalibrationT;
-using frc971::vision::calibration::TransformationMatrixT;
+using frc971::control_loops::Pose;
using frc971::control_loops::drivetrain::DrivetrainConfig;
using frc971::control_loops::drivetrain::LocalizerControl;
-using frc971::control_loops::Pose;
+using frc971::vision::calibration::CameraCalibrationT;
+using frc971::vision::calibration::TransformationMatrixT;
using y2022::vision::TargetEstimate;
using y2022::vision::TargetEstimateT;
@@ -82,14 +82,12 @@
config.is_simulated = true;
return config;
}
-}
+} // namespace
class LocalizerTest : public ::testing::Test {
protected:
LocalizerTest()
- : dt_config_(
- GetTest2022DrivetrainConfig()),
- localizer_(dt_config_) {
+ : dt_config_(GetTest2022DrivetrainConfig()), localizer_(dt_config_) {
localizer_.set_longitudinal_offset(0.0);
}
ModelState CallDiffModel(const ModelState &state, const ModelInput &U) {
@@ -102,7 +100,6 @@
const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
ModelBasedLocalizer localizer_;
-
};
TEST_F(LocalizerTest, AccelIntegrationTest) {
@@ -233,7 +230,8 @@
const Eigen::Vector2d encoders{0.0, 0.0};
const Eigen::Vector2d voltages{0.0, 0.0};
Eigen::Vector3d accel{5.0, 2.0, 9.80665};
- Eigen::Vector3d accel_gs = dt_config_.imu_transform.inverse() * accel / 9.80665;
+ Eigen::Vector3d accel_gs =
+ dt_config_.imu_transform.inverse() * accel / 9.80665;
while (t < start) {
// Spin to fill up the buffer.
localizer_.HandleImu(t, gyro, Eigen::Vector3d::UnitZ(), encoders, voltages);
@@ -744,10 +742,10 @@
ASSERT_FALSE(status_fetcher_->model_based()->using_model());
ASSERT_LT(0.1, status_fetcher_->model_based()->residual())
<< aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
- ASSERT_NEAR(drivetrain_plant_.state()(0),
- status_fetcher_->model_based()->x(), 1.0);
- ASSERT_NEAR(drivetrain_plant_.state()(1),
- status_fetcher_->model_based()->y(), 1e-6);
+ ASSERT_NEAR(drivetrain_plant_.state()(0), status_fetcher_->model_based()->x(),
+ 1.0);
+ ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(),
+ 1e-6);
}
// Tests that image corrections in the nominal case (no errors) causes no
@@ -864,7 +862,8 @@
EXPECT_TRUE(VerifyEstimatorAccurate(3.0));
// y should be noticeably more accurate than x, since we are just driving
// straight.
- ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(), 0.1);
+ ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(),
+ 0.1);
ASSERT_TRUE(status_fetcher_->model_based()->has_statistics());
ASSERT_LT(10,
status_fetcher_->model_based()->statistics()->total_candidates());
@@ -872,4 +871,16 @@
status_fetcher_->model_based()->statistics()->total_accepted());
}
-} // namespace frc91::controls::testing
+TEST_F(EventLoopLocalizerTest, LedOutputs) {
+ send_targets_ = true;
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(10));
+ CHECK(output_fetcher_.Fetch());
+ EXPECT_EQ(output_fetcher_->led_outputs()->size(),
+ ModelBasedLocalizer::kNumPis);
+ for (LedOutput led_output : *output_fetcher_->led_outputs()) {
+ EXPECT_EQ(led_output, LedOutput::ON);
+ }
+}
+
+} // namespace frc971::controls::testing