diff --git a/modules/map/pnc_map/path_test.cc b/modules/map/pnc_map/path_test.cc index 10d9be6bee3544a20fd9842e3119ee85e147e884..a215ea3e693513c4596c666db9f291867509bc96 100644 --- a/modules/map/pnc_map/path_test.cc +++ b/modules/map/pnc_map/path_test.cc @@ -558,6 +558,83 @@ TEST(TestSuite, hdmap_jerky_path) { } } +TEST(TestSuite, hdmap_s_path) { + std::vector points; + const double kRadius = 50.0; + const int kNumSegments = 100; + for (int i = 0; i <= kNumSegments; ++i) { + if (i <= kNumSegments / 2) { + const double p = -M_PI_2 + + 2.0 * M_PI * static_cast(i) / + static_cast(kNumSegments); + points.push_back( + make_map_path_point(kRadius * cos(p), kRadius * (sin(p) - 1.0))); + } else { + const double p = M_PI_2 - + 2.0 * M_PI * static_cast(i) / + static_cast(kNumSegments); + points.push_back( + make_map_path_point(kRadius * cos(p), kRadius * (sin(p) + 1.0))); + } + } + const Path path(points, {}, 2.0); + EXPECT_EQ(path.num_points(), kNumSegments + 1); + EXPECT_EQ(path.num_segments(), kNumSegments); + EXPECT_EQ(path.path_points().size(), kNumSegments + 1); + EXPECT_EQ(path.lane_segments().size(), 0); + EXPECT_EQ(path.segments().size(), kNumSegments); + const auto* path_approximation = path.approximation(); + EXPECT_NEAR(path_approximation->max_error(), 2.0, 1e-6); + EXPECT_EQ(path_approximation->original_ids().size(), 12); + EXPECT_EQ(path_approximation->original_ids()[0], 0); + EXPECT_EQ(path_approximation->original_ids()[1], 9); + EXPECT_EQ(path_approximation->original_ids()[2], 18); + EXPECT_EQ(path_approximation->original_ids()[3], 27); + EXPECT_EQ(path_approximation->original_ids()[4], 36); + EXPECT_EQ(path_approximation->original_ids()[5], 45); + EXPECT_EQ(path_approximation->original_ids()[6], 57); + EXPECT_EQ(path_approximation->original_ids()[7], 66); + EXPECT_EQ(path_approximation->original_ids()[8], 75); + EXPECT_EQ(path_approximation->original_ids()[9], 84); + EXPECT_EQ(path_approximation->original_ids()[10], 93); + EXPECT_EQ(path_approximation->original_ids()[11], 100); + const double total_length = path.accumulated_s().back(); + + const double kLargeEps = 0.1; + double accumulate_s; + double lateral; + double distance; + EXPECT_TRUE(path.get_projection({-1, -2.0 * kRadius - 1}, &accumulate_s, + &lateral, &distance)); + EXPECT_NEAR(accumulate_s, -1.0, kLargeEps); + EXPECT_NEAR(lateral, -1.0, kLargeEps); + EXPECT_NEAR(distance, sqrt(2.0), 1e-6); + + EXPECT_TRUE(path.get_projection({1, 2.0 * kRadius + 1}, &accumulate_s, + &lateral, &distance)); + EXPECT_NEAR(accumulate_s, total_length + 1.0, kLargeEps); + EXPECT_NEAR(lateral, 1.0, kLargeEps); + EXPECT_NEAR(distance, sqrt(2.0), 1e-6); + + const Path path_no_approximation(points, {}); + for (int case_id = 0; case_id < 10000; ++case_id) { + const double x = random_double(-kRadius * 1.5, kRadius * 1.5); + const double y = random_double(-kRadius * 2.5, kRadius * 2.5); + EXPECT_TRUE( + path.get_nearest_point({x, y}, &accumulate_s, &lateral, &distance)); + + double other_accumulate_s; + double other_lateral; + double other_distance; + EXPECT_TRUE(path_no_approximation.get_nearest_point( + {x, y}, &other_accumulate_s, &other_lateral, &other_distance)); + + EXPECT_NEAR(distance, other_distance, 1e-6); + EXPECT_NEAR(path.get_smooth_point(accumulate_s).DistanceTo({x, y}), + distance, 1e-6); + } +} + } // hdmap } // apollo