Reject targets that are too far away
Change-Id: I44374d289bc1c4b93edff0f52555e2f6180ccd07
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 7aa380e..6dd8f31 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -84,8 +84,10 @@
view.reading.distance = frame.targets[ii].distance;
view.reading.skew = frame.targets[ii].skew;
view.reading.height = frame.targets[ii].height;
- cameras_[frame.camera].PopulateNoise(&view);
- views.push_back(view);
+ if (view.reading.distance < 2.25) {
+ cameras_[frame.camera].PopulateNoise(&view);
+ views.push_back(view);
+ }
}
::aos::monotonic_clock::time_point t(
::std::chrono::nanoseconds(frame.timestamp));
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index c6928e0..1acdb66 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -289,15 +289,16 @@
// correct for it.
TEST_F(LocalizedDrivetrainTest, CameraUpdate) {
set_enable_cameras(true);
+ SetStartingPosition({4.0, 0.5, 0.0});
(*drivetrain_motor_plant_.mutable_state())(0, 0) += 0.05;
my_drivetrain_queue_.goal.MakeWithBuilder()
.controller_type(1)
.left_goal(-1.0)
.right_goal(1.0)
.Send();
- RunForTime(chrono::seconds(3));
+ RunForTime(chrono::seconds(5));
VerifyNearGoal();
- VerifyEstimatorAccurate(5e-4);
+ VerifyEstimatorAccurate(5e-3);
}
namespace {