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/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index f0685dd..d63837a 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -25,6 +25,11 @@
// Gets the current target pose. Should only be called if UpdateSelection has
// returned true.
virtual TypedPose<double> TargetPose() const = 0;
+ // The "radius" of the target--for y2019, we wanted to drive in so that a disc
+ // with radius r would hit the plane of the target at an offset of exactly r
+ // from the TargetPose--this is distinct from wanting the center of the
+ // robot to project straight onto the center of the target.
+ virtual double TargetRadius() const = 0;
};
// Defines an interface for classes that provide field-global localization.
@@ -66,14 +71,17 @@
return has_target_;
}
TypedPose<double> TargetPose() const override { return pose_; }
+ double TargetRadius() const override { return target_radius_; }
void set_pose(const TypedPose<double> &pose) { pose_ = pose; }
+ void set_target_radius(double radius) { target_radius_ = radius; }
void set_has_target(bool has_target) { has_target_ = has_target; }
bool has_target() const { return has_target_; }
private:
bool has_target_ = true;
TypedPose<double> pose_;
+ double target_radius_ = 0.0;
};
// Uses the generic HybridEkf implementation to provide a basic field estimator.
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 285cb21..dfd3c69 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -143,9 +143,9 @@
stilts_params->zeroing_constants.allowable_encoder_error = 0.9;
r->camera_noise_parameters = {.max_viewable_distance = 10.0,
- .heading_noise = 0.2,
+ .heading_noise = 0.25,
.nominal_distance_noise = 0.3,
- .nominal_skew_noise = 0.35,
+ .nominal_skew_noise = 0.45,
.nominal_height_noise = 0.01};
// Deliberately make FOV a bit large so that we are overly conservative in
@@ -308,59 +308,74 @@
constexpr double kHpSlotY = InchToMeters((26 * 12 + 10.5) / 2.0 - 25.9);
constexpr double kHpSlotTheta = M_PI;
- constexpr double kNormalZ = 0.85;
- constexpr double kPortZ = 1.04;
+ constexpr double kNormalZ = 0.80;
+ constexpr double kPortZ = 1.00;
- const Pose far_side_cargo_bay({kFarSideCargoBayX, kSideCargoBayY, kNormalZ},
- kSideCargoBayTheta);
- const Pose mid_side_cargo_bay({kMidSideCargoBayX, kSideCargoBayY, kNormalZ},
- kSideCargoBayTheta);
- const Pose near_side_cargo_bay({kNearSideCargoBayX, kSideCargoBayY, kNormalZ},
- kSideCargoBayTheta);
+ constexpr double kDiscRadius = InchToMeters(19.0 / 2.0);
+ // radius to use for placing the ball (not necessarily the radius of the ball
+ // itself...).
+ constexpr double kBallRadius = 0.05;
- const Pose face_cargo_bay({kFaceCargoBayX, kFaceCargoBayY, kNormalZ},
- kFaceCargoBayTheta);
+ constexpr Target::GoalType kBothGoal = Target::GoalType::kBoth;
+ constexpr Target::GoalType kBallGoal = Target::GoalType::kBalls;
+ constexpr Target::GoalType kDiscGoal = Target::GoalType::kHatches;
+ constexpr Target::GoalType kNoneGoal = Target::GoalType::kNone;
- const Pose rocket_port({kRocketPortX, kRocketPortY, kPortZ},
- kRocketPortTheta);
+ const Target far_side_cargo_bay(
+ {{kFarSideCargoBayX, kSideCargoBayY, kNormalZ}, kSideCargoBayTheta},
+ kDiscRadius, kBothGoal);
+ const Target mid_side_cargo_bay(
+ {{kMidSideCargoBayX, kSideCargoBayY, kNormalZ}, kSideCargoBayTheta},
+ kDiscRadius, kBothGoal);
+ const Target near_side_cargo_bay(
+ {{kNearSideCargoBayX, kSideCargoBayY, kNormalZ}, kSideCargoBayTheta},
+ kDiscRadius, kBothGoal);
- const Pose rocket_near({kRocketNearX, kRocketHatchY, kNormalZ},
- kRocketNearTheta);
- const Pose rocket_far({kRocketFarX, kRocketHatchY, kNormalZ},
- kRocketFarTheta);
+ const Target face_cargo_bay(
+ {{kFaceCargoBayX, kFaceCargoBayY, kNormalZ}, kFaceCargoBayTheta},
+ kDiscRadius, kBothGoal);
- const Pose hp_slot({0.0, kHpSlotY, kNormalZ}, kHpSlotTheta);
+ const Target rocket_port(
+ {{kRocketPortX, kRocketPortY, kPortZ}, kRocketPortTheta}, kBallRadius,
+ kBallGoal);
- const ::std::array<Pose, 8> quarter_field_targets{
+ const Target rocket_near(
+ {{kRocketNearX, kRocketHatchY, kNormalZ}, kRocketNearTheta}, kDiscRadius,
+ kDiscGoal);
+ const Target rocket_far(
+ {{kRocketFarX, kRocketHatchY, kNormalZ}, kRocketFarTheta}, kDiscRadius,
+ kDiscGoal);
+
+ const Target hp_slot({{0.0, kHpSlotY, kNormalZ}, kHpSlotTheta}, 0.05,
+ kBothGoal);
+
+ const ::std::array<Target, 8> quarter_field_targets{
{far_side_cargo_bay, mid_side_cargo_bay, near_side_cargo_bay,
face_cargo_bay, rocket_port, rocket_near, rocket_far, hp_slot}};
// Mirror across center field mid-line (short field axis):
- ::std::array<Pose, 16> half_field_targets;
+ ::std::array<Target, 16> half_field_targets;
::std::copy(quarter_field_targets.begin(), quarter_field_targets.end(),
half_field_targets.begin());
for (int ii = 0; ii < 8; ++ii) {
const int jj = ii + 8;
half_field_targets[jj] = quarter_field_targets[ii];
- half_field_targets[jj].mutable_pos()->x() =
- 2.0 * kCenterFieldX - half_field_targets[jj].rel_pos().x();
- half_field_targets[jj].set_theta(
- aos::math::NormalizeAngle(M_PI - half_field_targets[jj].rel_theta()));
+ half_field_targets[jj].mutable_pose()->mutable_pos()->x() =
+ 2.0 * kCenterFieldX - half_field_targets[jj].pose().rel_pos().x();
+ half_field_targets[jj].mutable_pose()->set_theta(aos::math::NormalizeAngle(
+ M_PI - half_field_targets[jj].pose().rel_theta()));
+ // Targets on the opposite side of the field can't be driven to.
+ half_field_targets[jj].set_goal_type(kNoneGoal);
}
- ::std::array<Pose, 32> target_poses_;
-
// Mirror across x-axis (long field axis):
::std::copy(half_field_targets.begin(), half_field_targets.end(),
- target_poses_.begin());
+ targets_.begin());
for (int ii = 0; ii < 16; ++ii) {
const int jj = ii + 16;
- target_poses_[jj] = half_field_targets[ii];
- target_poses_[jj].mutable_pos()->y() *= -1;
- target_poses_[jj].set_theta(-target_poses_[jj].rel_theta());
- }
- for (int ii = 0; ii < 32; ++ii) {
- targets_[ii] = {target_poses_[ii]};
+ targets_[jj] = half_field_targets[ii];
+ targets_[jj].mutable_pose()->mutable_pos()->y() *= -1;
+ targets_[jj].mutable_pose()->set_theta(-targets_[jj].pose().rel_theta());
}
// Define rocket obstacles as just being a single line that should block any
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,