Make 2019 target selector
Selects targets based on which direction the robot is moving and which
target is largest in the view of the robot, using some virtual cameras
that have 180 degree fields of view to identify targets (the 180 is a
tunable number that represents how wide we want the net we cast for
targets to be--it could be smaller if we want the driver to line
themselves up more, or bigger if we wanted to be able to turn more than
90 degrees to reach a target).
Also, lowered the degree of line following polynomial because the 4th
degree polynomial was being somewhat unstable for the 2019 drivetrain
config.
Change-Id: Idea8cd7c91c706362843a3aacc9c4aafe1490442
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index de52215..5a4dd40 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -121,12 +121,34 @@
)
cc_library(
+ name = "target_selector",
+ srcs = ["target_selector.cc"],
+ hdrs = ["target_selector.h"],
+ deps = [
+ ":camera",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:localizer",
+ "//y2019:constants",
+ ],
+)
+
+cc_test(
+ name = "target_selector_test",
+ srcs = ["target_selector_test.cc"],
+ deps = [
+ ":target_selector",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "event_loop_localizer",
srcs = ["event_loop_localizer.cc"],
hdrs = ["event_loop_localizer.h"],
deps = [
":camera_queue",
":localizer",
+ ":target_selector",
"//frc971/control_loops/drivetrain:localizer",
"//y2019:constants",
],
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 981701a..4656cb3 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -35,7 +35,6 @@
Localizer::State::Zero(), localizer_.P());
frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
".y2019.control_loops.drivetrain.camera_frames");
- target_selector_.set_has_target(false);
}
void EventLoopLocalizer::Reset(const Localizer::State &state) {
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 773410f..3c2cf4e 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -3,8 +3,9 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/localizer.h"
#include "y2019/control_loops/drivetrain/camera.q.h"
+#include "y2019/control_loops/drivetrain/localizer.h"
+#include "y2019/control_loops/drivetrain/target_selector.h"
namespace y2019 {
namespace control_loops {
@@ -61,8 +62,7 @@
return localizer_.X_hat(StateIdx::kRightVoltageError);
}
- ::frc971::control_loops::drivetrain::TrivialTargetSelector *target_selector()
- override {
+ TargetSelector *target_selector() override {
return &target_selector_;
}
@@ -79,7 +79,7 @@
Pose robot_pose_;
const ::std::array<Camera, constants::Values::kNumCameras> cameras_;
Localizer localizer_;
- ::frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
+ TargetSelector target_selector_;
};
// Constructs the cameras based on the constants in the //y2019/constants.*
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 3dde98d..0ebfef1 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -275,6 +275,32 @@
VerifyEstimatorAccurate(1e-5);
}
+namespace {
+EventLoopLocalizer::Pose HPSlotLeft() { return constants::Field().targets()[7].pose(); }
+} // namespace
+
+// Tests that using the line following drivetrain and just driving straight
+// forward from roughly the right spot gets us to the HP slot.
+TEST_F(LocalizedDrivetrainTest, LineFollowToHPSlot) {
+ set_enable_cameras(false);
+ SetStartingPosition({4, 3, M_PI});
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .controller_type(3)
+ .throttle(0.5)
+ .Send();
+ RunForTime(chrono::seconds(10));
+
+ VerifyEstimatorAccurate(1e-10);
+ // Due to the fact that we aren't modulating the throttle, we don't try to hit
+ // the target exactly. Instead, just run slightly past the target:
+ EXPECT_LT(::std::abs(::aos::math::DiffAngle(
+ M_PI, drivetrain_motor_plant_.state()(2, 0))),
+ 1e-5);
+ EXPECT_GT(HPSlotLeft().abs_pos().x(), drivetrain_motor_plant_.state().x());
+ EXPECT_NEAR(HPSlotLeft().abs_pos().y(),
+ drivetrain_motor_plant_.state().y(), 1e-4);
+}
+
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
new file mode 100644
index 0000000..2b70ab7
--- /dev/null
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -0,0 +1,48 @@
+#include "y2019/control_loops/drivetrain/target_selector.h"
+
+namespace y2019 {
+namespace control_loops {
+
+constexpr double TargetSelector::kFakeFov;
+
+TargetSelector::TargetSelector()
+ : front_viewer_({&robot_pose_, {0.0, 0.0, 0.0}, 0.0}, kFakeFov, fake_noise_,
+ constants::Field().targets(), {}),
+ back_viewer_({&robot_pose_, {0.0, 0.0, 0.0}, M_PI}, kFakeFov, fake_noise_,
+ constants::Field().targets(), {}) {}
+
+bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) {
+ const double speed = (state(3, 0) + state(4, 0)) / 2.0;
+ if (::std::abs(speed) < kMinDecisionSpeed) {
+ return false;
+ }
+ *robot_pose_.mutable_pos() << state.x(), state.y(), 0.0;
+ robot_pose_.set_theta(state(2, 0));
+ ::aos::SizedArray<FakeCamera::TargetView,
+ y2019::constants::Field::kNumTargets>
+ target_views;
+ if (speed > 0) {
+ target_views = front_viewer_.target_views();
+ } else {
+ target_views = back_viewer_.target_views();
+ }
+
+ if (target_views.empty()) {
+ // We can't see any targets...
+ return false;
+ }
+
+ // Choose the target that has the smallest distance noise (currently, this
+ // means the largest target in the camera view).
+ double largest_target_noise = ::std::numeric_limits<double>::infinity();
+ for (const auto &view : target_views) {
+ if (view.noise.distance < largest_target_noise) {
+ target_pose_ = view.target->pose();
+ largest_target_noise = view.noise.distance;
+ }
+ }
+ return true;
+}
+
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
new file mode 100644
index 0000000..d3f1a9d
--- /dev/null
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -0,0 +1,58 @@
+#ifndef Y2019_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
+#define Y2019_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
+
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/drivetrain/camera.h"
+
+namespace y2019 {
+namespace control_loops {
+
+// A class to identify which target the driver is currently driving towards so
+// that we can guide them into the target.
+// The current high-level algorithm is to:
+// (a) Determine which direction (forwards vs. backwardS) the driver is driving.
+// (b) Find the largest target in the X degree field-of-view to the front/back
+// of the robot, where X depends on how much of an angle we expect the driver to
+// typically want to drive in at.
+// (c) Assume that said largest target is the target that the driver wants to
+// drive to.
+class TargetSelector
+ : public ::frc971::control_loops::drivetrain::TargetSelectorInterface {
+ public:
+ typedef ::frc971::control_loops::TypedPose<double> Pose;
+ // For the virtual camera that we use to identify targets, ignore all
+ // obstacles and just assume that we have perfect field of view.
+ typedef TypedCamera<y2019::constants::Field::kNumTargets,
+ /*num_obstacles=*/0, double> FakeCamera;
+
+ TargetSelector();
+
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) override;
+ Pose TargetPose() const override { return target_pose_; }
+
+ private:
+ static constexpr double kFakeFov = M_PI;
+ // Longitudinal speed at which the robot must be going in order for us to make
+ // a decision.
+ static constexpr double kMinDecisionSpeed = 0.1; // m/s
+ Pose robot_pose_;
+ Pose target_pose_;
+ // For the noise of our fake cameras, we only really care about the max
+ // distance, which will be the maximum distance we let ourselves guide in
+ // from. The distance noise is set so that we can use the camera's estimate of
+ // the relative size of the targets.
+ FakeCamera::NoiseParameters fake_noise_ = {.max_viewable_distance = 5 /*m*/,
+ .heading_noise = 0,
+ .nominal_distance_noise = 1,
+ .nominal_skew_noise = 0,
+ .nominal_height_noise = 0};
+ FakeCamera front_viewer_;
+ FakeCamera back_viewer_;
+};
+
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
new file mode 100644
index 0000000..7c0ad7f
--- /dev/null
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -0,0 +1,85 @@
+#include "y2019/control_loops/drivetrain/target_selector.h"
+
+#include "gtest/gtest.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace testing {
+
+typedef ::frc971::control_loops::TypedPose<double> Pose;
+typedef ::Eigen::Matrix<double, 5, 1> State;
+
+namespace {
+// Accessors to get some useful particular targets on the field:
+Pose HPSlotLeft() { return constants::Field().targets()[7].pose(); }
+Pose CargoNearLeft() { return constants::Field().targets()[2].pose(); }
+Pose RocketPortalLeft() { return constants::Field().targets()[4].pose(); }
+} // namespace
+
+// Tests the target selector with:
+// -The [x, y, theta, left_vel, right_vel] state to test at
+// -Whether we expect to see a target.
+// -If (1) is true, the pose we expect to get back.
+struct TestParams {
+ State state;
+ bool expect_target;
+ Pose expected_pose;
+};
+class TargetSelectorParamTest : public ::testing::TestWithParam<TestParams> {};
+
+TEST_P(TargetSelectorParamTest, ExpectReturn) {
+ TargetSelector selector;
+ bool expect_target = GetParam().expect_target;
+ const State state = GetParam().state;
+ ASSERT_EQ(expect_target, selector.UpdateSelection(state))
+ << "We expected a return of " << expect_target << " at state "
+ << state.transpose();
+ if (expect_target) {
+ const Pose expected_pose = GetParam().expected_pose;
+ const Pose actual_pose = selector.TargetPose();
+ const ::Eigen::Vector3d expected_pos = expected_pose.abs_pos();
+ const ::Eigen::Vector3d actual_pos = actual_pose.abs_pos();
+ const double expected_angle = expected_pose.abs_theta();
+ const double actual_angle = actual_pose.abs_theta();
+ EXPECT_EQ(expected_pos, actual_pos)
+ << "Expected the pose to be at " << expected_pos.transpose()
+ << " but got " << actual_pos.transpose() << " with the robot at "
+ << state.transpose();
+ EXPECT_EQ(expected_angle, actual_angle);
+ }
+}
+
+INSTANTIATE_TEST_CASE_P(
+ TargetSelectorTest, TargetSelectorParamTest,
+ ::testing::Values(
+ // When we are far away from anything, we should not register any
+ // targets:
+ TestParams{(State() << 0.0, 0.0, 0.0, 1.0, 1.0).finished(), false, {}},
+ // Aim for a human-player spot; at low speeds we should not register
+ // anything.
+ TestParams{
+ (State() << 4.0, 2.0, M_PI, 0.05, 0.05).finished(), false, {}},
+ TestParams{
+ (State() << 4.0, 2.0, M_PI, -0.05, -0.05).finished(), false, {}},
+ TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(), true,
+ HPSlotLeft()},
+ // Put ourselves between the rocket and cargo ship; we should see the
+ // portal driving one direction and the near cargo ship port the other.
+ TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(), true,
+ CargoNearLeft()},
+ TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(), true,
+ CargoNearLeft()},
+ TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(), true,
+ RocketPortalLeft()},
+ TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(), true,
+ RocketPortalLeft()},
+ // And we shouldn't see anything spinning in place:
+ TestParams{
+ (State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(), false, {}},
+ // Drive backwards off the field--we should not see anything.
+ TestParams{
+ (State() << -0.1, 0.0, 0.0, -0.5, -0.5).finished(), false, {}}));
+
+} // namespace testing
+} // namespace control_loops
+} // namespace y2019