From a6a430313ef1b1706e57f09006054e3a0f1488b4 Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Mon, 9 Oct 2023 19:25:47 -0300 Subject: [PATCH 1/2] Removes maliput::test_utilities dependency. Signed-off-by: Franco Cipollone --- .../multilane_types_compare.h | 4 +- .../CMakeLists.txt | 1 - test/CMakeLists.txt | 1 + test/assert_compare.h | 48 ++ .../multilane_arc_road_curve_test.cc | 119 ++--- .../multilane_builder_test.cc | 75 +-- .../multilane_connection_test.cc | 78 ++-- .../maliput_multilane/multilane_lanes_test.cc | 435 +++++++++--------- .../multilane_line_road_curve_test.cc | 69 +-- .../multilane_loader_test.cc | 1 - .../multilane_road_geometry_test.cc | 119 ++--- .../multilane_segments_test.cc | 18 +- 12 files changed, 534 insertions(+), 434 deletions(-) create mode 100644 test/assert_compare.h diff --git a/include/maliput_multilane_test_utilities/multilane_types_compare.h b/include/maliput_multilane_test_utilities/multilane_types_compare.h index b23bd66..a25663c 100644 --- a/include/maliput_multilane_test_utilities/multilane_types_compare.h +++ b/include/maliput_multilane_test_utilities/multilane_types_compare.h @@ -31,7 +31,7 @@ #include #include -#include +#include #include "maliput_multilane/builder.h" #include "maliput_multilane/connection.h" @@ -124,7 +124,7 @@ class HBoundsMatcher : public MatcherInterface { : elevation_bounds_(elevation_bounds), tolerance_(tolerance) {} bool MatchAndExplain(const api::HBounds& other, MatchResultListener*) const override { - return api::test::IsHBoundsClose(elevation_bounds_, other, tolerance_); + return !api::IsHBoundsClose(elevation_bounds_, other, tolerance_).message.has_value(); } void DescribeTo(std::ostream* os) const override { diff --git a/src/maliput_multilane_test_utilities/CMakeLists.txt b/src/maliput_multilane_test_utilities/CMakeLists.txt index 1876246..a0b46cb 100644 --- a/src/maliput_multilane_test_utilities/CMakeLists.txt +++ b/src/maliput_multilane_test_utilities/CMakeLists.txt @@ -29,7 +29,6 @@ target_link_libraries(test_utilities PUBLIC maliput::api maliput::common - maliput::test_utilities maliput::math maliput_multilane PRIVATE diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index edf4907..2435391 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -7,6 +7,7 @@ macro(add_dependencies_to_test target) target_include_directories(${target} PRIVATE ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/test ) set(MULTILANE_RESOURCE_ROOT ${PROJECT_SOURCE_DIR}/resources/) diff --git a/test/assert_compare.h b/test/assert_compare.h new file mode 100644 index 0000000..172afed --- /dev/null +++ b/test/assert_compare.h @@ -0,0 +1,48 @@ +// BSD 3-Clause License +// +// Copyright (c) 2023, Woven by Toyota. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include +#include + +namespace maliput { +namespace multilane { +namespace test { + +template +::testing::AssertionResult AssertCompare(const maliput::common::ComparisonResult& res) { + if (!res.message.has_value()) { + return ::testing::AssertionSuccess(); + } + return ::testing::AssertionFailure() << res.message.value(); +} + +} // namespace test +} // namespace multilane +} // namespace maliput diff --git a/test/maliput_multilane/multilane_arc_road_curve_test.cc b/test/maliput_multilane/multilane_arc_road_curve_test.cc index ddea7ed..1997bb1 100644 --- a/test/maliput_multilane/multilane_arc_road_curve_test.cc +++ b/test/maliput_multilane/multilane_arc_road_curve_test.cc @@ -34,13 +34,16 @@ #include #include #include -#include +#include + +#include "assert_compare.h" namespace maliput { namespace multilane { namespace { -using maliput::math::test::CompareVectors; +using maliput::math::CompareVectors; +using maliput::multilane::test::AssertCompare; class MultilaneArcRoadCurveTest : public ::testing::Test { protected: @@ -116,25 +119,27 @@ TEST_F(MultilaneArcRoadCurveTest, ArcGeometryTest) { EXPECT_THROW(p_from_s_at_r(2. * offset_line_length), maliput::common::assertion_error); // Checks the evaluation of xy at different values over the reference curve. - EXPECT_TRUE(CompareVectors( - dut.xy_of_p(0.0), kCenter + math::Vector2(kRadius * std::cos(kTheta0), kRadius * std::sin(kTheta0)), kVeryExact)); - EXPECT_TRUE(CompareVectors( + EXPECT_TRUE(AssertCompare( + CompareVectors(dut.xy_of_p(0.0), + kCenter + math::Vector2(kRadius * std::cos(kTheta0), kRadius * std::sin(kTheta0)), kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors( dut.xy_of_p(0.5), kCenter + math::Vector2(kRadius * std::cos(kTheta0 + kDTheta * 0.5), kRadius * std::sin(kTheta0 + kDTheta * 0.5)), - kVeryExact)); - EXPECT_TRUE(CompareVectors( - dut.xy_of_p(1.0), kCenter + math::Vector2(kRadius * std::cos(kTheta1), kRadius * std::sin(kTheta1)), kVeryExact)); + kVeryExact))); + EXPECT_TRUE(AssertCompare( + CompareVectors(dut.xy_of_p(1.0), + kCenter + math::Vector2(kRadius * std::cos(kTheta1), kRadius * std::sin(kTheta1)), kVeryExact))); // Checks the derivative of xy at different values over the reference curve. - EXPECT_TRUE(CompareVectors( + EXPECT_TRUE(AssertCompare(CompareVectors( dut.xy_dot_of_p(0.0), - math::Vector2(-kRadius * std::sin(kTheta0) * kDTheta, kRadius * std::cos(kTheta0) * kDTheta), kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.xy_dot_of_p(0.5), - math::Vector2(-kRadius * std::sin(kTheta0 + 0.5 * kDTheta) * kDTheta, - kRadius * std::cos(kTheta0 + 0.5 * kDTheta) * kDTheta), - kVeryExact)); - EXPECT_TRUE(CompareVectors( + math::Vector2(-kRadius * std::sin(kTheta0) * kDTheta, kRadius * std::cos(kTheta0) * kDTheta), kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors(dut.xy_dot_of_p(0.5), + math::Vector2(-kRadius * std::sin(kTheta0 + 0.5 * kDTheta) * kDTheta, + kRadius * std::cos(kTheta0 + 0.5 * kDTheta) * kDTheta), + kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors( dut.xy_dot_of_p(1.0), - math::Vector2(-kRadius * std::sin(kTheta1) * kDTheta, kRadius * std::cos(kTheta1) * kDTheta), kVeryExact)); + math::Vector2(-kRadius * std::sin(kTheta1) * kDTheta, kRadius * std::cos(kTheta1) * kDTheta), kVeryExact))); // Checks the heading at different values. EXPECT_NEAR(dut.heading_of_p(0.0), kTheta0 + M_PI / 2.0, kVeryExact); EXPECT_NEAR(dut.heading_of_p(0.5), kTheta0 + kDTheta / 2.0 + M_PI / 2.0, kVeryExact); @@ -206,31 +211,33 @@ TEST_F(MultilaneArcRoadCurveTest, ToCurveFrameTest) { const ArcRoadCurve dut(kCenter, kRadius, kTheta0, kDTheta, zp, zp, kLinearTolerance, kScaleLength, kComputationPolicy); // Checks points over the composed curve. - EXPECT_TRUE(CompareVectors(dut.ToCurveFrame(math::Vector3(kCenter[0] + kRadius * std::cos(kTheta0), - kCenter[1] + kRadius * std::sin(kTheta0), 0.0), - kRMin, kRMax, height_bounds), - math::Vector3(0.0, 0.0, 0.0), kVeryExact)); EXPECT_TRUE( + AssertCompare(CompareVectors(dut.ToCurveFrame(math::Vector3(kCenter[0] + kRadius * std::cos(kTheta0), + kCenter[1] + kRadius * std::sin(kTheta0), 0.0), + kRMin, kRMax, height_bounds), + math::Vector3(0.0, 0.0, 0.0), kVeryExact))); + EXPECT_TRUE(AssertCompare( CompareVectors(dut.ToCurveFrame(math::Vector3(kCenter[0] + kRadius * std::cos(kTheta0 + kDTheta / 2.0), kCenter[1] + kRadius * std::sin(kTheta0 + kDTheta / 2.0), 0.0), kRMin, kRMax, height_bounds), - math::Vector3(0.5, 0.0, 0.0), kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.ToCurveFrame(math::Vector3(kCenter[0] + kRadius * std::cos(kTheta1), - kCenter[1] + kRadius * std::sin(kTheta1), 0.0), - kRMin, kRMax, height_bounds), - math::Vector3(1., 0.0, 0.0), kVeryExact)); - // Checks with lateral and vertical deviations. + math::Vector3(0.5, 0.0, 0.0), kVeryExact))); EXPECT_TRUE( + AssertCompare(CompareVectors(dut.ToCurveFrame(math::Vector3(kCenter[0] + kRadius * std::cos(kTheta1), + kCenter[1] + kRadius * std::sin(kTheta1), 0.0), + kRMin, kRMax, height_bounds), + math::Vector3(1., 0.0, 0.0), kVeryExact))); + // Checks with lateral and vertical deviations. + EXPECT_TRUE(AssertCompare( CompareVectors(dut.ToCurveFrame(math::Vector3(kCenter[0] + (kRadius + 1.0) * std::cos(kTheta0 + M_PI / 8.0), kCenter[1] + (kRadius + 1.0) * std::sin(kTheta0 + M_PI / 8.0), 6.0), kRMin, kRMax, height_bounds), - math::Vector3(0.25, -1.0, 6.0), kVeryExact)); - EXPECT_TRUE(CompareVectors( + math::Vector3(0.25, -1.0, 6.0), kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors( dut.ToCurveFrame( math::Vector3(kCenter[0] + (kRadius - 2.0) * std::cos(kTheta0 + kDTheta / 2.0 + M_PI / 8.0), kCenter[1] + (kRadius - 2.0) * std::sin(kTheta0 + kDTheta / 2.0 + M_PI / 8.0), 3.0), kRMin, kRMax, height_bounds), - math::Vector3(0.75, 2.0, 3.0), kVeryExact)); + math::Vector3(0.75, 2.0, 3.0), kVeryExact))); } // Checks that l_max(), p_from_s() and s_from_p() with constant @@ -300,7 +307,7 @@ TEST_F(MultilaneArcRoadCurveTest, WorldFunction) { const double angle = kTheta0 + kDTheta * p; const math::Vector3 geo_position = kGeoCenter + kRadius * math::Vector3(std::cos(angle), std::sin(angle), 0.) + flat_rotation.apply({0., r, h}); - EXPECT_TRUE(CompareVectors(flat_dut.W_of_prh(p, r, h), geo_position, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(flat_dut.W_of_prh(p, r, h), geo_position, kVeryExact))); } } } @@ -322,7 +329,7 @@ TEST_F(MultilaneArcRoadCurveTest, WorldFunction) { const double angle = kTheta0 + kDTheta * p; const math::Vector3 geo_position = kGeoCenter + kRadius * math::Vector3(std::cos(angle), std::sin(angle), 0.) + linear_elevation.f_p(p) * z_vector + elevated_rotation.apply({0., r, h}); - EXPECT_TRUE(CompareVectors(elevated_dut.W_of_prh(p, r, h), geo_position, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(elevated_dut.W_of_prh(p, r, h), geo_position, kVeryExact))); } } } @@ -339,7 +346,7 @@ TEST_F(MultilaneArcRoadCurveTest, WorldFunction) { const double angle = kTheta0 + kDTheta * p; const math::Vector3 geo_position = kGeoCenter + kRadius * math::Vector3(std::cos(angle), std::sin(angle), 0.) + superelevated_rotation.apply({0., r, h}); - EXPECT_TRUE(CompareVectors(superelevated_dut.W_of_prh(p, r, h), geo_position, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(superelevated_dut.W_of_prh(p, r, h), geo_position, kVeryExact))); } } } @@ -379,9 +386,9 @@ TEST_F(MultilaneArcRoadCurveTest, WorldFunctionDerivative) { const double g_prime = flat_dut.elevation().f_dot_p(p); const math::Vector3 w_prime = flat_dut.W_prime_of_prh(p, r, h, rotation, g_prime); const math::Vector3 numeric_w_prime = numeric_w_prime_of_prh(flat_dut, p, r, h); - EXPECT_TRUE(CompareVectors(w_prime, numeric_w_prime, kQuiteExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime, numeric_w_prime, kQuiteExact))); const math::Vector3 s_hat = flat_dut.s_hat_of_prh(p, r, h, rotation, g_prime); - EXPECT_TRUE(CompareVectors(w_prime.normalized(), s_hat, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime.normalized(), s_hat, kVeryExact))); } } } @@ -400,9 +407,9 @@ TEST_F(MultilaneArcRoadCurveTest, WorldFunctionDerivative) { const double g_prime = elevated_dut.elevation().f_dot_p(p); const math::Vector3 w_prime = elevated_dut.W_prime_of_prh(p, r, h, rotation, g_prime); const math::Vector3 numeric_w_prime = numeric_w_prime_of_prh(elevated_dut, p, r, h); - EXPECT_TRUE(CompareVectors(w_prime, numeric_w_prime, kQuiteExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime, numeric_w_prime, kQuiteExact))); const math::Vector3 s_hat = elevated_dut.s_hat_of_prh(p, r, h, rotation, g_prime); - EXPECT_TRUE(CompareVectors(w_prime.normalized(), s_hat, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime.normalized(), s_hat, kVeryExact))); } } } @@ -420,9 +427,9 @@ TEST_F(MultilaneArcRoadCurveTest, WorldFunctionDerivative) { const double g_prime = superelevated_dut.elevation().f_dot_p(p); const math::Vector3 w_prime = superelevated_dut.W_prime_of_prh(p, r, h, rotation, g_prime); const math::Vector3 numeric_w_prime = numeric_w_prime_of_prh(superelevated_dut, p, r, h); - EXPECT_TRUE(CompareVectors(w_prime, numeric_w_prime, kQuiteExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime, numeric_w_prime, kQuiteExact))); const math::Vector3 s_hat = superelevated_dut.s_hat_of_prh(p, r, h, rotation, g_prime); - EXPECT_TRUE(CompareVectors(w_prime.normalized(), s_hat, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime.normalized(), s_hat, kVeryExact))); } } } @@ -453,28 +460,31 @@ TEST_F(MultilaneArcRoadCurveTest, ReferenceCurveRotation) { EXPECT_NEAR(rotation.pitch(), kZeroPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), 3. * M_PI / 4., kVeryExact); math::Vector3 r_hat = flat_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(-0.707106781186548, -0.707106781186548, 0.), kVeryExact)); + EXPECT_TRUE( + AssertCompare(CompareVectors(r_hat, math::Vector3(-0.707106781186548, -0.707106781186548, 0.), kVeryExact))); rotation = flat_dut.Rabg_of_p(0.5); EXPECT_NEAR(rotation.roll(), kZeroRoll, kVeryExact); EXPECT_NEAR(rotation.pitch(), kZeroPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -M_PI, kVeryExact); r_hat = flat_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(0., -1., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(r_hat, math::Vector3(0., -1., 0.), kVeryExact))); rotation = flat_dut.Rabg_of_p(0.75); EXPECT_NEAR(rotation.roll(), kZeroRoll, kVeryExact); EXPECT_NEAR(rotation.pitch(), kZeroPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -7. * M_PI / 8., kVeryExact); r_hat = flat_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(0.382683432365090, -0.923879532511287, 0.), kVeryExact)); + EXPECT_TRUE( + AssertCompare(CompareVectors(r_hat, math::Vector3(0.382683432365090, -0.923879532511287, 0.), kVeryExact))); rotation = flat_dut.Rabg_of_p(1.); EXPECT_NEAR(rotation.roll(), kZeroRoll, kVeryExact); EXPECT_NEAR(rotation.pitch(), kZeroPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -3. * M_PI / 4., kVeryExact); r_hat = flat_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(0.707106781186548, -0.707106781186548, 0.), kVeryExact)); + EXPECT_TRUE( + AssertCompare(CompareVectors(r_hat, math::Vector3(0.707106781186548, -0.707106781186548, 0.), kVeryExact))); } // Checks for a linearly elevated curve. @@ -493,28 +503,31 @@ TEST_F(MultilaneArcRoadCurveTest, ReferenceCurveRotation) { EXPECT_NEAR(wrap(rotation.pitch()), kElevationPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), 3. * M_PI / 4., kVeryExact); math::Vector3 r_hat = elevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(-0.707106781186548, -0.707106781186548, 0.), kVeryExact)); + EXPECT_TRUE( + AssertCompare(CompareVectors(r_hat, math::Vector3(-0.707106781186548, -0.707106781186548, 0.), kVeryExact))); rotation = elevated_dut.Rabg_of_p(0.5); EXPECT_NEAR(rotation.roll(), kZeroRoll, kVeryExact); EXPECT_NEAR(wrap(rotation.pitch()), kElevationPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -M_PI, kVeryExact); r_hat = elevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(0., -1., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(r_hat, math::Vector3(0., -1., 0.), kVeryExact))); rotation = elevated_dut.Rabg_of_p(0.75); EXPECT_NEAR(rotation.roll(), kZeroRoll, kVeryExact); EXPECT_NEAR(wrap(rotation.pitch()), kElevationPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -7. * M_PI / 8., kVeryExact); r_hat = elevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(0.382683432365090, -0.923879532511287, 0.), kVeryExact)); + EXPECT_TRUE( + AssertCompare(CompareVectors(r_hat, math::Vector3(0.382683432365090, -0.923879532511287, 0.), kVeryExact))); rotation = elevated_dut.Rabg_of_p(1.); EXPECT_NEAR(rotation.roll(), kZeroRoll, kVeryExact); EXPECT_NEAR(wrap(rotation.pitch()), kElevationPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -3. * M_PI / 4., kVeryExact); r_hat = elevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(0.707106781186548, -0.707106781186548, 0.), kVeryExact)); + EXPECT_TRUE( + AssertCompare(CompareVectors(r_hat, math::Vector3(0.707106781186548, -0.707106781186548, 0.), kVeryExact))); } // Checks for a curve with constant non zero superelevation. @@ -531,31 +544,31 @@ TEST_F(MultilaneArcRoadCurveTest, ReferenceCurveRotation) { EXPECT_NEAR(wrap(rotation.pitch()), kZeroPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), 3. * M_PI / 4., kVeryExact); math::Vector3 r_hat = superelevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE( - CompareVectors(r_hat, math::Vector3(-0.353553390593274, -0.353553390593274, 0.866025403784439), kVeryExact)); + EXPECT_TRUE(AssertCompare( + CompareVectors(r_hat, math::Vector3(-0.353553390593274, -0.353553390593274, 0.866025403784439), kVeryExact))); rotation = superelevated_dut.Rabg_of_p(0.5); EXPECT_NEAR(rotation.roll(), kSuperelevationOffset, kVeryExact); EXPECT_NEAR(wrap(rotation.pitch()), kZeroPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -M_PI, kVeryExact); r_hat = superelevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_hat, math::Vector3(0.0, -0.5, 0.866025403784439), kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(r_hat, math::Vector3(0.0, -0.5, 0.866025403784439), kVeryExact))); rotation = superelevated_dut.Rabg_of_p(0.75); EXPECT_NEAR(rotation.roll(), kSuperelevationOffset, kVeryExact); EXPECT_NEAR(wrap(rotation.pitch()), kZeroPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -7. * M_PI / 8., kVeryExact); r_hat = superelevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE( - CompareVectors(r_hat, math::Vector3(0.191341716182545, -0.461939766255643, 0.866025403784439), kVeryExact)); + EXPECT_TRUE(AssertCompare( + CompareVectors(r_hat, math::Vector3(0.191341716182545, -0.461939766255643, 0.866025403784439), kVeryExact))); rotation = superelevated_dut.Rabg_of_p(1.); EXPECT_NEAR(rotation.roll(), kSuperelevationOffset, kVeryExact); EXPECT_NEAR(wrap(rotation.pitch()), kZeroPitch, kVeryExact); EXPECT_NEAR(wrap(rotation.yaw()), -3. * M_PI / 4., kVeryExact); r_hat = superelevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE( - CompareVectors(r_hat, math::Vector3(0.353553390593274, -0.353553390593274, 0.866025403784439), kVeryExact)); + EXPECT_TRUE(AssertCompare( + CompareVectors(r_hat, math::Vector3(0.353553390593274, -0.353553390593274, 0.866025403784439), kVeryExact))); } } diff --git a/test/maliput_multilane/multilane_builder_test.cc b/test/maliput_multilane/multilane_builder_test.cc index a4c2fdb..da6197a 100644 --- a/test/maliput_multilane/multilane_builder_test.cc +++ b/test/maliput_multilane/multilane_builder_test.cc @@ -38,14 +38,14 @@ #include #include +#include #include #include #include #include #include -#include -#include +#include "assert_compare.h" #include "maliput_multilane_test_utilities/multilane_types_compare.h" namespace maliput { @@ -53,6 +53,7 @@ namespace multilane { namespace { using Which = api::LaneEnd::Which; +using maliput::multilane::test::AssertCompare; // StartReference::Spec using an Endpoint. GTEST_TEST(StartReferenceSpecTest, Endpoint) { @@ -179,7 +180,7 @@ GTEST_TEST(MultilaneBuilderTest, ParameterConstructor) { Builder builder(kLaneWidth, kElevationBounds, kLinearTolerance, kAngularTolerance, kScaleLength, kComputationPolicy, std::make_unique()); EXPECT_EQ(builder.get_lane_width(), kLaneWidth); - EXPECT_TRUE(api::test::IsHBoundsClose(builder.get_elevation_bounds(), kElevationBounds, 0.)); + EXPECT_TRUE(AssertCompare(api::IsHBoundsClose(builder.get_elevation_bounds(), kElevationBounds, 0.))); EXPECT_EQ(builder.get_linear_tolerance(), kLinearTolerance); EXPECT_EQ(builder.get_angular_tolerance(), kAngularTolerance); EXPECT_EQ(builder.get_scale_length(), kScaleLength); @@ -343,7 +344,8 @@ GTEST_TEST(MultilaneBuilderTest, Fig8) { EXPECT_EQ(bp->GetBSide()->size(), 1); } - EXPECT_TRUE(api::test::CheckIdIndexing(rg.get())); + const auto check_id_indexing = api::CheckIdIndexing(rg.get()); + EXPECT_FALSE(check_id_indexing.has_value()) << check_id_indexing.value(); }; GTEST_TEST(MultilaneBuilderTest, QuadRing) { @@ -457,7 +459,8 @@ GTEST_TEST(MultilaneBuilderTest, QuadRing) { } } - EXPECT_TRUE(api::test::CheckIdIndexing(rg.get())); + const auto check_id_indexing = api::CheckIdIndexing(rg.get()); + EXPECT_FALSE(check_id_indexing.has_value()) << check_id_indexing.value(); }; // Holds common properties for reference-to-reference curve primitive tests. @@ -510,8 +513,8 @@ TEST_F(MultilaneBuilderReferenceCurvePrimitivesTest, LineSegment) { // applied. const math::Vector3 lane_start_geo = start_reference_curve + (kRefR0 + static_cast(i) * kLaneWidth) * r_versor; - EXPECT_TRUE(api::test::IsInertialPositionClose(lane->ToInertialPosition({0., 0., 0.}), - api::InertialPosition::FromXyz(lane_start_geo), kLinearTolerance)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( + lane->ToInertialPosition({0., 0., 0.}), api::InertialPosition::FromXyz(lane_start_geo), kLinearTolerance))); // Checks lane start and end BranchPoint. const api::BranchPoint* const start_bp = lane->GetBranchPoint(api::LaneEnd::kStart); EXPECT_EQ(start_bp->GetASide()->size(), 1); @@ -557,8 +560,9 @@ TEST_F(MultilaneBuilderReferenceCurvePrimitivesTest, ArcSegment) { // applied. const math::Vector3 lane_start_inertial = start_reference_curve + (kRefR0 + static_cast(i) * kLaneWidth) * r_versor; - EXPECT_TRUE(api::test::IsInertialPositionClose( - lane->ToInertialPosition({0., 0., 0.}), api::InertialPosition::FromXyz(lane_start_inertial), kLinearTolerance)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(lane->ToInertialPosition({0., 0., 0.}), + api::InertialPosition::FromXyz(lane_start_inertial), + kLinearTolerance))); // Checks lane start and end BranchPoint. const api::BranchPoint* const start_bp = lane->GetBranchPoint(api::LaneEnd::kStart); EXPECT_EQ(start_bp->GetASide()->size(), 1); @@ -712,8 +716,9 @@ TEST_F(MultilaneBuilderLaneToLanePrimitivesTest, FlatLineSegment) { // applied. const math::Vector3 lane_start_inertial = start_reference_curve + (-kRefR0 + static_cast(i) * kLaneWidth) * r_versor; - EXPECT_TRUE(api::test::IsInertialPositionClose( - lane->ToInertialPosition({0., 0., 0.}), api::InertialPosition::FromXyz(lane_start_inertial), kLinearTolerance)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(lane->ToInertialPosition({0., 0., 0.}), + api::InertialPosition::FromXyz(lane_start_inertial), + kLinearTolerance))); // Checks lane start and end BranchPoint. const api::BranchPoint* const start_bp = lane->GetBranchPoint(api::LaneEnd::kStart); EXPECT_EQ(start_bp->GetASide()->size(), 1); @@ -763,13 +768,13 @@ TEST_F(MultilaneBuilderLaneToLanePrimitivesTest, ElevatedEndLineSegment) { // Checks lane start inertial position to verify that spacing is correctly // applied. const math::Vector3 r_offset = (-kRefR0 + static_cast(i) * kLaneWidth) * r_versor; - EXPECT_TRUE(api::test::IsInertialPositionClose(lane->ToInertialPosition({0., 0., 0.}), - api::InertialPosition::FromXyz(start_reference_curve + r_offset), - kLinearTolerance)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( + lane->ToInertialPosition({0., 0., 0.}), api::InertialPosition::FromXyz(start_reference_curve + r_offset), + kLinearTolerance))); // Checks lane end inertial position to verify elevation is correctly applied. - EXPECT_TRUE(api::test::IsInertialPositionClose(lane->ToInertialPosition({lane->length(), 0., 0.}), - api::InertialPosition::FromXyz(end_reference_curve + r_offset), - kLinearTolerance)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( + lane->ToInertialPosition({lane->length(), 0., 0.}), + api::InertialPosition::FromXyz(end_reference_curve + r_offset), kLinearTolerance))); // Checks lane start and end BranchPoint. const api::BranchPoint* const start_bp = lane->GetBranchPoint(api::LaneEnd::kStart); EXPECT_EQ(start_bp->GetASide()->size(), 1); @@ -816,8 +821,9 @@ TEST_F(MultilaneBuilderLaneToLanePrimitivesTest, ArcSegment) { // applied. const math::Vector3 lane_start_inertial = start_reference_curve + (-kRefR0 + static_cast(i) * kLaneWidth) * r_versor; - EXPECT_TRUE(api::test::IsInertialPositionClose( - lane->ToInertialPosition({0., 0., 0.}), api::InertialPosition::FromXyz(lane_start_inertial), kLinearTolerance)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(lane->ToInertialPosition({0., 0., 0.}), + api::InertialPosition::FromXyz(lane_start_inertial), + kLinearTolerance))); // Checks lane start and end BranchPoint. const api::BranchPoint* const start_bp = lane->GetBranchPoint(api::LaneEnd::kStart); EXPECT_EQ(start_bp->GetASide()->size(), 1); @@ -867,14 +873,14 @@ TEST_F(MultilaneBuilderLaneToLanePrimitivesTest, ElevatedEndArcSegment) { // Checks lane start inertial position to verify that spacing is correctly // applied. const math::Vector3 r_offset_start = (-kRefR0 + static_cast(i) * kLaneWidth) * r_versor_start; - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( lane->ToInertialPosition({0., 0., 0.}), api::InertialPosition::FromXyz(start_reference_curve + r_offset_start), - kLinearTolerance)); + kLinearTolerance))); // Checks lane end inertial position to verify elevation is correctly applied. const math::Vector3 r_offset_end = (-kRefR0 + static_cast(i) * kLaneWidth) * r_versor_end; - EXPECT_TRUE(api::test::IsInertialPositionClose(lane->ToInertialPosition({lane->length(), 0., 0.}), - api::InertialPosition::FromXyz(end_reference_curve + r_offset_end), - kLinearTolerance)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( + lane->ToInertialPosition({lane->length(), 0., 0.}), + api::InertialPosition::FromXyz(end_reference_curve + r_offset_end), kLinearTolerance))); // Checks lane start and end BranchPoint. const api::BranchPoint* const start_bp = lane->GetBranchPoint(api::LaneEnd::kStart); EXPECT_EQ(start_bp->GetASide()->size(), 1); @@ -976,21 +982,21 @@ class TurnBuildProcedure : public BuildProcedure { for (double r = lbounds.min(); r <= lbounds.max(); r += lane_width / 10) { // Since the curved lane heading at the origin is opposite to that of // the straight lane by construction, the r-offset sign is reversed. - EXPECT_TRUE(api::test::IsInertialPositionClose(straight_lane->ToInertialPosition({kS, r, kH}), - curved_lane->ToInertialPosition({kS, -r, kH}), - road_geometry.linear_tolerance())) + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(straight_lane->ToInertialPosition({kS, r, kH}), + curved_lane->ToInertialPosition({kS, -r, kH}), + road_geometry.linear_tolerance()))) << "Position discontinuity at r = " << r; // Since the curved lane heading at the origin is opposite to that of // the straight lane by construction, the resulting orientation is // rotated pi radians about the h-axis. const api::Rotation rrotation = curved_lane->GetOrientation({kS, -r, kH}); const math::Quaternion pi_rotation(M_PI, math::Vector3(0., 0., 1.)); - EXPECT_TRUE(api::test::IsRotationClose(straight_lane->GetOrientation({kS, r, kH}), - // Applies a pi radians rotation around the h-axis to the curved - // lane orientation (i.e. apply an intrinsic pi radians rotation - // about the lane local z-axis, effectively post-multiplying). - api::Rotation::FromQuat(rrotation.quat() * pi_rotation), - road_geometry.angular_tolerance())) + EXPECT_TRUE(AssertCompare(api::IsRotationClose(straight_lane->GetOrientation({kS, r, kH}), + // Applies a pi radians rotation around the h-axis to the curved + // lane orientation (i.e. apply an intrinsic pi radians rotation + // about the lane local z-axis, effectively post-multiplying). + api::Rotation::FromQuat(rrotation.quat() * pi_rotation), + road_geometry.angular_tolerance()))) << " Orientation discontinuity at r = " << r; } } @@ -1418,7 +1424,8 @@ class MultilaneBuilderMultilaneCrossTest : public ::testing::Test { } EXPECT_EQ(rg->num_branch_points(), 20); - EXPECT_TRUE(api::test::CheckIdIndexing(rg.get())); + const auto check_id_indexing = api::CheckIdIndexing(rg.get()); + EXPECT_FALSE(check_id_indexing.has_value()) << check_id_indexing.value(); } }; diff --git a/test/maliput_multilane/multilane_connection_test.cc b/test/maliput_multilane/multilane_connection_test.cc index 5aea657..82cebee 100644 --- a/test/maliput_multilane/multilane_connection_test.cc +++ b/test/maliput_multilane/multilane_connection_test.cc @@ -36,21 +36,23 @@ #include #include +#include #include -#include +#include "assert_compare.h" #include "maliput_multilane/arc_road_curve.h" #include "maliput_multilane/cubic_polynomial.h" #include "maliput_multilane/line_road_curve.h" #include "maliput_multilane/make_road_curve_for_connection.h" #include "maliput_multilane_test_utilities/multilane_types_compare.h" +using ::maliput::math::CompareVectors; +using ::maliput::multilane::test::AssertCompare; + namespace maliput { namespace multilane { namespace test { -using maliput::math::test::CompareVectors; - // Compares equality within @p tolerance of @p cubic1 and @p cubic2 // coefficients. // @param cubic1 A CubicPolynomial object to compare. @@ -92,8 +94,6 @@ ::testing::AssertionResult IsCubicPolynomialClose(const CubicPolynomial& cubic1, namespace { -using maliput::math::test::CompareVectors; - // EndpointXy checks. GTEST_TEST(EndpointXyTest, DefaultConstructor) { const EndpointXy dut{}; @@ -273,16 +273,17 @@ TEST_F(MultilaneConnectionTest, ArcRoadCurveValidation) { EXPECT_NE(dynamic_cast(road_curve.get()), nullptr); // Checks that the road curve starts and ends at given endpoints. const math::Vector3 flat_origin = road_curve->W_of_prh(0., 0., 0.); - EXPECT_TRUE( + EXPECT_TRUE(AssertCompare( CompareVectors(math::Vector3(flat_dut.start().xy().x(), flat_dut.start().xy().y(), flat_dut.start().z().z()), - flat_origin, kZeroTolerance)); - EXPECT_TRUE(CompareVectors(math::Vector3(kStartEndpoint.xy().x(), kStartEndpoint.xy().y(), kStartEndpoint.z().z()), - flat_origin, kZeroTolerance)); + flat_origin, kZeroTolerance))); + EXPECT_TRUE(AssertCompare( + CompareVectors(math::Vector3(kStartEndpoint.xy().x(), kStartEndpoint.xy().y(), kStartEndpoint.z().z()), + flat_origin, kZeroTolerance))); const math::Vector3 flat_end = road_curve->W_of_prh(1., 0., 0.); - EXPECT_TRUE(CompareVectors(math::Vector3(flat_dut.end().xy().x(), flat_dut.end().xy().y(), flat_dut.end().z().z()), - flat_end, kVeryExact)); - EXPECT_TRUE(CompareVectors(math::Vector3(kEndEndpoint.xy().x(), kEndEndpoint.xy().y(), kEndEndpoint.z().z()), - flat_end, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors( + math::Vector3(flat_dut.end().xy().x(), flat_dut.end().xy().y(), flat_dut.end().z().z()), flat_end, kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors( + math::Vector3(kEndEndpoint.xy().x(), kEndEndpoint.xy().y(), kEndEndpoint.z().z()), flat_end, kVeryExact))); // Checks that elevation and superelevation polynomials are correctly built // for the trivial case of a flat dut. EXPECT_TRUE(test::IsCubicPolynomialClose(road_curve->elevation(), CubicPolynomial(), kZeroTolerance)); @@ -295,18 +296,19 @@ TEST_F(MultilaneConnectionTest, ArcRoadCurveValidation) { std::unique_ptr complex_road_curve = MakeRoadCurveFor(complex_dut); // Checks that the road curve starts and ends at given endpoints. const math::Vector3 complex_origin = complex_road_curve->W_of_prh(0., 0., 0.); - EXPECT_TRUE(CompareVectors( + EXPECT_TRUE(AssertCompare(CompareVectors( math::Vector3(complex_dut.start().xy().x(), complex_dut.start().xy().y(), complex_dut.start().z().z()), - complex_origin, kZeroTolerance)); - EXPECT_TRUE(CompareVectors(math::Vector3(kStartEndpoint.xy().x(), kStartEndpoint.xy().y(), kStartEndpoint.z().z()), - complex_origin, kZeroTolerance)); + complex_origin, kZeroTolerance))); + EXPECT_TRUE(AssertCompare( + CompareVectors(math::Vector3(kStartEndpoint.xy().x(), kStartEndpoint.xy().y(), kStartEndpoint.z().z()), + complex_origin, kZeroTolerance))); const math::Vector3 complex_end = complex_road_curve->W_of_prh(1., 0., 0.); - EXPECT_TRUE( + EXPECT_TRUE(AssertCompare( CompareVectors(math::Vector3(complex_dut.end().xy().x(), complex_dut.end().xy().y(), complex_dut.end().z().z()), - complex_end, kVeryExact)); - EXPECT_TRUE(CompareVectors( + complex_end, kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors( math::Vector3(kEndElevatedEndpoint.xy().x(), kEndElevatedEndpoint.xy().y(), kEndElevatedEndpoint.z().z()), - complex_end, kVeryExact)); + complex_end, kVeryExact))); EXPECT_TRUE(test::IsCubicPolynomialClose( complex_road_curve->elevation(), CubicPolynomial(0., 0., -0.32476276288217043, 0.549841841921447), kVeryExact)); EXPECT_TRUE(test::IsCubicPolynomialClose(complex_road_curve->superelevation(), @@ -326,16 +328,17 @@ TEST_F(MultilaneConnectionTest, LineRoadCurveValidation) { // Checks that the road curve starts and ends at given endpoints. const math::Vector3 flat_origin = road_curve->W_of_prh(0., 0., 0.); - EXPECT_TRUE( + EXPECT_TRUE(AssertCompare( CompareVectors(math::Vector3(flat_dut.start().xy().x(), flat_dut.start().xy().y(), flat_dut.start().z().z()), - flat_origin, kZeroTolerance)); - EXPECT_TRUE(CompareVectors(math::Vector3(kStartEndpoint.xy().x(), kStartEndpoint.xy().y(), kStartEndpoint.z().z()), - flat_origin, kZeroTolerance)); + flat_origin, kZeroTolerance))); + EXPECT_TRUE(AssertCompare( + CompareVectors(math::Vector3(kStartEndpoint.xy().x(), kStartEndpoint.xy().y(), kStartEndpoint.z().z()), + flat_origin, kZeroTolerance))); const math::Vector3 flat_end = road_curve->W_of_prh(1., 0., 0.); - EXPECT_TRUE(CompareVectors(math::Vector3(flat_dut.end().xy().x(), flat_dut.end().xy().y(), flat_dut.end().z().z()), - flat_end, kVeryExact)); - EXPECT_TRUE(CompareVectors(math::Vector3(kEndEndpoint.xy().x(), kEndEndpoint.xy().y(), kEndEndpoint.z().z()), - flat_end, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors( + math::Vector3(flat_dut.end().xy().x(), flat_dut.end().xy().y(), flat_dut.end().z().z()), flat_end, kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors( + math::Vector3(kEndEndpoint.xy().x(), kEndEndpoint.xy().y(), kEndEndpoint.z().z()), flat_end, kVeryExact))); // Checks that elevation and superelevation polynomials are correctly built // for the trivial case of a flat dut. EXPECT_TRUE(test::IsCubicPolynomialClose(road_curve->elevation(), CubicPolynomial(), kZeroTolerance)); @@ -349,18 +352,19 @@ TEST_F(MultilaneConnectionTest, LineRoadCurveValidation) { // Checks that the road curve starts and ends at given endpoints. const math::Vector3 complex_origin = complex_road_curve->W_of_prh(0., 0., 0.); - EXPECT_TRUE(CompareVectors( + EXPECT_TRUE(AssertCompare(CompareVectors( math::Vector3(complex_dut.start().xy().x(), complex_dut.start().xy().y(), complex_dut.start().z().z()), - complex_origin, kZeroTolerance)); - EXPECT_TRUE(CompareVectors(math::Vector3(kStartEndpoint.xy().x(), kStartEndpoint.xy().y(), kStartEndpoint.z().z()), - complex_origin, kZeroTolerance)); + complex_origin, kZeroTolerance))); + EXPECT_TRUE(AssertCompare( + CompareVectors(math::Vector3(kStartEndpoint.xy().x(), kStartEndpoint.xy().y(), kStartEndpoint.z().z()), + complex_origin, kZeroTolerance))); const math::Vector3 complex_end = complex_road_curve->W_of_prh(1., 0., 0.); - EXPECT_TRUE( + EXPECT_TRUE(AssertCompare( CompareVectors(math::Vector3(complex_dut.end().xy().x(), complex_dut.end().xy().y(), complex_dut.end().z().z()), - complex_end, kVeryExact)); - EXPECT_TRUE(CompareVectors( + complex_end, kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors( math::Vector3(kEndElevatedEndpoint.xy().x(), kEndElevatedEndpoint.xy().y(), kEndElevatedEndpoint.z().z()), - complex_end, kVeryExact)); + complex_end, kVeryExact))); EXPECT_TRUE(test::IsCubicPolynomialClose(complex_road_curve->elevation(), CubicPolynomial(0., 0., -0.646446609406726, 0.764297739604484), kVeryExact)); EXPECT_TRUE(test::IsCubicPolynomialClose(complex_road_curve->superelevation(), diff --git a/test/maliput_multilane/multilane_lanes_test.cc b/test/maliput_multilane/multilane_lanes_test.cc index e30e304..837ef79 100644 --- a/test/maliput_multilane/multilane_lanes_test.cc +++ b/test/maliput_multilane/multilane_lanes_test.cc @@ -35,9 +35,10 @@ #include #include -#include -#include +#include +#include +#include "assert_compare.h" #include "maliput_multilane/arc_road_curve.h" #include "maliput_multilane/junction.h" #include "maliput_multilane/line_road_curve.h" @@ -49,7 +50,8 @@ namespace maliput { namespace multilane { namespace { -using maliput::math::test::CompareVectors; +using maliput::math::CompareVectors; +using maliput::multilane::test::AssertCompare; const double kLinearTolerance = 1e-6; const double kAngularTolerance = 1e-6; @@ -58,9 +60,9 @@ const double kVeryExact = 1e-12; GTEST_TEST(MultilaneLanesTest, Rot3) { // Spot-check that Rot3 is behaving as advertised. Rot3 rpy90{M_PI / 2., M_PI / 2., M_PI / 2.}; - EXPECT_TRUE(CompareVectors(rpy90.apply({1., 0., 0.}), math::Vector3(0., 0., -1.), kVeryExact)); - EXPECT_TRUE(CompareVectors(rpy90.apply({0., 1., 0.}), math::Vector3(0., 1., 0.), kVeryExact)); - EXPECT_TRUE(CompareVectors(rpy90.apply({0., 0., 1.}), math::Vector3(1., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(rpy90.apply({1., 0., 0.}), math::Vector3(0., 0., -1.), kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors(rpy90.apply({0., 1., 0.}), math::Vector3(0., 1., 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors(rpy90.apply({0., 0., 1.}), math::Vector3(1., 0., 0.), kVeryExact))); } class MultilaneLanesParamTest : public ::testing::TestWithParam { @@ -99,33 +101,35 @@ TEST_P(MultilaneLanesParamTest, FlatLineLane) { EXPECT_NEAR(l1->length(), std::sqrt((100. * 100) + (50. * 50.)), kVeryExact); + EXPECT_TRUE(AssertCompare( + api::IsRBoundsClose(l1->lane_bounds(0.), api::RBounds(-kHalfLaneWidth, kHalfLaneWidth), kVeryExact))); EXPECT_TRUE( - api::test::IsRBoundsClose(l1->lane_bounds(0.), api::RBounds(-kHalfLaneWidth, kHalfLaneWidth), kVeryExact)); - EXPECT_TRUE(api::test::IsRBoundsClose(l1->segment_bounds(0.), api::RBounds(-kHalfWidth, kHalfWidth), kVeryExact)); - EXPECT_TRUE(api::test::IsHBoundsClose(l1->elevation_bounds(0., 0.), api::HBounds(0., kMaxHeight), kVeryExact)); + AssertCompare(api::IsRBoundsClose(l1->segment_bounds(0.), api::RBounds(-kHalfWidth, kHalfWidth), kVeryExact))); + EXPECT_TRUE( + AssertCompare(api::IsHBoundsClose(l1->elevation_bounds(0., 0.), api::HBounds(0., kMaxHeight), kVeryExact))); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l1->ToInertialPosition({0., 0., 0.}), - api::InertialPosition::FromXyz(math::Vector3(100., -75., 0.) + r_offset_vector), kLinearTolerance)); + api::InertialPosition::FromXyz(math::Vector3(100., -75., 0.) + r_offset_vector), kLinearTolerance))); // A little bit along the lane, but still on the reference line. - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l1->ToInertialPosition({1., 0., 0.}), api::InertialPosition::FromXyz( math::Vector3(100. + ((100. / l1->length()) * 1.), -75. + ((50. / l1->length()) * 1.), 0.) + r_offset_vector), - kLinearTolerance)); + kLinearTolerance))); // At the very beginning of the lane, but laterally off the reference line. - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l1->ToInertialPosition({0., 3., 0.}), api::InertialPosition::FromXyz( math::Vector3(100. + ((-50. / l1->length()) * 3.), -75. + ((100. / l1->length()) * 3.), 0.) + r_offset_vector), - kLinearTolerance)); + kLinearTolerance))); // At the very end of the lane. - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l1->ToInertialPosition({l1->length(), 0., 0.}), api::InertialPosition(200. + r_offset_vector.x(), -25. + r_offset_vector.y(), 0. + r_offset_vector.z()), - kLinearTolerance)); + kLinearTolerance))); // Case 1: Tests LineLane::ToSegmentPosition() and LineLane::ToLanePosition() with a closest point that lies // within the lane bounds. const api::InertialPosition point_within_lane{150., -50., 0.}; @@ -135,18 +139,18 @@ TEST_P(MultilaneLanesParamTest, FlatLineLane) { // ToSegmentPosition api::LanePositionResult result = l1->ToSegmentPosition(point_within_lane); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.lane_position, api::LanePosition(expected_s, expected_r, 0.), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, api::InertialPosition(150., -50., 0.), - kLinearTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.lane_position, api::LanePosition(expected_s, expected_r, 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare( + api::IsInertialPositionClose(result.nearest_position, api::InertialPosition(150., -50., 0.), kLinearTolerance))); EXPECT_NEAR(result.distance, 0., kVeryExact); // ToLanePosition result = l1->ToLanePosition(point_within_lane); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.lane_position, api::LanePosition(expected_s, expected_r, 0.), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, api::InertialPosition(150., -50., 0.), - kLinearTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.lane_position, api::LanePosition(expected_s, expected_r, 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare( + api::IsInertialPositionClose(result.nearest_position, api::InertialPosition(150., -50., 0.), kLinearTolerance))); EXPECT_NEAR(result.distance, 0., kVeryExact); // Case 2: Tests LineLane::ToSegmentPosition() with a closest point that lies @@ -156,23 +160,23 @@ TEST_P(MultilaneLanesParamTest, FlatLineLane) { // ToSegmentPosition result = l1->ToSegmentPosition(point_outside_lane); - EXPECT_TRUE(api::test::IsLanePositionClose( - result.lane_position, api::LanePosition(0., expected_r_outside_segment, kMaxHeight), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(0., expected_r_outside_segment, kMaxHeight), kVeryExact))); const math::Vector3 extreme_segment_point = math::Vector3(100., -75, 0.0) + r_offset_vector + kHalfWidth * r_vector.normalized() + math::Vector3(0., 0., kMaxHeight); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, - api::InertialPosition::FromXyz(extreme_segment_point), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( + result.nearest_position, api::InertialPosition::FromXyz(extreme_segment_point), kVeryExact))); EXPECT_NEAR(result.distance, (point_outside_lane.xyz() - extreme_segment_point).norm(), kVeryExact); // ToLanePosition const double expected_r_outside_lane = kHalfLaneWidth; result = l1->ToLanePosition(point_outside_lane); - EXPECT_TRUE(api::test::IsLanePositionClose(result.lane_position, - api::LanePosition(0., expected_r_outside_lane, kMaxHeight), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(0., expected_r_outside_lane, kMaxHeight), kVeryExact))); const math::Vector3 extreme_lane_point = math::Vector3(100., -75, 0.0) + r_offset_vector + kHalfLaneWidth * r_vector.normalized() + math::Vector3(0., 0., kMaxHeight); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, - api::InertialPosition::FromXyz(extreme_lane_point), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( + result.nearest_position, api::InertialPosition::FromXyz(extreme_lane_point), kVeryExact))); EXPECT_NEAR(result.distance, (point_outside_lane.xyz() - extreme_lane_point).norm(), kVeryExact); // Case 3: Tests LineLane::ToSegmentPosition() at a non-zero but flat elevation. @@ -187,45 +191,45 @@ TEST_P(MultilaneLanesParamTest, FlatLineLane) { Lane* l1_with_z = s2->NewLane(api::LaneId{"l1_with_z"}, r0, {-kHalfLaneWidth, kHalfLaneWidth}); result = l1_with_z->ToSegmentPosition(point_outside_lane); - EXPECT_TRUE(api::test::IsLanePositionClose( - result.lane_position, api::LanePosition(0., expected_r_outside_segment, kMaxHeight), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(0., expected_r_outside_segment, kMaxHeight), kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition::FromXyz(extreme_segment_point + math::Vector3(0., 0., elevation)), - kVeryExact)); + kVeryExact))); EXPECT_NEAR(result.distance, (point_outside_lane.xyz() - extreme_segment_point - math::Vector3(0., 0., elevation)).norm(), kVeryExact); // Verifies the output of LineLane::GetOrientation(). - EXPECT_TRUE(api::test::IsRotationClose(l1->GetOrientation({0., 0., 0.}), - api::Rotation::FromRpy(0., 0., std::atan2(50., 100.)), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l1->GetOrientation({0., 0., 0.}), + api::Rotation::FromRpy(0., 0., std::atan2(50., 100.)), kVeryExact))); - EXPECT_TRUE(api::test::IsRotationClose(l1->GetOrientation({1., 0., 0.}), - api::Rotation::FromRpy(0., 0., std::atan2(50., 100.)), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l1->GetOrientation({1., 0., 0.}), + api::Rotation::FromRpy(0., 0., std::atan2(50., 100.)), kVeryExact))); - EXPECT_TRUE(api::test::IsRotationClose(l1->GetOrientation({0., 1., 0.}), - api::Rotation::FromRpy(0., 0., std::atan2(50., 100.)), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l1->GetOrientation({0., 1., 0.}), + api::Rotation::FromRpy(0., 0., std::atan2(50., 100.)), kVeryExact))); - EXPECT_TRUE(api::test::IsRotationClose(l1->GetOrientation({l1->length(), 0., 0.}), - api::Rotation::FromRpy(0., 0., std::atan2(50., 100.)), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l1->GetOrientation({l1->length(), 0., 0.}), + api::Rotation::FromRpy(0., 0., std::atan2(50., 100.)), kVeryExact))); // Derivative map should be identity (for a flat, straight road). - EXPECT_TRUE(api::test::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), - api::LanePosition(0., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), - api::LanePosition(1., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), - api::LanePosition(0., 1., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), - api::LanePosition(0., 0., 1.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {1., 1., 1.}), - api::LanePosition(1., 1., 1.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l1->EvalMotionDerivatives({0., 0., 0.}, {1., 1., 1.}), + api::LanePosition(1., 1., 1.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l1->EvalMotionDerivatives({10., 5., 3.}, {1., 2., 3.}), - api::LanePosition(1., 2., 3.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l1->EvalMotionDerivatives({10., 5., 3.}, {1., 2., 3.}), + api::LanePosition(1., 2., 3.), kVeryExact))); } namespace { @@ -374,10 +378,12 @@ TEST_P(MultilaneLanesParamTest, CorkScrewLane) { EXPECT_NEAR(l1->length(), corkscrew_curve.length(), kLinearTolerance); + EXPECT_TRUE(AssertCompare( + api::IsRBoundsClose(l1->lane_bounds(0.), api::RBounds(-kHalfLaneWidth, kHalfLaneWidth), kVeryExact))); EXPECT_TRUE( - api::test::IsRBoundsClose(l1->lane_bounds(0.), api::RBounds(-kHalfLaneWidth, kHalfLaneWidth), kVeryExact)); - EXPECT_TRUE(api::test::IsRBoundsClose(l1->segment_bounds(0.), api::RBounds(-kHalfWidth, kHalfWidth), kVeryExact)); - EXPECT_TRUE(api::test::IsHBoundsClose(l1->elevation_bounds(0., 0.), api::HBounds(0., kMaxHeight), kVeryExact)); + AssertCompare(api::IsRBoundsClose(l1->segment_bounds(0.), api::RBounds(-kHalfWidth, kHalfWidth), kVeryExact))); + EXPECT_TRUE( + AssertCompare(api::IsHBoundsClose(l1->elevation_bounds(0., 0.), api::HBounds(0., kMaxHeight), kVeryExact))); const api::IsoLaneVelocity lane_velocity(1., 10., 100.); const math::Vector3 lane_velocity_as_vector(lane_velocity.sigma_v, lane_velocity.rho_v, lane_velocity.eta_v); @@ -402,29 +408,29 @@ TEST_P(MultilaneLanesParamTest, CorkScrewLane) { // Checks position in the (x, y, z) frame i.e. world // down to kLinearTolerance accuracy (as that's the // tolerance the RoadGeometry was constructed with). - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l1->ToInertialPosition(lane_position), api::InertialPosition::FromXyz( {position_at_srh_drake.x(), position_at_srh_drake.y(), position_at_srh_drake.z()}), - kLinearTolerance)); + kLinearTolerance))); // Checks orientation in the (x, y, z) frame i.e. world // down to kAngularTolerance accuracy (as that's the // tolerance the RoadGeometry was constructed with). - EXPECT_TRUE(api::test::IsRotationClose( - l1->GetOrientation(lane_position), - api::Rotation::FromRpy( - {orientation_at_srh_drake.x(), orientation_at_srh_drake.y(), orientation_at_srh_drake.z()}), - kAngularTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsRotationClose(l1->GetOrientation(lane_position), + api::Rotation::FromRpy({orientation_at_srh_drake.x(), orientation_at_srh_drake.y(), + orientation_at_srh_drake.z()}), + kAngularTolerance))); // Checks motion derivatives in the (s, r, h) frame i.e. // lane down to kLinearTolerance accuracy (as that's // the tolerance the RoadGeometry was constructed with). - EXPECT_TRUE(api::test::IsLanePositionClose( + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( l1->EvalMotionDerivatives(lane_position, lane_velocity), api::LanePosition::FromSrh({motion_derivative_at_srh_drake.x(), motion_derivative_at_srh_drake.y(), motion_derivative_at_srh_drake.z()}), - kLinearTolerance)); + kLinearTolerance))); // TODO(hidmic): Add Lane::ToLanePosition() tests when the zero // superelevation restriction in Multilane is lifted. @@ -459,36 +465,38 @@ TEST_P(MultilaneLanesParamTest, FlatArcLane) { EXPECT_NEAR(l2->length(), offset_radius * d_theta, kVeryExact); + EXPECT_TRUE(AssertCompare( + api::IsRBoundsClose(l2->lane_bounds(0.), api::RBounds(-kHalfLaneWidth, kHalfLaneWidth), kVeryExact))); + EXPECT_TRUE( + AssertCompare(api::IsRBoundsClose(l2->segment_bounds(0.), api::RBounds(-kHalfWidth, kHalfWidth), kVeryExact))); EXPECT_TRUE( - api::test::IsRBoundsClose(l2->lane_bounds(0.), api::RBounds(-kHalfLaneWidth, kHalfLaneWidth), kVeryExact)); - EXPECT_TRUE(api::test::IsRBoundsClose(l2->segment_bounds(0.), api::RBounds(-kHalfWidth, kHalfWidth), kVeryExact)); - EXPECT_TRUE(api::test::IsHBoundsClose(l2->elevation_bounds(0., 0.), api::HBounds(0., kMaxHeight), kVeryExact)); + AssertCompare(api::IsHBoundsClose(l2->elevation_bounds(0., 0.), api::HBounds(0., kMaxHeight), kVeryExact))); // Recall that the arc has center (100, -75). const math::Vector3 inertial_center(100., -75., 0.); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l2->ToInertialPosition({0., 0., 0.}), api::InertialPosition::FromXyz(inertial_center + math::Vector3(std::cos(theta0), std::sin(theta0), 0.0) * offset_radius), - kLinearTolerance)); + kLinearTolerance))); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l2->ToInertialPosition({1., 0., 0.}), api::InertialPosition::FromXyz(inertial_center + math::Vector3(offset_radius * std::cos(theta0 + 1.0 / offset_radius), offset_radius * std::sin(theta0 + 1.0 / offset_radius), 0.0)), - kLinearTolerance)); + kLinearTolerance))); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l2->ToInertialPosition({0., 1., 0.}), api::InertialPosition::FromXyz(inertial_center + math::Vector3((offset_radius - 1.) * std::cos(theta0), (offset_radius - 1.) * std::sin(theta0), 0.0)), - kLinearTolerance)); + kLinearTolerance))); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l2->ToInertialPosition({l2->length(), 0., 0.}), api::InertialPosition::FromXyz(inertial_center + math::Vector3(offset_radius * std::cos(theta0 + d_theta), offset_radius * std::sin(theta0 + d_theta), 0.0)), - kLinearTolerance)); + kLinearTolerance))); // Case 1: Tests ArcLane::ToSegmentPosition() with a closest point that lies // within the lane bounds. @@ -496,14 +504,14 @@ TEST_P(MultilaneLanesParamTest, FlatArcLane) { const double expected_s = 0.5 * M_PI / d_theta * l2->length(); const double expected_r = std::min(offset_radius - std::sqrt(2) * 50., kHalfWidth); api::LanePositionResult result = l2->ToSegmentPosition(point_within_lane); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.lane_position, api::LanePosition(expected_s, expected_r, 0.), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.lane_position, api::LanePosition(expected_s, expected_r, 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition::FromXyz(inertial_center + math::Vector3((offset_radius - kHalfWidth) * std::cos(0.5 * M_PI + theta0), (offset_radius - kHalfWidth) * std::sin(0.5 * M_PI + theta0), 0.)), - kVeryExact)); + kVeryExact))); EXPECT_NEAR(result.distance, (offset_radius - kHalfWidth) - std::sqrt(std::pow(50., 2.) + std::pow(50., 2.)), kVeryExact); @@ -512,14 +520,14 @@ TEST_P(MultilaneLanesParamTest, FlatArcLane) { const api::InertialPosition point_outside_lane{center[0] + 200., center[1] - 20., 20.}; // θ ~= 1.9π. const double expected_r_outside = -kHalfWidth; result = l2->ToSegmentPosition(point_outside_lane); - EXPECT_TRUE(api::test::IsLanePositionClose( - result.lane_position, api::LanePosition(l2->length(), expected_r_outside, kMaxHeight), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(l2->length(), expected_r_outside, kMaxHeight), kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition::FromXyz( inertial_center + math::Vector3((offset_radius + kHalfWidth) * std::cos(theta0 + d_theta), (offset_radius + kHalfWidth) * std::sin(theta0 + d_theta), kMaxHeight)), - kVeryExact)); + kVeryExact))); EXPECT_DOUBLE_EQ(result.distance, (result.nearest_position.xyz() - point_outside_lane.xyz()).norm()); // Case 3: Tests ArcLane::ToSegmentPosition() at a non-zero but flat elevation. @@ -532,15 +540,15 @@ TEST_P(MultilaneLanesParamTest, FlatArcLane) { {0., kMaxHeight}); Lane* l2_with_z = s2->NewLane(api::LaneId{"l2_with_z"}, r0, {-kHalfLaneWidth, kHalfLaneWidth}); result = l2_with_z->ToSegmentPosition(point_outside_lane); - EXPECT_TRUE(api::test::IsLanePositionClose( - result.lane_position, api::LanePosition(l2_with_z->length(), expected_r_outside, kMaxHeight), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(l2_with_z->length(), expected_r_outside, kMaxHeight), kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition::FromXyz(inertial_center + math::Vector3((offset_radius + kHalfWidth) * std::cos(theta0 + d_theta), (offset_radius + kHalfWidth) * std::sin(theta0 + d_theta), kMaxHeight + elevation)), - kVeryExact)); + kVeryExact))); EXPECT_DOUBLE_EQ(result.distance, (result.nearest_position.xyz() - point_outside_lane.xyz()).norm()); // Case 4: Tests ArcLane::ToSegmentPosition() with a lane that overlaps itself. @@ -553,14 +561,14 @@ TEST_P(MultilaneLanesParamTest, FlatArcLane) { {0., kMaxHeight}); Lane* l2_overlapping = s3->NewLane(api::LaneId{"l2_overlapping"}, r0, {-kHalfLaneWidth, kHalfLaneWidth}); result = l2_overlapping->ToSegmentPosition(point_within_lane); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.lane_position, api::LanePosition(expected_s, expected_r, 0.), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.lane_position, api::LanePosition(expected_s, expected_r, 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition::FromXyz(inertial_center + math::Vector3((offset_radius - kHalfWidth) * std::cos(0.5 * M_PI + theta0), (offset_radius - kHalfWidth) * std::sin(0.5 * M_PI + theta0), 0.)), - kVeryExact)); + kVeryExact))); EXPECT_NEAR(result.distance, (offset_radius - kHalfWidth) - std::sqrt(std::pow(50., 2.) + std::pow(50., 2.)), kVeryExact); @@ -584,52 +592,53 @@ TEST_P(MultilaneLanesParamTest, FlatArcLane) { const double expected_s_wrap = std::abs(0.1 * M_PI / d_theta_wrap) * l2_wrap->length(); const double expected_r_wrap = 2.; result = l2_wrap->ToLanePosition(point_in_third_quadrant); - EXPECT_TRUE(api::test::IsLanePositionClose(result.lane_position, - api::LanePosition(expected_s_wrap, expected_r_wrap, 0.), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, point_in_third_quadrant, kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(expected_s_wrap, expected_r_wrap, 0.), kVeryExact))); + EXPECT_TRUE( + AssertCompare(api::IsInertialPositionClose(result.nearest_position, point_in_third_quadrant, kVeryExact))); EXPECT_NEAR(result.distance, 0. /* within lane */, kVeryExact); // Verifies the output of ArcLane::GetOrientation(). - EXPECT_TRUE(api::test::IsRotationClose(l2->GetOrientation({0., 0., 0.}), - api::Rotation::FromRpy(0., 0., (0.25 + 0.5) * M_PI), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l2->GetOrientation({0., 0., 0.}), + api::Rotation::FromRpy(0., 0., (0.25 + 0.5) * M_PI), kVeryExact))); - EXPECT_TRUE(api::test::IsRotationClose(l2->GetOrientation({0., 1., 0.}), - api::Rotation::FromRpy(0., 0., (0.25 + 0.5) * M_PI), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l2->GetOrientation({0., 1., 0.}), + api::Rotation::FromRpy(0., 0., (0.25 + 0.5) * M_PI), kVeryExact))); - EXPECT_TRUE(api::test::IsRotationClose(l2->GetOrientation({l2->length(), 0., 0.}), - api::Rotation::FromRpy(0., 0, 0.25 * M_PI), - kVeryExact)); // 0.25 + 1.5 + 0.5 + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l2->GetOrientation({l2->length(), 0., 0.}), + api::Rotation::FromRpy(0., 0, 0.25 * M_PI), + kVeryExact))); // 0.25 + 1.5 + 0.5 // For r=0, derivative map should be identity. - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), - api::LanePosition(0., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), - api::LanePosition(1., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), - api::LanePosition(0., 1., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), - api::LanePosition(0., 0., 1.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({l2->length(), 0., 0.}, {1., 1., 1.}), - api::LanePosition(1., 1., 1.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({l2->length(), 0., 0.}, {1., 1., 1.}), + api::LanePosition(1., 1., 1.), kVeryExact))); // For a left-turning curve, r = +10 will decrease the radius of the path // from the original 100 down to 90. - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 10., 0.}, {1., 1., 1.}), - api::LanePosition((offset_radius / (offset_radius - 10.0)) * 1., 1., 1.), - kVeryExact)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 10., 0.}, {1., 1., 1.}), + api::LanePosition((offset_radius / (offset_radius - 10.0)) * 1., 1., 1.), kVeryExact))); // Likewise, r = -10 will increase the radius of the path from the // original 100 up to 110. - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., -10., 0.}, {1., 1., 1.}), - api::LanePosition(offset_radius / (offset_radius + 10.) * 1., 1., 1.), - kVeryExact)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(l2->EvalMotionDerivatives({0., -10., 0.}, {1., 1., 1.}), + api::LanePosition(offset_radius / (offset_radius + 10.) * 1., 1., 1.), kVeryExact))); // ...and only r should matter for an otherwise flat arc. - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({l2->length(), -10., 100.}, {1., 1., 1.}), - api::LanePosition(offset_radius / (offset_radius + 10.), 1., 1.), - kVeryExact)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(l2->EvalMotionDerivatives({l2->length(), -10., 100.}, {1., 1., 1.}), + api::LanePosition(offset_radius / (offset_radius + 10.), 1., 1.), kVeryExact))); } api::LanePosition IntegrateTrivially(const api::Lane* lane, const api::LanePosition& lp_initial, @@ -680,16 +689,16 @@ TEST_P(MultilaneLanesParamTest, HillIntegration) { // Checks center lane endpoints' position in the world frame // against their analytically known values. const api::LanePosition kInitialLanePosition{0., 0., 0.}; - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( center_lane->ToInertialPosition(kInitialLanePosition), api::InertialPosition(-100. + (offset_radius * std::cos(theta0)), -100. + (offset_radius * std::sin(theta0)), z0), - kLinearTolerance)); + kLinearTolerance))); const api::LanePosition kFinalLanePosition{center_lane->length(), 0., 0.}; - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( center_lane->ToInertialPosition(kFinalLanePosition), api::InertialPosition(-100. + (offset_radius * std::cos(theta1)), -100. + (offset_radius * std::sin(theta1)), z1), - kLinearTolerance)); + kLinearTolerance))); // Checks EvalMotionDerivatives() accuracy. To that end, motion derivatives // are (1) queried for a given constant velocity σᵥ from the center lane at an @@ -707,17 +716,17 @@ TEST_P(MultilaneLanesParamTest, HillIntegration) { const int kStepCountR = right_lane->length() / (kVelocity.sigma_v * kTimeStep); const api::LanePosition kInitialLanePositionR{0., -kLaneSpacing, 0.}; const api::LanePosition kExpectedFinalLanePositionR{center_lane->length(), -kLaneSpacing, 0.}; - EXPECT_TRUE(api::test::IsLanePositionClose( + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( IntegrateTrivially(center_lane, kInitialLanePositionR, kVelocity, kTimeStep, kStepCountR), - kExpectedFinalLanePositionR, kIntegrationTolerance)); + kExpectedFinalLanePositionR, kIntegrationTolerance))); // Tests using the lane on the left side. const int kStepCountL = left_lane->length() / (kVelocity.sigma_v * kTimeStep); const api::LanePosition kInitialLanePositionL{0., kLaneSpacing, 0.}; const api::LanePosition kExpectedFinalLanePositionL{center_lane->length(), kLaneSpacing, 0.}; - EXPECT_TRUE(api::test::IsLanePositionClose( + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( IntegrateTrivially(center_lane, kInitialLanePositionL, kVelocity, kTimeStep, kStepCountL), - kExpectedFinalLanePositionL, kIntegrationTolerance)); + kExpectedFinalLanePositionL, kIntegrationTolerance))); } INSTANTIATE_TEST_CASE_P(Offset, MultilaneLanesParamTest, testing::Values(0., 5., -5.)); @@ -745,65 +754,66 @@ GTEST_TEST(MultilaneLanesTest, ArcLaneWithConstantSuperelevation) { EXPECT_NEAR(l2->length(), 100. * 1.5 * M_PI, kVeryExact); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l2->ToInertialPosition({0., 0., 0.}), api::InertialPosition(100. + (100. * std::cos(0.25 * M_PI)), -75. + (100. * std::sin(0.25 * M_PI)), 0.), - kLinearTolerance)); + kLinearTolerance))); // NB: (1.25 * M_PI) is the direction of the r-axis at s = 0. - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( l2->ToInertialPosition({0., 10., 0.}), api::InertialPosition(100. + (100. * std::cos(0.25 * M_PI)) + (10. * std::cos(kTheta) * std::cos(1.25 * M_PI)), -75. + (100. * std::sin(0.25 * M_PI)) + (10. * std::cos(kTheta) * std::sin(1.25 * M_PI)), 10. * std::sin(kTheta)), - kLinearTolerance)); + kLinearTolerance))); // TODO(maddog@tri.global) Test ToLanePosition(). - EXPECT_TRUE(api::test::IsRotationClose(l2->GetOrientation({0., 0., 0.}), - api::Rotation::FromRpy(kTheta, 0., (0.25 + 0.5) * M_PI), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l2->GetOrientation({0., 0., 0.}), + api::Rotation::FromRpy(kTheta, 0., (0.25 + 0.5) * M_PI), kVeryExact))); - EXPECT_TRUE(api::test::IsRotationClose(l2->GetOrientation({0., 1., 0.}), - api::Rotation::FromRpy(kTheta, 0., (0.25 + 0.5) * M_PI), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l2->GetOrientation({0., 1., 0.}), + api::Rotation::FromRpy(kTheta, 0., (0.25 + 0.5) * M_PI), kVeryExact))); - EXPECT_TRUE(api::test::IsRotationClose(l2->GetOrientation({l2->length(), 0., 0.}), - api::Rotation::FromRpy(kTheta, 0., 0.25 * M_PI), - kVeryExact)); // 0.25 + 1.5 + 0.5 + EXPECT_TRUE(AssertCompare(api::IsRotationClose(l2->GetOrientation({l2->length(), 0., 0.}), + api::Rotation::FromRpy(kTheta, 0., 0.25 * M_PI), + kVeryExact))); // 0.25 + 1.5 + 0.5 api::LanePosition pdot; // For r=0, derivative map should be identity. - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), - api::LanePosition(0., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), - api::LanePosition(1., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), - api::LanePosition(0., 1., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), - api::LanePosition(0., 0., 1.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({l2->length(), 0., 0.}, {1., 1., 1.}), - api::LanePosition(1., 1., 1.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l2->EvalMotionDerivatives({l2->length(), 0., 0.}, {1., 1., 1.}), + api::LanePosition(1., 1., 1.), kVeryExact))); // For a left-turning curve, r = +10 will decrease the radius of the path // from the original 100 down to almost 90. (r is scaled by the cosine of // the superelevation since it is no longer horizontal). - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., 10., 0.}, {1., 1., 1.}), - api::LanePosition((100. / (100. - (10. * std::cos(kTheta)))) * 1., 1., 1.), - kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + l2->EvalMotionDerivatives({0., 10., 0.}, {1., 1., 1.}), + api::LanePosition((100. / (100. - (10. * std::cos(kTheta)))) * 1., 1., 1.), kVeryExact))); // Likewise, r = -10 will increase the radius of the path from the // original 100 up to almost 110 (since r is no longer horizontal). - EXPECT_TRUE(api::test::IsLanePositionClose(l2->EvalMotionDerivatives({0., -10., 0.}, {1., 1., 1.}), - api::LanePosition((100. / (100 + (10. * std::cos(kTheta)))) * 1., 1., 1.), - kVeryExact)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(l2->EvalMotionDerivatives({0., -10., 0.}, {1., 1., 1.}), + api::LanePosition((100. / (100 + (10. * std::cos(kTheta)))) * 1., 1., 1.), kVeryExact))); // h matters, too (because hovering above a tilted road changes one's // distance to the center of the arc). - EXPECT_TRUE(api::test::IsLanePositionClose( + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( l2->EvalMotionDerivatives({l2->length(), -10., 8.}, {1., 1., 1.}), - api::LanePosition((100. / (100 + (10. * std::cos(kTheta)) + (8. * std::sin(kTheta)))) * 1., 1., 1.), kVeryExact)); + api::LanePosition((100. / (100 + (10. * std::cos(kTheta)) + (8. * std::sin(kTheta)))) * 1., 1., 1.), + kVeryExact))); } class MultilaneMultipleLanesTest : public ::testing::Test { @@ -861,13 +871,13 @@ TEST_F(MultilaneMultipleLanesTest, MultipleLineLanes) { for (const Lane* lane : lanes) { for (const double p : p_vector) { for (const double r : r_offset_vector) { - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( lane->ToInertialPosition({p * lane_length, r, 0.}), api::InertialPosition::FromXyz(math::Vector3(100., -75., 0.) + (p * lane_length) * s_vector + (kR0 + lane_spacing + r) * r_vector), - kVeryExact)); - EXPECT_TRUE(api::test::IsRotationClose(lane->GetOrientation({p * lane_length, r, 0.}), - api::Rotation::FromRpy(0., 0., theta), kVeryExact)); + kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsRotationClose(lane->GetOrientation({p * lane_length, r, 0.}), + api::Rotation::FromRpy(0., 0., theta), kVeryExact))); } } lane_spacing += kRSpacing; @@ -883,9 +893,9 @@ TEST_F(MultilaneMultipleLanesTest, MultipleLineLanes) { math::Vector3(100., -75., 0.) + (p * lane_length) * s_vector + (kR0 + lane_spacing + r) * r_vector); const api::LanePositionResult result = lane->ToSegmentPosition(inertial_point); - EXPECT_TRUE(api::test::IsLanePositionClose(result.lane_position, api::LanePosition(p * lane_length, r, 0.), - kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, inertial_point, kVeryExact)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.lane_position, api::LanePosition(p * lane_length, r, 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(result.nearest_position, inertial_point, kVeryExact))); EXPECT_NEAR(result.distance, 0., kVeryExact); } } @@ -901,33 +911,33 @@ TEST_F(MultilaneMultipleLanesTest, MultipleLineLanes) { api::InertialPosition::FromXyz(math::Vector3(100., -75., 0.) + (p * lane_length) * s_vector); const double expected_r = lane->segment_bounds(0.).min(); const api::LanePositionResult result = lane->ToSegmentPosition(inertial_point); - EXPECT_TRUE(api::test::IsLanePositionClose(result.lane_position, - api::LanePosition(p * lane_length, expected_r, 0.), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose( - result.nearest_position, - api::InertialPosition::FromXyz(math::Vector3(100., -75., 0.) + (p * lane_length) * s_vector + - kRMin * r_vector), - kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(p * lane_length, expected_r, 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare( + api::IsInertialPositionClose(result.nearest_position, + api::InertialPosition::FromXyz(math::Vector3(100., -75., 0.) + + (p * lane_length) * s_vector + kRMin * r_vector), + kVeryExact))); EXPECT_NEAR(result.distance, kRMin, kVeryExact); } } // Derivative map should be identity (for a flat, straight road). for (const Lane* lane : lanes) { - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), - api::LanePosition(0., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), - api::LanePosition(1., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), - api::LanePosition(0., 1., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), - api::LanePosition(0., 0., 1.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(l1->EvalMotionDerivatives({10., 5., 3.}, {1., 2., 3.}), - api::LanePosition(1., 2., 3.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(l1->EvalMotionDerivatives({10., 5., 3.}, {1., 2., 3.}), + api::LanePosition(1., 2., 3.), kVeryExact))); } } @@ -983,15 +993,15 @@ TEST_F(MultilaneMultipleLanesTest, MultipleArcLanes) { for (const double r : r_offset_vector) { const double effective_radius = radius - r; const double effective_angle = p * kDTheta + kTheta0; - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( lane->ToInertialPosition({p * radius * kDTheta, r, 0.}), api::InertialPosition::FromXyz(kGeoCenter + effective_radius * math::Vector3(std::cos(effective_angle), std::sin(effective_angle), 0.)), - kVeryExact)); - EXPECT_TRUE(api::test::IsRotationClose( + kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsRotationClose( lane->GetOrientation({p * radius * kDTheta, r, 0.}), - api::Rotation::FromRpy(0., 0., wrap_angle(effective_angle + (0.5 * M_PI))), kVeryExact)); + api::Rotation::FromRpy(0., 0., wrap_angle(effective_angle + (0.5 * M_PI))), kVeryExact))); } } } @@ -1008,9 +1018,9 @@ TEST_F(MultilaneMultipleLanesTest, MultipleArcLanes) { const api::InertialPosition inertial_point = api::InertialPosition::FromXyz( kGeoCenter + effective_radius * math::Vector3(std::cos(effective_angle), std::sin(effective_angle), 0.)); const api::LanePositionResult result = lane->ToSegmentPosition(inertial_point); - EXPECT_TRUE(api::test::IsLanePositionClose(result.lane_position, api::LanePosition(p * radius * kDTheta, r, 0.), - kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, inertial_point, kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(p * radius * kDTheta, r, 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(result.nearest_position, inertial_point, kVeryExact))); EXPECT_NEAR(result.distance, 0., kVeryExact); } } @@ -1028,13 +1038,13 @@ TEST_F(MultilaneMultipleLanesTest, MultipleArcLanes) { kGeoCenter + kRadius * math::Vector3(std::cos(effective_angle), std::sin(effective_angle), 0.)); const double expected_r = lane->segment_bounds(0.).min(); const api::LanePositionResult result = lane->ToSegmentPosition(inertial_point); - EXPECT_TRUE(api::test::IsLanePositionClose(result.lane_position, - api::LanePosition(p * radius * kDTheta, expected_r, 0.), kVeryExact)); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.lane_position, api::LanePosition(p * radius * kDTheta, expected_r, 0.), kVeryExact))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition::FromXyz( kGeoCenter + (kRadius - kRMin) * math::Vector3(std::cos(effective_angle), std::sin(effective_angle), 0.)), - kVeryExact)); + kVeryExact))); EXPECT_NEAR(result.distance, kRMin, kVeryExact); } } @@ -1045,27 +1055,30 @@ TEST_F(MultilaneMultipleLanesTest, MultipleArcLanes) { const double radius = lane_radius[i]; // For r=0, derivative map should be identity. - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), - api::LanePosition(0., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 0.}), + api::LanePosition(0., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), - api::LanePosition(1., 0., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {1., 0., 0.}), + api::LanePosition(1., 0., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), - api::LanePosition(0., 1., 0.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 1., 0.}), + api::LanePosition(0., 1., 0.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), - api::LanePosition(0., 0., 1.), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., 0., 0.}, {0., 0., 1.}), + api::LanePosition(0., 0., 1.), kVeryExact))); - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({lane->length(), 0., 0.}, {1., 1., 1.}), - api::LanePosition(1., 1., 1.), kVeryExact)); + EXPECT_TRUE( + AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({lane->length(), 0., 0.}, {1., 1., 1.}), + api::LanePosition(1., 1., 1.), kVeryExact))); // Checks motion derivatives at different offsets from Lane's centerline. for (const double r : r_offset_vector) { - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({0., r, 0.}, {1., 1., 1.}), - api::LanePosition((radius / (radius - r)) * 1., 1., 1.), kVeryExact)); - EXPECT_TRUE(api::test::IsLanePositionClose(lane->EvalMotionDerivatives({lane->length(), r, 100.}, {1., 1., 1.}), - api::LanePosition(radius / (radius - r), 1., 1.), kVeryExact)); + EXPECT_TRUE( + AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({0., r, 0.}, {1., 1., 1.}), + api::LanePosition((radius / (radius - r)) * 1., 1., 1.), kVeryExact))); + EXPECT_TRUE( + AssertCompare(api::IsLanePositionClose(lane->EvalMotionDerivatives({lane->length(), r, 100.}, {1., 1., 1.}), + api::LanePosition(radius / (radius - r), 1., 1.), kVeryExact))); } } } diff --git a/test/maliput_multilane/multilane_line_road_curve_test.cc b/test/maliput_multilane/multilane_line_road_curve_test.cc index fb5f30b..13ae0c6 100644 --- a/test/maliput_multilane/multilane_line_road_curve_test.cc +++ b/test/maliput_multilane/multilane_line_road_curve_test.cc @@ -34,16 +34,20 @@ #include #include +#include #include #include +#include #include -#include + +#include "assert_compare.h" namespace maliput { namespace multilane { namespace { -using maliput::math::test::CompareVectors; +using maliput::math::CompareVectors; +using maliput::multilane::test::AssertCompare; class MultilaneLineRoadCurveTest : public ::testing::Test { protected: @@ -91,13 +95,13 @@ TEST_F(MultilaneLineRoadCurveTest, LineRoadCurve) { EXPECT_THROW(p_from_s_at_r0(2. * offset_line_length), maliput::common::assertion_error); // Check the evaluation of xy at different p values. - EXPECT_TRUE(CompareVectors(dut.xy_of_p(0.0), kOrigin, kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.xy_of_p(0.5), kOrigin + 0.5 * kDirection, kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.xy_of_p(1.0), kOrigin + kDirection, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(dut.xy_of_p(0.0), kOrigin, kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors(dut.xy_of_p(0.5), kOrigin + 0.5 * kDirection, kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors(dut.xy_of_p(1.0), kOrigin + kDirection, kVeryExact))); // Check the derivative of xy with respect to p at different p values. - EXPECT_TRUE(CompareVectors(dut.xy_dot_of_p(0.0), kDirection, kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.xy_dot_of_p(0.5), kDirection, kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.xy_dot_of_p(1.0), kDirection, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(dut.xy_dot_of_p(0.0), kDirection, kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors(dut.xy_dot_of_p(0.5), kDirection, kVeryExact))); + EXPECT_TRUE(AssertCompare(CompareVectors(dut.xy_dot_of_p(1.0), kDirection, kVeryExact))); // Check the heading at different p values. EXPECT_NEAR(dut.heading_of_p(0.0), kHeading, kVeryExact); EXPECT_NEAR(dut.heading_of_p(0.5), kHeading, kVeryExact); @@ -136,17 +140,22 @@ TEST_F(MultilaneLineRoadCurveTest, IsValidTest) { TEST_F(MultilaneLineRoadCurveTest, ToCurveFrameTest) { const LineRoadCurve dut(kOrigin, kDirection, zp, zp, kLinearTolerance, kScaleLength, kComputationPolicy); // Checks over the base line. - EXPECT_TRUE(CompareVectors(dut.ToCurveFrame(math::Vector3(10.0, 10.0, 0.0), kRMin, kRMax, elevation_bounds), - math::Vector3(0.0, 0.0, 0.0), kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.ToCurveFrame(math::Vector3(20.0, 20.0, 0.0), kRMin, kRMax, elevation_bounds), - math::Vector3(1., 0.0, 0.0), kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.ToCurveFrame(math::Vector3(15.0, 15.0, 0.0), kRMin, kRMax, elevation_bounds), - math::Vector3(0.5, 0.0, 0.0), kVeryExact)); + EXPECT_TRUE( + AssertCompare(CompareVectors(dut.ToCurveFrame(math::Vector3(10.0, 10.0, 0.0), kRMin, kRMax, elevation_bounds), + math::Vector3(0.0, 0.0, 0.0), kVeryExact))); + EXPECT_TRUE( + AssertCompare(CompareVectors(dut.ToCurveFrame(math::Vector3(20.0, 20.0, 0.0), kRMin, kRMax, elevation_bounds), + math::Vector3(1., 0.0, 0.0), kVeryExact))); + EXPECT_TRUE( + AssertCompare(CompareVectors(dut.ToCurveFrame(math::Vector3(15.0, 15.0, 0.0), kRMin, kRMax, elevation_bounds), + math::Vector3(0.5, 0.0, 0.0), kVeryExact))); // Check with lateral and vertical deviation. - EXPECT_TRUE(CompareVectors(dut.ToCurveFrame(math::Vector3(11.0, 12.0, 5.0), kRMin, kRMax, elevation_bounds), - math::Vector3(0.15, 0.707106781186547, 5.0), kVeryExact)); - EXPECT_TRUE(CompareVectors(dut.ToCurveFrame(math::Vector3(11.0, 10.0, 7.0), kRMin, kRMax, elevation_bounds), - math::Vector3(0.05, -0.707106781186547, 7.0), kVeryExact)); + EXPECT_TRUE( + AssertCompare(CompareVectors(dut.ToCurveFrame(math::Vector3(11.0, 12.0, 5.0), kRMin, kRMax, elevation_bounds), + math::Vector3(0.15, 0.707106781186547, 5.0), kVeryExact))); + EXPECT_TRUE( + AssertCompare(CompareVectors(dut.ToCurveFrame(math::Vector3(11.0, 10.0, 7.0), kRMin, kRMax, elevation_bounds), + math::Vector3(0.05, -0.707106781186547, 7.0), kVeryExact))); } // Checks that l_max(), p_from_s() and s_from_p() with constant @@ -209,7 +218,7 @@ TEST_F(MultilaneLineRoadCurveTest, WorldFunction) { for (double h : h_vector) { const math::Vector3 geo_position = kGeoOrigin + p * kDirection.norm() * p_versor + flat_rotation.apply({0., r, h}); - EXPECT_TRUE(CompareVectors(flat_dut.W_of_prh(p, r, h), geo_position, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(flat_dut.W_of_prh(p, r, h), geo_position, kVeryExact))); } } } @@ -228,7 +237,7 @@ TEST_F(MultilaneLineRoadCurveTest, WorldFunction) { for (double h : h_vector) { const math::Vector3 geo_position = kGeoOrigin + p * kDirection.norm() * p_versor + linear_elevation.f_p(p) * z_vector + elevated_rotation.apply({0., r, h}); - EXPECT_TRUE(CompareVectors(elevated_dut.W_of_prh(p, r, h), geo_position, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(elevated_dut.W_of_prh(p, r, h), geo_position, kVeryExact))); } } } @@ -244,7 +253,7 @@ TEST_F(MultilaneLineRoadCurveTest, WorldFunction) { for (double h : h_vector) { const math::Vector3 geo_position = kGeoOrigin + p * kDirection.norm() * p_versor + superelevated_rotation.apply({0., r, h}); - EXPECT_TRUE(CompareVectors(superelevated_dut.W_of_prh(p, r, h), geo_position, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(superelevated_dut.W_of_prh(p, r, h), geo_position, kVeryExact))); } } } @@ -281,9 +290,9 @@ TEST_F(MultilaneLineRoadCurveTest, WorldFunctionDerivative) { const double g_prime = flat_dut.elevation().f_dot_p(p); const math::Vector3 w_prime = flat_dut.W_prime_of_prh(p, r, h, rotation, g_prime); const math::Vector3 numeric_w_prime = numeric_w_prime_of_prh(flat_dut, p, r, h); - EXPECT_TRUE(CompareVectors(w_prime, numeric_w_prime, kQuiteExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime, numeric_w_prime, kQuiteExact))); const math::Vector3 s_hat = flat_dut.s_hat_of_prh(p, r, h, rotation, g_prime); - EXPECT_TRUE(CompareVectors(w_prime.normalized(), s_hat, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime.normalized(), s_hat, kVeryExact))); } } } @@ -302,9 +311,9 @@ TEST_F(MultilaneLineRoadCurveTest, WorldFunctionDerivative) { const double g_prime = elevated_dut.elevation().f_dot_p(p); const math::Vector3 w_prime = elevated_dut.W_prime_of_prh(p, r, h, rotation, g_prime); const math::Vector3 numeric_w_prime = numeric_w_prime_of_prh(elevated_dut, p, r, h); - EXPECT_TRUE(CompareVectors(w_prime, numeric_w_prime, kQuiteExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime, numeric_w_prime, kQuiteExact))); const math::Vector3 s_hat = elevated_dut.s_hat_of_prh(p, r, h, rotation, g_prime); - EXPECT_TRUE(CompareVectors(w_prime.normalized(), s_hat, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime.normalized(), s_hat, kVeryExact))); } } } @@ -321,9 +330,9 @@ TEST_F(MultilaneLineRoadCurveTest, WorldFunctionDerivative) { const double g_prime = superelevated_dut.elevation().f_dot_p(p); const math::Vector3 w_prime = superelevated_dut.W_prime_of_prh(p, r, h, rotation, g_prime); const math::Vector3 numeric_w_prime = numeric_w_prime_of_prh(superelevated_dut, p, r, h); - EXPECT_TRUE(CompareVectors(w_prime, numeric_w_prime, kQuiteExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime, numeric_w_prime, kQuiteExact))); const math::Vector3 s_hat = superelevated_dut.s_hat_of_prh(p, r, h, rotation, g_prime); - EXPECT_TRUE(CompareVectors(w_prime.normalized(), s_hat, kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(w_prime.normalized(), s_hat, kVeryExact))); } } } @@ -344,7 +353,7 @@ TEST_F(MultilaneLineRoadCurveTest, ReferenceCurveRotation) { EXPECT_NEAR(rotation.pitch(), kZeroPitch, kVeryExact); EXPECT_NEAR(rotation.yaw(), kHeading, kVeryExact); const math::Vector3 r_versor = flat_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_versor, kFlatRDirection.normalized(), kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(r_versor, kFlatRDirection.normalized(), kVeryExact))); } // Checks for a linearly elevated curve. @@ -361,7 +370,7 @@ TEST_F(MultilaneLineRoadCurveTest, ReferenceCurveRotation) { EXPECT_NEAR(rotation.pitch(), kLinearPitch, kVeryExact); EXPECT_NEAR(rotation.yaw(), kHeading, kVeryExact); const math::Vector3 r_versor = elevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_versor, kElevatedRDirection.normalized(), kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(r_versor, kElevatedRDirection.normalized(), kVeryExact))); } // Checks for a curve with constant non zero superelevation. @@ -376,7 +385,7 @@ TEST_F(MultilaneLineRoadCurveTest, ReferenceCurveRotation) { EXPECT_NEAR(rotation.pitch(), kZeroPitch, kVeryExact); EXPECT_NEAR(rotation.yaw(), kHeading, kVeryExact); const math::Vector3 r_versor = superelevated_dut.r_hat_of_Rabg(rotation); - EXPECT_TRUE(CompareVectors(r_versor, kSuperelevatedRDirection.normalized(), kVeryExact)); + EXPECT_TRUE(AssertCompare(CompareVectors(r_versor, kSuperelevatedRDirection.normalized(), kVeryExact))); } } diff --git a/test/maliput_multilane/multilane_loader_test.cc b/test/maliput_multilane/multilane_loader_test.cc index 8322c26..30aa350 100644 --- a/test/maliput_multilane/multilane_loader_test.cc +++ b/test/maliput_multilane/multilane_loader_test.cc @@ -46,7 +46,6 @@ #include #include #include -#include #include "maliput_multilane/builder.h" #include "maliput_multilane/connection.h" diff --git a/test/maliput_multilane/multilane_road_geometry_test.cc b/test/maliput_multilane/multilane_road_geometry_test.cc index 6c0e5fd..71c9668 100644 --- a/test/maliput_multilane/multilane_road_geometry_test.cc +++ b/test/maliput_multilane/multilane_road_geometry_test.cc @@ -37,9 +37,10 @@ #include #include +#include #include -#include +#include "assert_compare.h" #include "maliput_multilane/builder.h" namespace maliput { @@ -50,6 +51,7 @@ using api::HBounds; using api::RBounds; using multilane::ArcOffset; using Which = api::LaneEnd::Which; +using test::AssertCompare; const double kVeryExact{1e-11}; const double kWidth{2.}; // Half lane width, used to feed the BuilderFactory. @@ -136,13 +138,13 @@ TEST_F(MultilaneLanesQueriesTest, DoToRoadPosition) { api::RoadPositionResult result = rg_->ToRoadPosition(inertial_pos); // Expect to locate the point centered within lane1 (straight segment). - EXPECT_TRUE(api::test::IsLanePositionClose( - result.road_position.pos, api::LanePosition(kLength / 2. /* s */, 0. /* r */, 0. /* h */), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.road_position.pos, api::LanePosition(kLength / 2. /* s */, 0. /* r */, 0. /* h */), kVeryExact))); EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane1_0")); EXPECT_EQ(result.distance, 0.); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition(inertial_pos.x(), inertial_pos.y(), inertial_pos.z()), - kVeryExact)); + kVeryExact))); // Place a point halfway to the end of lane1, just to the outside (left side) // of the lane bounds. @@ -151,51 +153,52 @@ TEST_F(MultilaneLanesQueriesTest, DoToRoadPosition) { // Expect to locate the point just outside (to the left) of lane1, by an // amount kWidth. - EXPECT_TRUE(api::test::IsLanePositionClose( - result.road_position.pos, api::LanePosition(kLength / 2. /* s */, kWidth /* r */, 0. /* h */), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.road_position.pos, api::LanePosition(kLength / 2. /* s */, kWidth /* r */, 0. /* h */), kVeryExact))); EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane1_0")); EXPECT_EQ(result.distance, kWidth); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition(inertial_pos.x() - kWidth, inertial_pos.y(), inertial_pos.z()), - kVeryExact)); + kVeryExact))); // Place a point at the middle of lane3a (straight segment). inertial_pos = api::InertialPosition(2. * kArcRadius + kLength / 2., -2. * kArcRadius - kLength, 0.); result = rg_->ToRoadPosition(inertial_pos); // Expect to locate the point centered within lane3a. - EXPECT_TRUE(api::test::IsLanePositionClose( - result.road_position.pos, api::LanePosition(kLength / 2. /* s */, 0. /* r */, 0. /* h */), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.road_position.pos, api::LanePosition(kLength / 2. /* s */, 0. /* r */, 0. /* h */), kVeryExact))); EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane3a_0")); EXPECT_EQ(result.distance, 0.); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition(inertial_pos.x(), inertial_pos.y(), inertial_pos.z()), - kVeryExact)); + kVeryExact))); // Place a point high above the middle of lane3a (straight segment). inertial_pos = api::InertialPosition(2. * kArcRadius + kLength / 2., -2. * kArcRadius - kLength, 50.); result = rg_->ToRoadPosition(inertial_pos); // Expect to locate the point centered above lane3a. - EXPECT_TRUE(api::test::IsLanePositionClose( - result.road_position.pos, api::LanePosition(kLength / 2. /* s */, 0. /* r */, kHeight /* h */), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.road_position.pos, api::LanePosition(kLength / 2. /* s */, 0. /* r */, kHeight /* h */), kVeryExact))); EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane3a_0")); EXPECT_EQ(result.distance, 50. - kHeight); - EXPECT_TRUE(api::test::IsInertialPositionClose( - result.nearest_position, api::InertialPosition(inertial_pos.x(), inertial_pos.y(), kHeight), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( + result.nearest_position, api::InertialPosition(inertial_pos.x(), inertial_pos.y(), kHeight), kVeryExact))); // Place a point at the end of lane3b (arc segment). inertial_pos = api::InertialPosition(2. * kArcRadius + kLength, -kArcRadius - kLength, 0.); result = rg_->ToRoadPosition(inertial_pos); // Expect to locate the point at the end of lane3b. - EXPECT_TRUE(api::test::IsLanePositionClose( - result.road_position.pos, api::LanePosition(kArcRadius * M_PI / 2. /* s */, 0. /* r */, 0. /* h */), kVeryExact)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.road_position.pos, + api::LanePosition(kArcRadius * M_PI / 2. /* s */, 0. /* r */, 0. /* h */), kVeryExact))); EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane3b_0")); EXPECT_EQ(result.distance, 0.); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition(inertial_pos.x(), inertial_pos.y(), inertial_pos.z()), - kVeryExact)); + kVeryExact))); // Supply a hint with a position at the start of lane3c to try and determine // the RoadPosition for a point at the end of lane3b. @@ -216,9 +219,9 @@ TEST_F(MultilaneLanesQueriesTest, DoToRoadPosition) { // within lane lane3b. EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane3b_0")); EXPECT_EQ(result.distance, 0.); // inertial_pos is inside lane3b. - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition(inertial_pos.x(), inertial_pos.y(), inertial_pos.z()), - kVeryExact)); + kVeryExact))); } TEST_F(MultilaneLanesQueriesTest, DoToFindRoadPositions) { @@ -232,13 +235,13 @@ TEST_F(MultilaneLanesQueriesTest, DoToFindRoadPositions) { api::RoadPositionResult result = results[0]; // Expect to locate the point centered within lane1 (straight segment). - EXPECT_TRUE(api::test::IsLanePositionClose( - result.road_position.pos, api::LanePosition(kLength / 2. /* s */, 0. /* r */, 0. /* h */), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.road_position.pos, api::LanePosition(kLength / 2. /* s */, 0. /* r */, 0. /* h */), kVeryExact))); EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane1_0")); EXPECT_EQ(result.distance, 0.); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, api::InertialPosition(inertial_pos.x(), inertial_pos.y(), inertial_pos.z()), - kVeryExact)); + kVeryExact))); // Place the point at the end of lane1. Lane1 and lane2 should be found. inertial_pos = {kArcRadius, -kArcRadius - kLength, 0.}; @@ -259,13 +262,13 @@ TEST_F(MultilaneLanesQueriesTest, DoToFindRoadPositions) { // Expect to locate the point at the end of lane1. const api::InertialPosition inertial_pos_lane_1{kArcRadius, -kArcRadius - kLength, 0.}; - EXPECT_TRUE(api::test::IsLanePositionClose(result.road_position.pos, - api::LanePosition(kLength /* s */, 0. /* r */, 0. /* h */), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.road_position.pos, api::LanePosition(kLength /* s */, 0. /* r */, 0. /* h */), kVeryExact))); EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane1_0")); EXPECT_EQ(result.distance, 0.); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, - api::InertialPosition(inertial_pos_lane_1.x(), inertial_pos_lane_1.y(), inertial_pos_lane_1.z()), kVeryExact)); + api::InertialPosition(inertial_pos_lane_1.x(), inertial_pos_lane_1.y(), inertial_pos_lane_1.z()), kVeryExact))); // Checking l:lane2_0 result. auto lane_2_0_itr = find_lane_in_results(api::LaneId("l:lane2_0"), results); @@ -274,13 +277,13 @@ TEST_F(MultilaneLanesQueriesTest, DoToFindRoadPositions) { // Expect to locate the point at the end of lane1. const api::InertialPosition inertial_pos_lane_2{kArcRadius, -kArcRadius - kLength, 0.}; - EXPECT_TRUE(api::test::IsLanePositionClose(result.road_position.pos, - api::LanePosition(0. /* s */, 0. /* r */, 0. /* h */), kVeryExact)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.road_position.pos, api::LanePosition(0. /* s */, 0. /* r */, 0. /* h */), kVeryExact))); EXPECT_EQ(result.road_position.lane->id(), api::LaneId("l:lane2_0")); EXPECT_EQ(result.distance, 0.); - EXPECT_TRUE(api::test::IsInertialPositionClose( + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose( result.nearest_position, - api::InertialPosition(inertial_pos_lane_2.x(), inertial_pos_lane_2.y(), inertial_pos_lane_2.z()), kVeryExact)); + api::InertialPosition(inertial_pos_lane_2.x(), inertial_pos_lane_2.y(), inertial_pos_lane_2.z()), kVeryExact))); } GTEST_TEST(MultilaneLanesTest, HintWithDisconnectedLanes) { @@ -424,10 +427,10 @@ GTEST_TEST(MultilaneLanesTest, MultipleLineLaneSegmentWithoutHint) { for (const auto truth_value : truth_vector) { EXPECT_NO_THROW(result = rg->ToRoadPosition(std::get<0>(truth_value))); EXPECT_EQ(result.road_position.lane->id(), std::get<1>(truth_value).lane->id()); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.road_position.pos, std::get<1>(truth_value).pos, kLinearTolerance)); - EXPECT_TRUE( - api::test::IsInertialPositionClose(result.nearest_position, std::get<2>(truth_value), kLinearTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.road_position.pos, std::get<1>(truth_value).pos, kLinearTolerance))); + EXPECT_TRUE(AssertCompare( + api::IsInertialPositionClose(result.nearest_position, std::get<2>(truth_value), kLinearTolerance))); EXPECT_NEAR(result.distance, std::get<4>(truth_value), kLinearTolerance); } @@ -435,10 +438,10 @@ GTEST_TEST(MultilaneLanesTest, MultipleLineLaneSegmentWithoutHint) { for (const auto truth_value : truth_vector) { EXPECT_NO_THROW(result = rg->ToRoadPosition(std::get<0>(truth_value), std::get<3>(truth_value))); EXPECT_EQ(result.road_position.lane->id(), std::get<1>(truth_value).lane->id()); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.road_position.pos, std::get<1>(truth_value).pos, kLinearTolerance)); - EXPECT_TRUE( - api::test::IsInertialPositionClose(result.nearest_position, std::get<2>(truth_value), kLinearTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.road_position.pos, std::get<1>(truth_value).pos, kLinearTolerance))); + EXPECT_TRUE(AssertCompare( + api::IsInertialPositionClose(result.nearest_position, std::get<2>(truth_value), kLinearTolerance))); EXPECT_NEAR(result.distance, std::get<4>(truth_value), kLinearTolerance); } } @@ -505,9 +508,9 @@ GTEST_TEST(MultilaneLanesTest, OverlappingLaneBounds) { const api::InertialPosition kAPoint(0., 0., 0.); EXPECT_NO_THROW(result = rg->ToRoadPosition(kAPoint)); EXPECT_NE(result.road_position.lane, nullptr); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.road_position.pos, api::LanePosition(0., 0., 0.), kLinearTolerance)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, kAPoint, kLinearTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.road_position.pos, api::LanePosition(0., 0., 0.), kLinearTolerance))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(result.nearest_position, kAPoint, kLinearTolerance))); EXPECT_NEAR(result.distance, 0., kLinearTolerance); // Using the same InertialPosition, but with a line-hint, we should get the @@ -515,18 +518,18 @@ GTEST_TEST(MultilaneLanesTest, OverlappingLaneBounds) { const api::RoadPosition kStartLineLane{line_lane, api::LanePosition(0., 0., 0.)}; EXPECT_NO_THROW(result = rg->ToRoadPosition(kAPoint, kStartLineLane)); EXPECT_EQ(result.road_position.lane->id(), line_lane->id()); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.road_position.pos, api::LanePosition(0., 0., 0.), kLinearTolerance)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, kAPoint, kLinearTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.road_position.pos, api::LanePosition(0., 0., 0.), kLinearTolerance))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(result.nearest_position, kAPoint, kLinearTolerance))); EXPECT_NEAR(result.distance, 0., kLinearTolerance); // Same as before, but with a hint on the arc-segment. const api::RoadPosition kStartArcLane{arc_lane, api::LanePosition(0., 0., 0.)}; EXPECT_NO_THROW(result = rg->ToRoadPosition(kAPoint, kStartArcLane)); EXPECT_EQ(result.road_position.lane->id(), arc_lane->id()); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.road_position.pos, api::LanePosition(0., 0., 0.), kLinearTolerance)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, kAPoint, kLinearTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.road_position.pos, api::LanePosition(0., 0., 0.), kLinearTolerance))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(result.nearest_position, kAPoint, kLinearTolerance))); EXPECT_NEAR(result.distance, 0., kLinearTolerance); // Tests a InertialPosition in between line and arc lane curves. It is closer to @@ -540,9 +543,9 @@ GTEST_TEST(MultilaneLanesTest, OverlappingLaneBounds) { const api::InertialPosition kBPoint(kX, kROffset, 0.); EXPECT_NO_THROW(result = rg->ToRoadPosition(kBPoint)); EXPECT_EQ(result.road_position.lane->id(), line_lane->id()); - EXPECT_TRUE( - api::test::IsLanePositionClose(result.road_position.pos, api::LanePosition(kX, kROffset, 0.), kLinearTolerance)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, kBPoint, kLinearTolerance)); + EXPECT_TRUE(AssertCompare( + api::IsLanePositionClose(result.road_position.pos, api::LanePosition(kX, kROffset, 0.), kLinearTolerance))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(result.nearest_position, kBPoint, kLinearTolerance))); EXPECT_NEAR(result.distance, 0., kLinearTolerance); // Tests a InertialPosition in between line and arc lane curves. It is closer to @@ -555,9 +558,9 @@ GTEST_TEST(MultilaneLanesTest, OverlappingLaneBounds) { const api::InertialPosition kCPoint(kX, kCY, 0.); EXPECT_NO_THROW(result = rg->ToRoadPosition(kCPoint)); EXPECT_EQ(result.road_position.lane->id(), arc_lane->id()); - EXPECT_TRUE(api::test::IsLanePositionClose(result.road_position.pos, - api::LanePosition(kRadius * kCTheta, -kROffset, 0.), kLinearTolerance)); - EXPECT_TRUE(api::test::IsInertialPositionClose(result.nearest_position, kCPoint, kLinearTolerance)); + EXPECT_TRUE(AssertCompare(api::IsLanePositionClose( + result.road_position.pos, api::LanePosition(kRadius * kCTheta, -kROffset, 0.), kLinearTolerance))); + EXPECT_TRUE(AssertCompare(api::IsInertialPositionClose(result.nearest_position, kCPoint, kLinearTolerance))); EXPECT_NEAR(result.distance, 0., kLinearTolerance); } diff --git a/test/maliput_multilane/multilane_segments_test.cc b/test/maliput_multilane/multilane_segments_test.cc index f414e2e..7a89d6e 100644 --- a/test/maliput_multilane/multilane_segments_test.cc +++ b/test/maliput_multilane/multilane_segments_test.cc @@ -32,9 +32,10 @@ /* clang-format on */ #include +#include #include -#include +#include "assert_compare.h" #include "maliput_multilane/arc_road_curve.h" #include "maliput_multilane/junction.h" #include "maliput_multilane/lane.h" @@ -46,6 +47,8 @@ namespace maliput { namespace multilane { namespace { +using test::AssertCompare; + const double kLinearTolerance = 1e-6; const double kAngularTolerance = 1e-6; const double kZeroTolerance = 0.; @@ -82,12 +85,13 @@ GTEST_TEST(MultilaneSegmentsTest, MultipleLanes) { EXPECT_EQ(rg.CheckInvariants(), std::vector()); - EXPECT_TRUE(api::test::IsRBoundsClose(l0->lane_bounds(0.), {-8., kHalfLaneWidth}, kZeroTolerance)); - EXPECT_TRUE(api::test::IsRBoundsClose(l0->segment_bounds(0.), {-8., 32.}, kZeroTolerance)); - EXPECT_TRUE(api::test::IsRBoundsClose(l1->lane_bounds(0.), {-kHalfLaneWidth, kHalfLaneWidth}, kZeroTolerance)); - EXPECT_TRUE(api::test::IsRBoundsClose(l1->segment_bounds(0.), {-23., 17.}, kZeroTolerance)); - EXPECT_TRUE(api::test::IsRBoundsClose(l2->lane_bounds(0.), {-kHalfLaneWidth, 2.}, kZeroTolerance)); - EXPECT_TRUE(api::test::IsRBoundsClose(l2->segment_bounds(0.), {-38., 2.}, kZeroTolerance)); + EXPECT_TRUE(AssertCompare(api::IsRBoundsClose(l0->lane_bounds(0.), {-8., kHalfLaneWidth}, kZeroTolerance))); + EXPECT_TRUE(AssertCompare(api::IsRBoundsClose(l0->segment_bounds(0.), {-8., 32.}, kZeroTolerance))); + EXPECT_TRUE( + AssertCompare(api::IsRBoundsClose(l1->lane_bounds(0.), {-kHalfLaneWidth, kHalfLaneWidth}, kZeroTolerance))); + EXPECT_TRUE(AssertCompare(api::IsRBoundsClose(l1->segment_bounds(0.), {-23., 17.}, kZeroTolerance))); + EXPECT_TRUE(AssertCompare(api::IsRBoundsClose(l2->lane_bounds(0.), {-kHalfLaneWidth, 2.}, kZeroTolerance))); + EXPECT_TRUE(AssertCompare(api::IsRBoundsClose(l2->segment_bounds(0.), {-38., 2.}, kZeroTolerance))); } } // namespace From eac1f5376dbfdeb5910ad3ee50ade5e9104a7d9a Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Tue, 10 Oct 2023 09:58:36 -0300 Subject: [PATCH 2/2] Updates CI Signed-off-by: Franco Cipollone --- .github/workflows/build.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ed4e67f..9400462 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -2,6 +2,8 @@ name: gcc on: push: + branches: + - main pull_request: branches: - main @@ -11,6 +13,11 @@ env: PACKAGE_NAME: maliput_multilane ROS_DISTRO: foxy +# Cancel previously running PR jobs +concurrency: + group: '${{ github.workflow }} @ ${{ github.event.pull_request.head.label || github.head_ref || github.ref }}' + cancel-in-progress: true + jobs: compile_and_test: name: Compile and test