added TestSuite, hdmap_s_path

modules/map/pnc_map/path_test.cc

@@ -558,6 +558,83 @@ TEST(TestSuite, hdmap_jerky_path) {
   }
 }
 
+TEST(TestSuite, hdmap_s_path) {
+  std::vector<MapPathPoint> 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<double>(i) /
+                           static_cast<double>(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<double>(i) /
+                           static_cast<double>(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