Provide 2023 target selector
This should let us do the exact same logic as y2019. This means that the
driver has to control drivetrain velocity directly (not, e.g.,
position), but is a logical first step.
This has an extremely initial tuning that should work sanely.
Does not support going backwards yet.
Change-Id: I29675be6e6d73106563f4abc6e823a1ffcb39946
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/constants/test_data/scoring_map.json b/y2023/constants/test_data/scoring_map.json
new file mode 100644
index 0000000..ebb9b38
--- /dev/null
+++ b/y2023/constants/test_data/scoring_map.json
@@ -0,0 +1,348 @@
+{
+ "red": {
+ "substation": {
+ "left": {
+ "x": -8.068,
+ "y": 1.74,
+ "z": 0.95
+ },
+ "right": {
+ "x": -8.068,
+ "y": 3.74,
+ "z": 0.95
+ }
+ },
+ "left_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": 0.973,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": 0.414,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -0.145,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": 0.973,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": 0.414,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -0.145,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": 0.973,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": 0.414,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -0.145,
+ "z": 1.17
+ }
+ }
+ },
+ "middle_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": -0.703,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": -1.262,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -1.821,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": -0.703,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": -1.262,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -1.821,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": -0.703,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": -1.262,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -1.821,
+ "z": 1.17
+ }
+ }
+ },
+ "right_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": -2.379,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": -2.938,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -3.497,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": -2.379,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": -2.938,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -3.497,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": -2.379,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": -2.938,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -3.497,
+ "z": 1.17
+ }
+ }
+ }
+ },
+ "blue": {
+ "substation": {
+ "left": {
+ "x": 8.069,
+ "y": 3.74,
+ "z": 0.95
+ },
+ "right": {
+ "x": 8.069,
+ "y": 1.74,
+ "z": 0.95
+ }
+ },
+ "left_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -0.145,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": 0.414,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": 0.973,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -0.145,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": 0.414,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": 0.973,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -0.145,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": 0.414,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": 0.973,
+ "z": 1.17
+ }
+ }
+ },
+ "middle_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -1.821,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": -1.262,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": -0.703,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -1.821,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": -1.262,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": -0.703,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -1.821,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": -1.262,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": -0.703,
+ "z": 1.17
+ }
+ }
+ },
+ "right_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -3.497,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": -2.938,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": -2.379,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -3.497,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": -2.938,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": -2.379,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -3.497,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": -2.938,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": -2.379,
+ "z": 1.17
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
index adc9eae..f09b23e 100644
--- a/y2023/constants/test_data/test_team.json
+++ b/y2023/constants/test_data/test_team.json
@@ -13,5 +13,6 @@
"calibration": {% include 'y2023/constants/test_data/calibration_pi-4.json' %}
}
],
- "target_map": {% include 'y2023/constants/test_data/target_map.json' %}
+ "target_map": {% include 'y2023/constants/test_data/target_map.json' %},
+ "scoring_map": {% include 'y2023/constants/test_data/scoring_map.json' %}
}
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 416d6d9..32b8c15 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -81,6 +81,7 @@
visibility = ["//visibility:public"],
deps = [
":drivetrain_base",
+ ":target_selector",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
@@ -131,3 +132,32 @@
gen_reflections = 1,
visibility = ["//visibility:public"],
)
+
+cc_library(
+ name = "target_selector",
+ srcs = ["target_selector.cc"],
+ hdrs = ["target_selector.h"],
+ deps = [
+ ":target_selector_hint_fbs",
+ "//aos/containers:sized_array",
+ "//aos/events:event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:localizer",
+ "//frc971/input:joystick_state_fbs",
+ "//y2023/constants:constants_fbs",
+ ],
+)
+
+cc_test(
+ name = "target_selector_test",
+ srcs = ["target_selector_test.cc"],
+ data = ["//y2023:aos_config"],
+ deps = [
+ ":target_selector",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//frc971/input:joystick_state_fbs",
+ "//y2023/constants:simulated_constants_sender",
+ ],
+)
diff --git a/y2023/control_loops/drivetrain/drivetrain_base.cc b/y2023/control_loops/drivetrain/drivetrain_base.cc
index cdb5a61..e8fdb3c 100644
--- a/y2023/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_base.cc
@@ -11,6 +11,7 @@
using ::frc971::control_loops::drivetrain::DownEstimatorConfig;
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::LineFollowConfig;
namespace chrono = ::std::chrono;
@@ -61,7 +62,20 @@
.finished(),
false /*is_simulated*/,
DownEstimatorConfig{.gravity_threshold = 0.015,
- .do_accel_corrections = 1000}};
+ .do_accel_corrections = 1000},
+ LineFollowConfig{
+ .Q = Eigen::Matrix3d((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << 1.0 / ::std::pow(0.1, 2),
+ 1.0 / ::std::pow(1.0, 2),
+ 1.0 / ::std::pow(1.0, 2))
+ .finished()
+ .asDiagonal()),
+ .R = Eigen::Matrix2d((::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 10.0 / ::std::pow(12.0, 2),
+ 10.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal()),
+ .max_controllable_offset = 0.5}};
return kDrivetrainConfig;
};
diff --git a/y2023/control_loops/drivetrain/drivetrain_main.cc b/y2023/control_loops/drivetrain/drivetrain_main.cc
index 32b2455..b13e76f 100644
--- a/y2023/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_main.cc
@@ -5,6 +5,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
+#include "y2023/control_loops/drivetrain/target_selector.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -19,7 +20,9 @@
localizer = std::make_unique<
::frc971::control_loops::drivetrain::PuppetLocalizer>(
&event_loop,
- ::y2023::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2023::control_loops::drivetrain::GetDrivetrainConfig(),
+ std::make_unique<::y2023::control_loops::drivetrain::TargetSelector>(
+ &event_loop));
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
y2023::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
localizer.get());
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
new file mode 100644
index 0000000..99805cb
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -0,0 +1,120 @@
+#include "y2023/control_loops/drivetrain/target_selector.h"
+
+#include "aos/containers/sized_array.h"
+
+namespace y2023::control_loops::drivetrain {
+TargetSelector::TargetSelector(aos::EventLoop *event_loop)
+ : joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ hint_fetcher_(event_loop->MakeFetcher<TargetSelectorHint>("/drivetrain")),
+ constants_fetcher_(event_loop) {
+ CHECK(constants_fetcher_.constants().has_scoring_map());
+ CHECK(constants_fetcher_.constants().scoring_map()->has_red());
+ CHECK(constants_fetcher_.constants().scoring_map()->has_blue());
+}
+
+void TargetSelector::UpdateAlliance() {
+ joystick_state_fetcher_.Fetch();
+ if (joystick_state_fetcher_.get() != nullptr &&
+ joystick_state_fetcher_->has_alliance()) {
+ switch (joystick_state_fetcher_->alliance()) {
+ case aos::Alliance::kRed:
+ scoring_map_ = constants_fetcher_.constants().scoring_map()->red();
+ break;
+ case aos::Alliance::kBlue:
+ scoring_map_ = constants_fetcher_.constants().scoring_map()->blue();
+ break;
+ case aos::Alliance::kInvalid:
+ // Do nothing.
+ break;
+ }
+ }
+}
+
+bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double /*command_speed*/) {
+ UpdateAlliance();
+ if (scoring_map_ == nullptr) {
+ // We don't know which alliance we are on yet; wait on a JoystickState
+ // message.
+ return false;
+ }
+ hint_fetcher_.Fetch();
+ if (hint_fetcher_.get() == nullptr) {
+ // We don't know where to go, wait on a hint.
+ return false;
+ }
+ aos::SizedArray<const localizer::ScoringGrid *, 3> possible_grids;
+ if (hint_fetcher_->has_grid()) {
+ possible_grids = {[this]() -> const localizer::ScoringGrid * {
+ switch (hint_fetcher_->grid()) {
+ case GridSelectionHint::LEFT:
+ return scoring_map_->left_grid();
+ case GridSelectionHint::MIDDLE:
+ return scoring_map_->middle_grid();
+ case GridSelectionHint::RIGHT:
+ return scoring_map_->right_grid();
+ }
+ // Make roborio compiler happy...
+ return nullptr;
+ }()};
+ } else {
+ possible_grids = {scoring_map_->left_grid(), scoring_map_->middle_grid(),
+ scoring_map_->right_grid()};
+ }
+
+ aos::SizedArray<const localizer::ScoringRow *, 3> possible_rows =
+ [this, possible_grids]() {
+ aos::SizedArray<const localizer::ScoringRow *, 3> rows;
+ for (const localizer::ScoringGrid *grid : possible_grids) {
+ CHECK_NOTNULL(grid);
+ switch (hint_fetcher_->row()) {
+ case RowSelectionHint::BOTTOM:
+ rows.push_back(grid->bottom());
+ break;
+ case RowSelectionHint::MIDDLE:
+ rows.push_back(grid->middle());
+ break;
+ case RowSelectionHint::TOP:
+ rows.push_back(grid->top());
+ break;
+ }
+ }
+ return rows;
+ }();
+ aos::SizedArray<const frc971::vision::Position *, 3> possible_positions =
+ [this, possible_rows]() {
+ aos::SizedArray<const frc971::vision::Position *, 3> positions;
+ for (const localizer::ScoringRow *row : possible_rows) {
+ CHECK_NOTNULL(row);
+ switch (hint_fetcher_->spot()) {
+ case SpotSelectionHint::LEFT:
+ positions.push_back(row->left_cone());
+ break;
+ case SpotSelectionHint::MIDDLE:
+ positions.push_back(row->cube());
+ break;
+ case SpotSelectionHint::RIGHT:
+ positions.push_back(row->right_cone());
+ break;
+ }
+ }
+ return positions;
+ }();
+ CHECK_LT(0u, possible_positions.size());
+ std::optional<double> closest_distance;
+ std::optional<Eigen::Vector3d> closest_position;
+ const Eigen::Vector3d robot_position(state.x(), state.y(), 0.0);
+ for (const frc971::vision::Position *position : possible_positions) {
+ const Eigen::Vector3d target(position->x(), position->y(), position->z());
+ double distance = (target - robot_position).norm();
+ if (!closest_distance.has_value() || distance < closest_distance.value()) {
+ closest_distance = distance;
+ closest_position = target;
+ }
+ }
+ CHECK(closest_position.has_value());
+ target_pose_ = Pose(closest_position.value(), /*theta=*/0.0);
+ return true;
+}
+} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
new file mode 100644
index 0000000..0290425
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -0,0 +1,49 @@
+#ifndef Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
+#define Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/input/joystick_state_generated.h"
+#include "y2023/constants/constants_generated.h"
+#include "y2023/control_loops/drivetrain/target_selector_hint_generated.h"
+
+namespace y2023::control_loops::drivetrain {
+// This target selector provides the logic to choose which position to try to
+// guide the robot to (primarily for game piece placement; but also for game
+// piece pickup).
+// Currently, this works by:
+// 1. Relying on the constants + JoystickState message to figure out which set
+// of targets are relevant to us given the alliance that we are on).
+// 2. If the TargetSelectorHint message fully specifies where to score the game
+// piece, go there.
+// 3. If the exact grid to score in is unpopulated, score in the closest grid.
+// In the future, the code could readily be expanded to score in the nearest
+// valid position or resolve any other set of extra ambiguity.
+class TargetSelector
+ : public frc971::control_loops::drivetrain::TargetSelectorInterface {
+ public:
+ typedef frc971::control_loops::TypedPose<double> Pose;
+
+ TargetSelector(aos::EventLoop *event_loop);
+
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) override;
+
+ Pose TargetPose() const override {
+ CHECK(target_pose_.has_value())
+ << "Did you check the return value of UpdateSelection()?";
+ return target_pose_.value();
+ }
+
+ double TargetRadius() const override { return 0.0; }
+
+ private:
+ void UpdateAlliance();
+ std::optional<Pose> target_pose_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ aos::Fetcher<TargetSelectorHint> hint_fetcher_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ const localizer::HalfField *scoring_map_ = nullptr;
+};
+} // namespace y2023::control_loops::drivetrain
+#endif // Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
diff --git a/y2023/control_loops/drivetrain/target_selector_hint.fbs b/y2023/control_loops/drivetrain/target_selector_hint.fbs
index 0a16a59..518d302 100644
--- a/y2023/control_loops/drivetrain/target_selector_hint.fbs
+++ b/y2023/control_loops/drivetrain/target_selector_hint.fbs
@@ -28,6 +28,8 @@
grid:GridSelectionHint (id: 0);
row:RowSelectionHint (id: 1);
spot:SpotSelectionHint (id: 2);
+ // TODO: support human player pickup auto-align?
+ // TODO: add flag for forwards vs. backwards.
}
root_type TargetSelectorHint;
diff --git a/y2023/control_loops/drivetrain/target_selector_test.cc b/y2023/control_loops/drivetrain/target_selector_test.cc
new file mode 100644
index 0000000..fdbd904
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector_test.cc
@@ -0,0 +1,137 @@
+#include "y2023/control_loops/drivetrain/target_selector.h"
+
+#include "frc971/input/joystick_state_generated.h"
+#include "gtest/gtest.h"
+#include "y2023/constants/simulated_constants_sender.h"
+
+namespace y2023::control_loops::drivetrain {
+class TargetSelectorTest : public ::testing::Test {
+ protected:
+ TargetSelectorTest()
+ : configuration_(aos::configuration::ReadConfig("y2023/aos_config.json")),
+ event_loop_factory_(&configuration_.message()),
+ roborio_node_([this]() {
+ // Get the constants sent before anything else happens.
+ // It has nothing to do with the roborio node.
+ SendSimulationConstants(&event_loop_factory_, 7971,
+ "y2023/constants/test_constants.json");
+ return aos::configuration::GetNode(&configuration_.message(),
+ "roborio");
+ }()),
+ selector_event_loop_(
+ event_loop_factory_.MakeEventLoop("drivetrain", roborio_node_)),
+ target_selector_(selector_event_loop_.get()),
+ test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", roborio_node_)),
+ constants_fetcher_(test_event_loop_.get()),
+ joystick_state_sender_(
+ test_event_loop_->MakeSender<aos::JoystickState>("/aos")),
+ hint_sender_(
+ test_event_loop_->MakeSender<TargetSelectorHint>("/drivetrain")) {}
+
+ void SendJoystickState() {
+ auto builder = joystick_state_sender_.MakeBuilder();
+ aos::JoystickState::Builder state_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+ state_builder.add_alliance(aos::Alliance::kRed);
+ builder.CheckOk(builder.Send(state_builder.Finish()));
+ }
+
+ void SendHint(GridSelectionHint grid, RowSelectionHint row,
+ SpotSelectionHint spot) {
+ auto builder = hint_sender_.MakeBuilder();
+ builder.CheckOk(builder.Send(
+ CreateTargetSelectorHint(*builder.fbb(), grid, row, spot)));
+ }
+ void SendHint(RowSelectionHint row, SpotSelectionHint spot) {
+ auto builder = hint_sender_.MakeBuilder();
+ TargetSelectorHint::Builder hint_builder =
+ builder.MakeBuilder<TargetSelectorHint>();
+ hint_builder.add_row(row);
+ hint_builder.add_spot(spot);
+ builder.CheckOk(builder.Send(hint_builder.Finish()));
+ }
+
+ const localizer::HalfField *scoring_map() const {
+ return constants_fetcher_.constants().scoring_map()->red();
+ }
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ const aos::Node *const roborio_node_;
+ std::unique_ptr<aos::EventLoop> selector_event_loop_;
+ TargetSelector target_selector_;
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ aos::Sender<aos::JoystickState> joystick_state_sender_;
+ aos::Sender<TargetSelectorHint> hint_sender_;
+};
+
+// Tests that no target is available if no input messages have been sent.
+TEST_F(TargetSelectorTest, NoTargetWithoutInputs) {
+ EXPECT_FALSE(target_selector_.UpdateSelection(
+ Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
+ EXPECT_DEATH(target_selector_.TargetPose(), "Did you check the return value");
+ EXPECT_EQ(0.0, target_selector_.TargetRadius());
+}
+
+// Tests that if we fully specify which target to go to that we always will do
+// so.
+TEST_F(TargetSelectorTest, FullySpecifiedTarget) {
+ SendJoystickState();
+ // Iterate over every available target.
+ for (const auto &[grid, grid_hint] : std::vector<
+ std::pair<const localizer::ScoringGrid *, GridSelectionHint>>{
+ {scoring_map()->left_grid(), GridSelectionHint::LEFT},
+ {scoring_map()->middle_grid(), GridSelectionHint::MIDDLE},
+ {scoring_map()->right_grid(), GridSelectionHint::RIGHT}}) {
+ for (const auto &[row, row_hint] : std::vector<
+ std::pair<const localizer::ScoringRow *, RowSelectionHint>>{
+ {grid->bottom(), RowSelectionHint::BOTTOM},
+ {grid->middle(), RowSelectionHint::MIDDLE},
+ {grid->top(), RowSelectionHint::TOP}}) {
+ for (const auto &[spot, spot_hint] : std::vector<
+ std::pair<const frc971::vision::Position *, SpotSelectionHint>>{
+ {row->left_cone(), SpotSelectionHint::LEFT},
+ {row->cube(), SpotSelectionHint::MIDDLE},
+ {row->right_cone(), SpotSelectionHint::RIGHT}}) {
+ SendHint(grid_hint, row_hint, spot_hint);
+ EXPECT_TRUE(target_selector_.UpdateSelection(
+ Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
+ EXPECT_EQ(0.0, target_selector_.TargetRadius());
+ EXPECT_EQ(spot->x(), target_selector_.TargetPose().abs_pos().x());
+ EXPECT_EQ(spot->y(), target_selector_.TargetPose().abs_pos().y());
+ EXPECT_EQ(spot->z(), target_selector_.TargetPose().abs_pos().z());
+ }
+ }
+ }
+}
+
+// Tests that if we leave the grid setting ambiguous that we select the
+// nearest possible target given the other settings.
+TEST_F(TargetSelectorTest, NoGridSpecified) {
+ SendJoystickState();
+ // We will leave the robot at (0, 0). This means that if we are going for the
+ // left cone we should go for the middle grid, and if we are going for the
+ // cube (middle) or right cone positions we should prefer the left grid.
+ // Note that the grids are not centered on the field (hence the middle isn't
+ // always preferred when at (0, 0)).
+
+ for (const auto &[spot, spot_hint] : std::vector<
+ std::pair<const frc971::vision::Position *, SpotSelectionHint>>{
+ {scoring_map()->middle_grid()->bottom()->left_cone(),
+ SpotSelectionHint::LEFT},
+ {scoring_map()->left_grid()->bottom()->cube(),
+ SpotSelectionHint::MIDDLE},
+ {scoring_map()->left_grid()->bottom()->right_cone(),
+ SpotSelectionHint::RIGHT}}) {
+ SendHint(RowSelectionHint::BOTTOM, spot_hint);
+ EXPECT_TRUE(target_selector_.UpdateSelection(
+ Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
+ EXPECT_EQ(spot->x(), target_selector_.TargetPose().abs_pos().x());
+ EXPECT_EQ(spot->y(), target_selector_.TargetPose().abs_pos().y());
+ EXPECT_EQ(spot->z(), target_selector_.TargetPose().abs_pos().z());
+ }
+}
+
+} // namespace y2023::control_loops::drivetrain