Selectively ignore opponent's human player station april tag
We don't have the april tags set up for our auto testing, so don't rely
on that tag.
Change-Id: I93cf62ace8e478f16e364797c0b5f4495188b548
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index ff027d0..d9e239b 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -30,6 +30,14 @@
: true;
}
+aos::Alliance LocalizationUtils::Alliance() {
+ joystick_state_fetcher_.Fetch();
+ return (joystick_state_fetcher_.get() != nullptr)
+ ? joystick_state_fetcher_->alliance()
+ : aos::Alliance::kInvalid;
+
+}
+
std::optional<aos::monotonic_clock::duration> LocalizationUtils::ClockOffset(
std::string_view node) {
std::optional<aos::monotonic_clock::duration> monotonic_offset;
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 26242f9..cfd443a 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -29,6 +29,7 @@
// Returns true if either there is no JoystickState message available or if
// we are currently in autonomous mode.
bool MaybeInAutonomous();
+ aos::Alliance Alliance();
// Returns the offset between our node and the specified node (or nullopt if
// no offset is available). The sign of this will be such that the time on
diff --git a/y2023/constants/common.json b/y2023/constants/common.json
index c559810..d63c518 100644
--- a/y2023/constants/common.json
+++ b/y2023/constants/common.json
@@ -1,2 +1,6 @@
"target_map": {% include 'y2023/vision/maps/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %},
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
+ }
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
index bd038b8..61c3365 100644
--- a/y2023/constants/constants.fbs
+++ b/y2023/constants/constants.fbs
@@ -29,11 +29,19 @@
tof:TimeOfFlight (id: 0);
}
+// Set of april tag targets, by april tag ID, to ignore when on a
+// given alliance.
+table IgnoreTargets {
+ red:[uint64] (id: 0);
+ blue:[uint64] (id: 1);
+}
+
table Constants {
cameras:[CameraConfiguration] (id: 0);
target_map:frc971.vision.TargetMap (id: 1);
scoring_map:localizer.ScoringMap (id: 2);
robot:RobotConstants (id: 3);
+ ignore_targets:IgnoreTargets (id: 4);
}
root_type Constants;
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
index a1e77af..0a226f0 100644
--- a/y2023/constants/test_data/test_team.json
+++ b/y2023/constants/test_data/test_team.json
@@ -28,5 +28,9 @@
}
]
}
+ },
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
}
}
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index efffca3..c663954 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -248,6 +248,29 @@
return builder->Finish();
}
+bool Localizer::UseAprilTag(uint64_t target_id) {
+ if (target_poses_.count(target_id) == 0) {
+ return false;
+ }
+
+ const flatbuffers::Vector<uint64_t> *ignore_tags = nullptr;
+
+ switch (utils_.Alliance()) {
+ case aos::Alliance::kRed:
+ ignore_tags =
+ CHECK_NOTNULL(constants_fetcher_.constants().ignore_targets()->red());
+ break;
+ case aos::Alliance::kBlue:
+ ignore_tags = CHECK_NOTNULL(
+ constants_fetcher_.constants().ignore_targets()->blue());
+ break;
+ case aos::Alliance::kInvalid:
+ return true;
+ }
+ return std::find(ignore_tags->begin(), ignore_tags->end(), target_id) ==
+ ignore_tags->end();
+}
+
flatbuffers::Offset<TargetEstimateDebug> Localizer::HandleTarget(
int camera_index, const aos::monotonic_clock::time_point capture_time,
const frc971::vision::TargetPoseFbs &target,
@@ -267,7 +290,7 @@
const uint64_t target_id = target.id();
builder.add_april_tag(target_id);
VLOG(2) << aos::FlatbufferToJson(&target);
- if (target_poses_.count(target_id) == 0) {
+ if (!UseAprilTag(target_id)) {
VLOG(1) << "Rejecting target due to invalid ID " << target_id;
return RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, &builder);
}
diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
index accb0c1..4b0715b 100644
--- a/y2023/localizer/localizer.h
+++ b/y2023/localizer/localizer.h
@@ -91,6 +91,8 @@
static flatbuffers::Offset<CumulativeStatistics> StatisticsForCamera(
const CameraState &camera, flatbuffers::FlatBufferBuilder *fbb);
+ bool UseAprilTag(uint64_t target_id);
+
aos::EventLoop *const event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;