Add types to targets and ignore rocket portals
This also adds a "radius" to individual targets to allow us to
conceptualize the edge of the velcro for the hatch targets.
Change-Id: Ic6081c3eca784328f80fe83ab799efe90c0d50b0
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index d59ac21..73079c9 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -34,12 +34,32 @@
class TypedTarget {
public:
typedef ::frc971::control_loops::TypedPose<Scalar> Pose;
- TypedTarget(const Pose &pose) : pose_(pose) {}
+ // The nature of the target as a goal--to mark what modes it is a valid
+ // potential goal pose and to mark targets on the opposite side of the field
+ // as not being viable targets.
+ enum class GoalType {
+ // None marks targets that are on the opposite side of the field and not
+ // viable goal poses.
+ kNone,
+ // Spots where we can touch hatch panels.
+ kHatches,
+ // Spots where we can mess with balls.
+ kBalls,
+ // Spots for both (cargo ship, human loading).
+ kBoth,
+ };
+ TypedTarget(const Pose &pose, double radius = 0,
+ GoalType goal_type = GoalType::kBoth)
+ : pose_(pose), radius_(radius), goal_type_(goal_type) {}
TypedTarget() {}
Pose pose() const { return pose_; }
+ Pose *mutable_pose() { return &pose_; }
bool occluded() const { return occluded_; }
void set_occluded(bool occluded) { occluded_ = occluded; }
+ double radius() const { return radius_; }
+ GoalType goal_type() const { return goal_type_; }
+ void set_goal_type(GoalType goal_type) { goal_type_ = goal_type; }
// Get a list of points for plotting. These points should be plotted on
// an x/y plane in the global frame with lines connecting the points.
@@ -63,6 +83,13 @@
private:
Pose pose_;
bool occluded_ = false;
+ // The effective radius of the target--for placing discs, this should be the
+ // radius of the disc; for fetching discs and working with balls this should
+ // be near zero.
+ // TODO(james): We may actually want a non-zero (possibly negative?) number
+ // here for balls.
+ double radius_ = 0.0;
+ GoalType goal_type_ = GoalType::kBoth;
}; // class TypedTarget
typedef TypedTarget<double> Target;
diff --git a/y2019/control_loops/drivetrain/camera_test.cc b/y2019/control_loops/drivetrain/camera_test.cc
index d0a4b85..347e84c 100644
--- a/y2019/control_loops/drivetrain/camera_test.cc
+++ b/y2019/control_loops/drivetrain/camera_test.cc
@@ -8,12 +8,14 @@
// Check that a Target's basic operations work.
TEST(TargetTest, BasicTargetTest) {
- Target target({{1, 2, 3}, M_PI / 2.0});
+ Target target({{1, 2, 3}, M_PI / 2.0}, 1.234, Target::GoalType::kHatches);
EXPECT_EQ(1.0, target.pose().abs_pos().x());
EXPECT_EQ(2.0, target.pose().abs_pos().y());
EXPECT_EQ(3.0, target.pose().abs_pos().z());
EXPECT_EQ(M_PI / 2.0, target.pose().abs_theta());
+ EXPECT_EQ(1.234, target.radius());
+ EXPECT_EQ(Target::GoalType::kHatches, target.goal_type());
EXPECT_FALSE(target.occluded());
target.set_occluded(true);
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index e086e36..f0732a3 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -296,7 +296,7 @@
.Send();
RunForTime(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate(1e-4);
+ VerifyEstimatorAccurate(5e-4);
}
namespace {
@@ -307,12 +307,12 @@
// 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});
+ SetStartingPosition({4, HPSlotLeft().abs_pos().y(), M_PI});
my_drivetrain_queue_.goal.MakeWithBuilder()
.controller_type(3)
.throttle(0.9)
.Send();
- RunForTime(chrono::seconds(10));
+ RunForTime(chrono::seconds(6));
VerifyEstimatorAccurate(1e-8);
// Due to the fact that we aren't modulating the throttle, we don't try to hit
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
index 449ce64..b7b7448 100644
--- a/y2019/control_loops/drivetrain/target_selector.cc
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -36,8 +36,16 @@
// means the largest target in the camera view).
double largest_target_noise = ::std::numeric_limits<double>::infinity();
for (const auto &view : target_views) {
+ // Skip targets that aren't viable for going to (e.g., on the opposite side
+ // of the field).
+ // TODO(james): Support ball vs. hatch mode filtering.
+ if (view.target->goal_type() == Target::GoalType::kNone ||
+ view.target->goal_type() == Target::GoalType::kBalls) {
+ continue;
+ }
if (view.noise.distance < largest_target_noise) {
target_pose_ = view.target->pose();
+ target_radius_ = view.target->radius();
largest_target_noise = view.noise.distance;
}
}
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 965d7cc..7d09306 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -33,6 +33,8 @@
double command_speed) override;
Pose TargetPose() const override { return target_pose_; }
+ double TargetRadius() const override { return target_radius_; }
+
private:
static constexpr double kFakeFov = M_PI * 0.7;
// Longitudinal speed at which the robot must be going in order for us to make
@@ -40,6 +42,7 @@
static constexpr double kMinDecisionSpeed = 0.7; // m/s
Pose robot_pose_;
Pose target_pose_;
+ double target_radius_;
// 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
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index 4b440c2..20bf19a 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -13,7 +13,7 @@
// 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(); }
+Pose RocketHatchFarLeft() { return constants::Field().targets()[6].pose(); }
} // namespace
// Tests the target selector with:
@@ -72,7 +72,7 @@
TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(), 1.0, 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.
+ // hatches driving one direction and the near cargo ship port the other.
// We also command a speed opposite the current direction of motion and
// confirm that that behaves as expected.
TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(), 1.0,
@@ -80,9 +80,9 @@
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(), -1.0,
true, CargoNearLeft()},
TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(), -1.0,
- true, RocketPortalLeft()},
+ true, RocketHatchFarLeft()},
TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(), 1.0,
- true, RocketPortalLeft()},
+ true, RocketHatchFarLeft()},
// And we shouldn't see anything spinning in place:
TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(),
0.0,