From c528f46ba0556fb89d7683d79efb21775b91ad3c Mon Sep 17 00:00:00 2001 From: Phillip Schmidt Date: Tue, 19 Mar 2019 10:32:49 -0700 Subject: [PATCH 1/2] unit-tests: seperate wheel odometry unit test --- unit-tests/unit-tests-live.cpp | 89 +++++++++++++++++++++++++++------- 1 file changed, 71 insertions(+), 18 deletions(-) diff --git a/unit-tests/unit-tests-live.cpp b/unit-tests/unit-tests-live.cpp index 565387303f..eabf0718a1 100644 --- a/unit-tests/unit-tests-live.cpp +++ b/unit-tests/unit-tests-live.cpp @@ -5607,9 +5607,6 @@ TEST_CASE("Positional_Sensors_API", "[live]") auto pose_snr = dev.first(); CAPTURE(pose_snr); REQUIRE(pose_snr); - auto wheel_odom_snr = dev.first(); - CAPTURE(wheel_odom_snr); - REQUIRE(wheel_odom_snr); WHEN("Sequence - Streaming.") { @@ -5661,18 +5658,6 @@ TEST_CASE("Positional_Sensors_API", "[live]") CAPTURE(vnv.size()); REQUIRE(vnv.size()); REQUIRE(vnv == results); - - //Odometry API - std::ifstream calibrationFile("calibration_odometry.json"); - if (calibrationFile) - { - const std::string json_str((std::istreambuf_iterator(calibrationFile)), - std::istreambuf_iterator()); - const std::vector wo_calib(json_str.begin(), json_str.end()); - bool b; - REQUIRE_NOTHROW(b = wheel_odom_snr.load_wheel_odometery_config(wo_calib)); - REQUIRE(b); - } } } @@ -5687,9 +5672,6 @@ TEST_CASE("Positional_Sensors_API", "[live]") auto pose_snr = d.first(); CAPTURE(pose_snr); REQUIRE(pose_snr); - auto wo_snr = d.first(); - CAPTURE(wo_snr); - REQUIRE(wo_snr); rs2::frameset frames; // The frames are required to get pose with sufficient confidence for static node marker @@ -5718,6 +5700,77 @@ TEST_CASE("Positional_Sensors_API", "[live]") REQUIRE(test_or.z == Approx(vnv_or.z)); REQUIRE(test_or.w == Approx(vnv_or.w)); + REQUIRE_NOTHROW(pipe.stop()); + } + } + } + } +} + +TEST_CASE("Wheel_Odometry_API", "[live]") +{ + rs2::context ctx; + auto dev_list = ctx.query_devices(); + log_to_console(RS2_LOG_SEVERITY_WARN); + std::this_thread::sleep_for(std::chrono::seconds(5)); // T265 invocation workaround + + if (make_context(SECTION_FROM_TEST_NAME, &ctx, "2.18.1")) + { + rs2::device dev; + rs2::pipeline pipe(ctx); + rs2::config cfg; + rs2::pipeline_profile profile; + REQUIRE_NOTHROW(profile = cfg.resolve(pipe)); + REQUIRE(profile); + REQUIRE_NOTHROW(dev = profile.get_device()); + REQUIRE(dev); + disable_sensitive_options_for(dev); + dev_type PID = get_PID(dev); + CAPTURE(PID.first); + + // T265 Only + if (!librealsense::val_in_range(PID.first, { std::string("0B37")})) + { + WARN("Skipping test - Applicable for Positional Tracking sensors only. Current device type: " << PID.first << (PID.second ? " USB3" : " USB2")); + } + else + { + CAPTURE(dev); + REQUIRE(dev.is()); + auto wheel_odom_snr = dev.first(); + CAPTURE(wheel_odom_snr); + REQUIRE(wheel_odom_snr); + + WHEN("Sequence - idle.") + { + THEN("Load wheel odometry calibration") + { + //Odometry API + std::ifstream calibrationFile("calibration_odometry.json"); + if (calibrationFile) + { + const std::string json_str((std::istreambuf_iterator(calibrationFile)), + std::istreambuf_iterator()); + const std::vector wo_calib(json_str.begin(), json_str.end()); + bool b; + REQUIRE_NOTHROW(b = wheel_odom_snr.load_wheel_odometery_config(wo_calib)); + REQUIRE(b); + } + } + } + + WHEN("Sequence - Streaming.") + { + THEN("Send wheel odometry data") + { + rs2::pipeline_profile pf; + REQUIRE_NOTHROW(pf = pipe.start(cfg)); + rs2::device d = pf.get_device(); + REQUIRE(d); + auto wo_snr = d.first(); + CAPTURE(wo_snr); + REQUIRE(wo_snr); + //Odometry send data API bool b; for (int i = 0; i < 20; i++) From fcd1b61d43b7d6df6e2479f284450c9781d14d53 Mon Sep 17 00:00:00 2001 From: Phillip Schmidt Date: Tue, 19 Mar 2019 14:57:08 -0700 Subject: [PATCH 2/2] wheel odometry unit test: detect change/effect in pose output (test with static device!) --- unit-tests/unit-tests-live.cpp | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/unit-tests/unit-tests-live.cpp b/unit-tests/unit-tests-live.cpp index eabf0718a1..3ca3ba9b1e 100644 --- a/unit-tests/unit-tests-live.cpp +++ b/unit-tests/unit-tests-live.cpp @@ -5745,7 +5745,6 @@ TEST_CASE("Wheel_Odometry_API", "[live]") { THEN("Load wheel odometry calibration") { - //Odometry API std::ifstream calibrationFile("calibration_odometry.json"); if (calibrationFile) { @@ -5771,13 +5770,26 @@ TEST_CASE("Wheel_Odometry_API", "[live]") CAPTURE(wo_snr); REQUIRE(wo_snr); - //Odometry send data API bool b; - for (int i = 0; i < 20; i++) + float norm_max = 0; + WARN("T2xx Collecting frames started - Keep device static"); + rs2::frameset frames; + for (int i = 0; i < 100; i++) { - REQUIRE_NOTHROW(b = wo_snr.send_wheel_odometry(0, 0, { 1,2,3 })); + REQUIRE_NOTHROW(b = wo_snr.send_wheel_odometry(0, 0, { 1,0,0 })); REQUIRE(b); + + REQUIRE_NOTHROW(frames = pipe.wait_for_frames()); + REQUIRE(frames.size() > 0); + auto f = frames.first_or_default(RS2_STREAM_POSE); + auto pose_data = f.as().get_pose_data(); + float norm = sqrt(pose_data.translation.x*pose_data.translation.x + pose_data.translation.y*pose_data.translation.y + + pose_data.translation.z*pose_data.translation.z); + if (norm > norm_max) norm_max = norm; } + Approx approx_norm(0); + approx_norm.epsilon(0.005); // 0.5cm threshold + REQUIRE_FALSE(norm_max == approx_norm); REQUIRE_NOTHROW(pipe.stop()); } }