blob: 2cb575609a0c21a2d4d1555006e6e464e31aacaf [file] [log] [blame]
James Kuszmaul04a343c2023-02-20 16:38:22 -08001#include "y2023/localizer/localizer.h"
2
3#include "aos/events/logging/log_writer.h"
4#include "aos/events/simulated_event_loop.h"
5#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
6#include "frc971/control_loops/drivetrain/localizer_generated.h"
7#include "frc971/control_loops/pose.h"
8#include "frc971/vision/target_map_generated.h"
9#include "gtest/gtest.h"
10#include "y2023/constants/simulated_constants_sender.h"
11#include "y2023/control_loops/drivetrain/drivetrain_base.h"
12#include "y2023/localizer/status_generated.h"
James Kuszmaul18008f82023-02-23 20:52:50 -080013#include "y2023/localizer/utils.h"
James Kuszmaul04a343c2023-02-20 16:38:22 -080014
15DEFINE_string(output_folder, "",
16 "If set, logs all channels to the provided logfile.");
milind-u48601192023-03-11 19:44:46 -080017DECLARE_double(max_distance_to_target);
James Kuszmaul04a343c2023-02-20 16:38:22 -080018
19namespace y2023::localizer::testing {
20
21using frc971::control_loops::drivetrain::Output;
22
23class LocalizerTest : public ::testing::Test {
24 protected:
25 static constexpr uint64_t kTargetId = 1;
26 LocalizerTest()
27 : configuration_(aos::configuration::ReadConfig("y2023/aos_config.json")),
28 event_loop_factory_(&configuration_.message()),
29 roborio_node_([this]() {
30 // Get the constants sent before anything else happens.
31 // It has nothing to do with the roborio node.
32 SendSimulationConstants(&event_loop_factory_, 7971,
33 "y2023/constants/test_constants.json");
34 return aos::configuration::GetNode(&configuration_.message(),
35 "roborio");
36 }()),
37 imu_node_(
38 aos::configuration::GetNode(&configuration_.message(), "imu")),
39 camera_node_(
40 aos::configuration::GetNode(&configuration_.message(), "pi1")),
41 dt_config_(frc971::control_loops::drivetrain::testing::
42 GetTestDrivetrainConfig()),
43 localizer_event_loop_(
44 event_loop_factory_.MakeEventLoop("localizer", imu_node_)),
45 localizer_(localizer_event_loop_.get(), dt_config_),
46 drivetrain_plant_event_loop_(event_loop_factory_.MakeEventLoop(
47 "drivetrain_plant", roborio_node_)),
48 drivetrain_plant_imu_event_loop_(
49 event_loop_factory_.MakeEventLoop("drivetrain_plant", imu_node_)),
50 drivetrain_plant_(drivetrain_plant_event_loop_.get(),
51 drivetrain_plant_imu_event_loop_.get(), dt_config_,
52 std::chrono::microseconds(500)),
53 roborio_test_event_loop_(
54 event_loop_factory_.MakeEventLoop("test", roborio_node_)),
55 imu_test_event_loop_(
56 event_loop_factory_.MakeEventLoop("test", imu_node_)),
57 camera_test_event_loop_(
58 event_loop_factory_.MakeEventLoop("test", camera_node_)),
59 logger_test_event_loop_(
60 event_loop_factory_.GetNodeEventLoopFactory("logger")
61 ->MakeEventLoop("test")),
62 constants_fetcher_(imu_test_event_loop_.get()),
63 output_sender_(
64 roborio_test_event_loop_->MakeSender<Output>("/drivetrain")),
65 target_sender_(
66 camera_test_event_loop_->MakeSender<frc971::vision::TargetMap>(
67 "/camera")),
68 control_sender_(roborio_test_event_loop_->MakeSender<
69 frc971::control_loops::drivetrain::LocalizerControl>(
70 "/drivetrain")),
71 output_fetcher_(
72 roborio_test_event_loop_
73 ->MakeFetcher<frc971::controls::LocalizerOutput>("/localizer")),
74 status_fetcher_(
75 imu_test_event_loop_->MakeFetcher<Status>("/localizer")) {
milind-u48601192023-03-11 19:44:46 -080076 FLAGS_max_distance_to_target = 100.0;
James Kuszmaul04a343c2023-02-20 16:38:22 -080077 {
78 aos::TimerHandler *timer = roborio_test_event_loop_->AddTimer([this]() {
79 {
80 auto builder = output_sender_.MakeBuilder();
81 auto output_builder = builder.MakeBuilder<Output>();
82 output_builder.add_left_voltage(output_voltages_(0));
83 output_builder.add_right_voltage(output_voltages_(1));
84 builder.CheckOk(builder.Send(output_builder.Finish()));
85 }
86 });
87 roborio_test_event_loop_->OnRun([timer, this]() {
88 timer->Setup(roborio_test_event_loop_->monotonic_now(),
89 std::chrono::milliseconds(5));
90 });
91 }
92 {
93 // Sanity check that the test calibration files look like what we
94 // expect.
95 CHECK_EQ("pi1", constants_fetcher_.constants()
96 .cameras()
97 ->Get(0)
98 ->calibration()
99 ->node_name()
100 ->string_view());
101 const Eigen::Matrix<double, 4, 4> H_robot_camera =
102 frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
103 *constants_fetcher_.constants()
104 .cameras()
105 ->Get(0)
106 ->calibration()
107 ->fixed_extrinsics());
108
109 CHECK_EQ(kTargetId, constants_fetcher_.constants()
110 .target_map()
111 ->target_poses()
112 ->Get(0)
113 ->id());
114 const Eigen::Matrix<double, 4, 4> H_field_target = PoseToTransform(
115 constants_fetcher_.constants().target_map()->target_poses()->Get(0));
116 // For reference, the camera should pointed straight forwards on the
117 // robot, offset by 1 meter.
118 aos::TimerHandler *timer = camera_test_event_loop_->AddTimer(
119 [this, H_robot_camera, H_field_target]() {
120 if (!send_targets_) {
121 return;
122 }
123 const frc971::control_loops::Pose robot_pose(
124 {drivetrain_plant_.GetPosition().x(),
125 drivetrain_plant_.GetPosition().y(), 0.0},
milind-ub45c6d92023-03-11 18:56:15 -0800126 drivetrain_plant_.state()(2, 0) + implied_yaw_error_);
James Kuszmaul04a343c2023-02-20 16:38:22 -0800127
128 const Eigen::Matrix<double, 4, 4> H_field_camera =
129 robot_pose.AsTransformationMatrix() * H_robot_camera;
130 const Eigen::Matrix<double, 4, 4> H_camera_target =
131 H_field_camera.inverse() * H_field_target;
132
133 const Eigen::Quaterniond quat(H_camera_target.block<3, 3>(0, 0));
134 const Eigen::Vector3d translation(
135 H_camera_target.block<3, 1>(0, 3));
136
137 auto builder = target_sender_.MakeBuilder();
138 frc971::vision::Quaternion::Builder quat_builder(*builder.fbb());
139 quat_builder.add_w(quat.w());
140 quat_builder.add_x(quat.x());
141 quat_builder.add_y(quat.y());
142 quat_builder.add_z(quat.z());
143 auto quat_offset = quat_builder.Finish();
144 frc971::vision::Position::Builder position_builder(*builder.fbb());
145 position_builder.add_x(translation.x());
146 position_builder.add_y(translation.y());
147 position_builder.add_z(translation.z());
148 auto position_offset = position_builder.Finish();
149
150 frc971::vision::TargetPoseFbs::Builder target_builder(
151 *builder.fbb());
152 target_builder.add_id(send_target_id_);
153 target_builder.add_position(position_offset);
154 target_builder.add_orientation(quat_offset);
milind-u7a7f6662023-02-26 16:41:29 -0800155 target_builder.add_pose_error(pose_error_);
milind-u08fb9722023-03-25 18:23:59 -0700156 target_builder.add_pose_error_ratio(pose_error_ratio_);
James Kuszmaul04a343c2023-02-20 16:38:22 -0800157 auto target_offset = target_builder.Finish();
158
159 auto targets_offset = builder.fbb()->CreateVector({target_offset});
160 frc971::vision::TargetMap::Builder map_builder(*builder.fbb());
161 map_builder.add_target_poses(targets_offset);
162 map_builder.add_monotonic_timestamp_ns(
163 std::chrono::duration_cast<std::chrono::nanoseconds>(
164 camera_test_event_loop_->monotonic_now().time_since_epoch())
165 .count());
166
167 builder.CheckOk(builder.Send(map_builder.Finish()));
168 });
169 camera_test_event_loop_->OnRun([timer, this]() {
170 timer->Setup(camera_test_event_loop_->monotonic_now(),
171 std::chrono::milliseconds(50));
172 });
173 }
174
175 localizer_control_send_timer_ =
176 roborio_test_event_loop_->AddTimer([this]() {
177 auto builder = control_sender_.MakeBuilder();
178 auto control_builder = builder.MakeBuilder<
179 frc971::control_loops::drivetrain::LocalizerControl>();
180 control_builder.add_x(localizer_control_x_);
181 control_builder.add_y(localizer_control_y_);
182 control_builder.add_theta(localizer_control_theta_);
183 control_builder.add_theta_uncertainty(0.01);
184 control_builder.add_keep_current_theta(false);
185 builder.CheckOk(builder.Send(control_builder.Finish()));
186 });
187
188 // Get things zeroed.
189 event_loop_factory_.RunFor(std::chrono::seconds(10));
190 CHECK(status_fetcher_.Fetch());
191 CHECK(status_fetcher_->imu()->zeroed());
192
193 if (!FLAGS_output_folder.empty()) {
194 logger_event_loop_ =
195 event_loop_factory_.MakeEventLoop("logger", imu_node_);
196 logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
197 logger_->StartLoggingOnRun(FLAGS_output_folder);
198 }
199 }
200
201 void SendLocalizerControl(double x, double y, double theta) {
202 localizer_control_x_ = x;
203 localizer_control_y_ = y;
204 localizer_control_theta_ = theta;
205 localizer_control_send_timer_->Setup(
206 roborio_test_event_loop_->monotonic_now());
207 }
208 ::testing::AssertionResult IsNear(double expected, double actual,
209 double epsilon) {
210 if (std::abs(expected - actual) < epsilon) {
211 return ::testing::AssertionSuccess();
212 } else {
213 return ::testing::AssertionFailure()
214 << "Expected " << expected << " but got " << actual
215 << " with a max difference of " << epsilon
216 << " and an actual difference of " << std::abs(expected - actual);
217 }
218 }
219 ::testing::AssertionResult VerifyEstimatorAccurate(double eps) {
220 const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
221 ::testing::AssertionResult result(true);
222 status_fetcher_.Fetch();
223 if (!(result = IsNear(status_fetcher_->state()->x(), true_state(0), eps))) {
224 return result;
225 }
226 if (!(result = IsNear(status_fetcher_->state()->y(), true_state(1), eps))) {
227 return result;
228 }
229 if (!(result =
230 IsNear(status_fetcher_->state()->theta(), true_state(2), eps))) {
231 return result;
232 }
233 return result;
234 }
235
236 aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
237 aos::SimulatedEventLoopFactory event_loop_factory_;
238 const aos::Node *const roborio_node_;
239 const aos::Node *const imu_node_;
240 const aos::Node *const camera_node_;
241 const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
242 std::unique_ptr<aos::EventLoop> localizer_event_loop_;
243 Localizer localizer_;
244
245 std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
246 std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_;
247 frc971::control_loops::drivetrain::testing::DrivetrainSimulation
248 drivetrain_plant_;
249
250 std::unique_ptr<aos::EventLoop> roborio_test_event_loop_;
251 std::unique_ptr<aos::EventLoop> imu_test_event_loop_;
252 std::unique_ptr<aos::EventLoop> camera_test_event_loop_;
253 std::unique_ptr<aos::EventLoop> logger_test_event_loop_;
254
255 frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
256
257 aos::Sender<Output> output_sender_;
258 aos::Sender<frc971::vision::TargetMap> target_sender_;
259 aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
260 control_sender_;
261 aos::Fetcher<frc971::controls::LocalizerOutput> output_fetcher_;
262 aos::Fetcher<Status> status_fetcher_;
263
264 Eigen::Vector2d output_voltages_ = Eigen::Vector2d::Zero();
265
266 aos::TimerHandler *localizer_control_send_timer_;
267
268 bool send_targets_ = false;
269
270 double localizer_control_x_ = 0.0;
271 double localizer_control_y_ = 0.0;
272 double localizer_control_theta_ = 0.0;
273
274 std::unique_ptr<aos::EventLoop> logger_event_loop_;
275 std::unique_ptr<aos::logger::Logger> logger_;
276
277 uint64_t send_target_id_ = kTargetId;
milind-u7a7f6662023-02-26 16:41:29 -0800278 double pose_error_ = 1e-7;
milind-u08fb9722023-03-25 18:23:59 -0700279 double pose_error_ratio_ = 0.1;
milind-ub45c6d92023-03-11 18:56:15 -0800280 double implied_yaw_error_ = 0.0;
James Kuszmaul04a343c2023-02-20 16:38:22 -0800281
282 gflags::FlagSaver flag_saver_;
283};
284
285// Test a simple scenario with no errors where the robot should just drive
286// straight forwards.
287TEST_F(LocalizerTest, Nominal) {
288 output_voltages_ << 1.0, 1.0;
289 event_loop_factory_.RunFor(std::chrono::seconds(2));
290 CHECK(output_fetcher_.Fetch());
291 CHECK(status_fetcher_.Fetch());
292 // The two can be different because they may've been sent at different
293 // times.
294 EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-2);
295 EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
296 EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
297 1e-6);
298 // Confirm that we did indeed drive forwards (and straight), as expected.
299 EXPECT_LT(0.1, output_fetcher_->x());
300 EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
301 EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
302 EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
303 EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
304
305 // And check that we actually think that we are near where the simulator
306 // says we are.
307 EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
308}
309
310// Confirm that when the robot drives backwards that we localize correctly.
311TEST_F(LocalizerTest, NominalReverse) {
312 output_voltages_ << -1.0, -1.0;
313 event_loop_factory_.RunFor(std::chrono::seconds(2));
314 CHECK(output_fetcher_.Fetch());
315 CHECK(status_fetcher_.Fetch());
316 // The two can be different because they may've been sent at different
317 // times.
318 EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-2);
319 EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
320 EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
321 1e-6);
322 // Confirm that we did indeed drive backwards (and straight), as expected.
323 EXPECT_GT(-0.1, output_fetcher_->x());
324 EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
325 EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
326 EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
327 EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
328
329 // And check that we actually think that we are near where the simulator
330 // says we are.
331 EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
332}
333
334// Confirm that when the robot turns counter-clockwise that we localize
335// correctly.
336TEST_F(LocalizerTest, NominalSpinInPlace) {
337 output_voltages_ << -1.0, 1.0;
James Kuszmaul456774b2023-03-08 21:29:15 -0800338 // Go 1 ms over 2 sec to make sure we actually see relatively recent messages
339 // on each channel.
340 event_loop_factory_.RunFor(std::chrono::milliseconds(2001));
James Kuszmaul04a343c2023-02-20 16:38:22 -0800341 CHECK(output_fetcher_.Fetch());
342 CHECK(status_fetcher_.Fetch());
343 // The two can be different because they may've been sent at different
344 // times.
345 EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-6);
346 EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
347 EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
348 1e-2);
349 // Confirm that we did indeed turn counter-clockwise.
350 EXPECT_NEAR(0.0, output_fetcher_->x(), 1e-10);
351 EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
352 EXPECT_LT(0.1, output_fetcher_->theta());
353 EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
354 EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
355
356 // And check that we actually think that we are near where the simulator
357 // says we are.
358 EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
359}
360
361// Confirm that when the robot drives in a curve that we localize
362// successfully.
363TEST_F(LocalizerTest, NominalCurve) {
364 output_voltages_ << 2.0, 3.0;
365 event_loop_factory_.RunFor(std::chrono::seconds(4));
366 CHECK(output_fetcher_.Fetch());
367 CHECK(status_fetcher_.Fetch());
368 // The two can be different because they may've been sent at different
369 // times.
370 EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-2);
371 EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-2);
372 EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
373 1e-2);
374 // Confirm that we did indeed drive in a rough, counter-clockwise, curve.
375 EXPECT_LT(0.1, output_fetcher_->x());
376 EXPECT_LT(0.1, output_fetcher_->y());
377 EXPECT_LT(0.1, output_fetcher_->theta());
378
379 // And check that we actually think that we are near where the simulator
380 // says we are.
381 EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
382}
383
384// Tests that, in the presence of a non-zero voltage error, that we correct
385// for it.
386TEST_F(LocalizerTest, VoltageErrorDisabled) {
387 output_voltages_ << 0.0, 0.0;
388 drivetrain_plant_.set_left_voltage_offset(2.0);
389 drivetrain_plant_.set_right_voltage_offset(2.0);
390
391 event_loop_factory_.RunFor(std::chrono::seconds(2));
392 CHECK(output_fetcher_.Fetch());
393 CHECK(status_fetcher_.Fetch());
394 // We should've just ended up driving straight forwards.
395 EXPECT_LT(0.1, output_fetcher_->x());
396 EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
397 EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
398 EXPECT_NEAR(2.0, status_fetcher_->state()->left_voltage_error(), 1.0);
399 EXPECT_NEAR(2.0, status_fetcher_->state()->right_voltage_error(), 1.0);
400
401 // And check that we actually think that we are near where the simulator
402 // says we are.
403 EXPECT_TRUE(VerifyEstimatorAccurate(0.05));
404}
405
406// Tests that image corrections in the nominal case (no errors) causes no
407// issues.
408TEST_F(LocalizerTest, NominalImageCorrections) {
409 output_voltages_ << 3.0, 2.0;
410 send_targets_ = true;
411
412 event_loop_factory_.RunFor(std::chrono::seconds(4));
413 CHECK(status_fetcher_.Fetch());
414 EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
415 ASSERT_TRUE(status_fetcher_->has_statistics());
416 ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
417 ASSERT_EQ(status_fetcher_->statistics()->Get(0)->total_candidates(),
418 status_fetcher_->statistics()->Get(0)->total_accepted());
419 ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
420}
421
422// Tests that image corrections when there is an error at the start results
423// in us actually getting corrected over time.
424TEST_F(LocalizerTest, ImageCorrections) {
425 output_voltages_ << 0.0, 0.0;
426 drivetrain_plant_.mutable_state()->x() = 2.0;
427 drivetrain_plant_.mutable_state()->y() = 2.0;
428 SendLocalizerControl(5.0, 3.0, 0.0);
429 event_loop_factory_.RunFor(std::chrono::seconds(4));
430 CHECK(output_fetcher_.Fetch());
431 ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5);
432 ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5);
433 ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5);
434
435 send_targets_ = true;
436
437 event_loop_factory_.RunFor(std::chrono::seconds(4));
438 CHECK(status_fetcher_.Fetch());
439 EXPECT_TRUE(VerifyEstimatorAccurate(0.1));
440 ASSERT_TRUE(status_fetcher_->has_statistics());
441 ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
442 ASSERT_EQ(status_fetcher_->statistics()->Get(0)->total_candidates(),
443 status_fetcher_->statistics()->Get(0)->total_accepted());
444 ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
445}
446
447// Tests that we correctly reject an invalid target.
448TEST_F(LocalizerTest, InvalidTargetId) {
449 output_voltages_ << 0.0, 0.0;
450 send_targets_ = true;
451 send_target_id_ = 100;
452
453 event_loop_factory_.RunFor(std::chrono::seconds(4));
454 CHECK(status_fetcher_.Fetch());
455 ASSERT_TRUE(status_fetcher_->has_statistics());
456 ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
457 ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
458 ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
459 ASSERT_EQ(status_fetcher_->statistics()
460 ->Get(0)
461 ->rejection_reasons()
462 ->Get(static_cast<size_t>(RejectionReason::NO_SUCH_TARGET))
463 ->count(),
464 status_fetcher_->statistics()->Get(0)->total_candidates());
465}
466
milind-u7a7f6662023-02-26 16:41:29 -0800467// Tests that we correctly reject a detection with a high pose error.
468TEST_F(LocalizerTest, HighPoseError) {
469 output_voltages_ << 0.0, 0.0;
470 send_targets_ = true;
471 // Send the minimum pose error to be rejected
472 constexpr double kEps = 1e-9;
473 pose_error_ = 1e-6 + kEps;
474
475 event_loop_factory_.RunFor(std::chrono::seconds(4));
476 CHECK(status_fetcher_.Fetch());
477 ASSERT_TRUE(status_fetcher_->has_statistics());
478 ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
479 ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
480 ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
481 ASSERT_EQ(status_fetcher_->statistics()
482 ->Get(0)
483 ->rejection_reasons()
484 ->Get(static_cast<size_t>(RejectionReason::HIGH_POSE_ERROR))
485 ->count(),
486 status_fetcher_->statistics()->Get(0)->total_candidates());
487}
488
milind-ub45c6d92023-03-11 18:56:15 -0800489// Tests that we correctly reject a detection with a high implied yaw error.
490TEST_F(LocalizerTest, HighImpliedYawError) {
491 output_voltages_ << 0.0, 0.0;
492 send_targets_ = true;
493 implied_yaw_error_ = 31.0 * M_PI / 180.0;
494
495 event_loop_factory_.RunFor(std::chrono::seconds(4));
496 CHECK(status_fetcher_.Fetch());
497 ASSERT_TRUE(status_fetcher_->has_statistics());
498 ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
499 ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
500 ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
501 ASSERT_EQ(
502 status_fetcher_->statistics()
503 ->Get(0)
504 ->rejection_reasons()
505 ->Get(static_cast<size_t>(RejectionReason::HIGH_IMPLIED_YAW_ERROR))
506 ->count(),
507 status_fetcher_->statistics()->Get(0)->total_candidates());
508}
509
milind-u08fb9722023-03-25 18:23:59 -0700510// Tests that we correctly reject a detection with a high pose error ratio.
511TEST_F(LocalizerTest, HighPoseErrorRatio) {
512 output_voltages_ << 0.0, 0.0;
513 send_targets_ = true;
514 // Send the minimum pose error to be rejected
515 constexpr double kEps = 1e-9;
516 pose_error_ratio_ = 0.4 + kEps;
517
518 event_loop_factory_.RunFor(std::chrono::seconds(4));
519 CHECK(status_fetcher_.Fetch());
520 ASSERT_TRUE(status_fetcher_->has_statistics());
521 ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
522 ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
523 ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
524 ASSERT_EQ(
525 status_fetcher_->statistics()
526 ->Get(0)
527 ->rejection_reasons()
528 ->Get(static_cast<size_t>(RejectionReason::HIGH_POSE_ERROR_RATIO))
529 ->count(),
530 status_fetcher_->statistics()->Get(0)->total_candidates());
531}
532
James Kuszmaul04a343c2023-02-20 16:38:22 -0800533} // namespace y2023::localizer::testing