fix(gtest): update to latest and minor fix

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp

@@ -31,7 +31,7 @@ namespace bg = boost::geometry;
 namespace lg = lanelet::geometry;
 
 void createOffsetLineString(
-  BasicLineString2d & in, const double offset, BasicLineString2d & offset_line_string)
+  const BasicLineString2d & in, const double offset, BasicLineString2d & offset_line_string)
 {
   for (size_t i = 0; i < in.size() - 1; i++) {
     const auto & p0 = in.at(i);
@@ -45,9 +45,13 @@ void createOffsetLineString(
     const double offset_x = p0[0] - std::sin(yaw) * offset;
     const double offset_y = p0[1] + std::cos(yaw) * offset;
     offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y});
+    //! insert final offset linestring using prev vertical direction
+    if (i == in.size() - 2) {
+      const double offset_x = p1[0] - std::sin(yaw) * offset;
+      const double offset_y = p1[1] + std::cos(yaw) * offset;
+      offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y});
+    }
   }
-  // to make inner bound same size as outer
-  in.pop_back();
   return;
 }
 
@@ -65,12 +69,14 @@ void buildSlices(
    */
   BasicLineString2d inner_bounds = path_lanelet.centerline2d().basicLineString();
   BasicLineString2d outer_bounds;
+  if (inner_bounds.size() < 2) return;
   createOffsetLineString(inner_bounds, range.max_distance, outer_bounds);
   const double ratio_dist_start = std::abs(range.min_distance / range.max_distance);
   const double ratio_dist_increment = std::min(1.0, slice_width / std::abs(range.max_distance));
   lanelet::BasicPolygon2d poly;
   const int num_step = static_cast<int>(slice_length / resolution);
-  const int max_index = static_cast<int>(inner_bounds.size());
+  //! max index is the last index of path point
+  const int max_index = static_cast<int>(inner_bounds.size() - 1);
   for (int s = 0; s < max_index; s += num_step) {
     const double length = s * slice_length;
     const double next_length = (s + num_step) * resolution;
@@ -112,14 +118,14 @@ void buildSidewalkSlices(
   SliceRange left_slice_range = {
     longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist};
   // in most case lateral distance is much more effective for velocity planning
-  const double ratio_w_h = 2.0;
-  const double slice_length = ratio_w_h * slice_size;
+  const double slice_length = 2.0 * slice_size;
   const double slice_width = slice_size;
-  buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, 1.0);
+  const double resolution = 1.0;
+  buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, resolution);
   SliceRange right_slice_range = {
     longitudinal_offset, longitudinal_max_dist, -lateral_offset,
     -lateral_offset - lateral_max_dist};
-  buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, 1.0);
+  buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, resolution);
   // Properly order lanelets from closest to furthest
   slices = left_slices;
   slices.insert(slices.end(), right_slices.begin(), right_slices.end());

planning/behavior_velocity_planner/test/src/test_geometry.cpp

@@ -45,7 +45,7 @@ TEST(buildSlices, one_full_slice)
   const double slice_width = 1.5;
   std::vector<Slice> slices;
   buildSlices(slices, traj_lanelet, range, slice_length, slice_width, 0.5);
-  ASSERT_EQ(slices.size(), static_cast<size_t>(2));
+  ASSERT_EQ(slices.size(), static_cast<size_t>(4));
   EXPECT_EQ(slices[0].range.min_length, 0.0);
   EXPECT_EQ(slices[0].range.min_distance, 0.0);
   EXPECT_EQ(slices[0].range.max_length, 3.0);
@@ -65,50 +65,29 @@ TEST(buildSlices, 3x3square_slice)
       1 | |     |
       2 | |SLICE|
       3 | |-----|
-      4 | |
-      5 | |
       */
-  const int nb_points = 6;
-  const double len = 5.0;
-  lanelet::ConstLanelet traj_lanelet = test::verticalLanelet({0.0, 0.0}, 1.0, len, nb_points);
+  const int nb_points = 4;
+  const double len = 3.0;
+  lanelet::ConstLanelet lanelet = test::createLanelet({0.0, 0.0}, {0.0, len}, nb_points);
   SliceRange range;
-  range.min_distance = 0.0;
+  range.min_distance = -1.0;
   range.max_distance = -3.0;
   const double slice_length = 1.0;
   const double slice_width = 1.0;
   const int num_right_slice =
-    (len - 1) / slice_length * std::abs(range.max_distance - range.min_distance) / slice_width;
+    (len / slice_length) * (std::abs(range.max_distance - range.min_distance) / slice_width);
+  //
   {
     std::vector<Slice> slices;
-    buildSlices(slices, traj_lanelet, range, slice_length, slice_width, 0.5);
-    SliceRange ref;
+    buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0);
     ASSERT_EQ(slices.size(), static_cast<size_t>(num_right_slice));
-    for (double l = range.min_length; l < range.max_length; l += slice_length) {
-      for (double d = range.min_distance; d > range.max_distance; d -= slice_width) {
-        ref.min_length = l;
-        ref.max_length = l + slice_length;
-        ref.min_distance = d;
-        ref.max_distance = d - slice_width;
-        // EXPECT_NE(std::find_if(slices.begin(), slices.end(), equal_range), slices.end());
-      }
-    }
   }
-
   // change to the left side (positive distance)
+  range.min_distance = 1.0;
   range.max_distance = 3.0;
   {
     std::vector<Slice> slices;
-    buildSlices(slices, traj_lanelet, range, slice_length, slice_width, 0.5);
-    SliceRange ref;
+    buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0);
     ASSERT_EQ(slices.size(), static_cast<size_t>(num_right_slice));
-    for (double l = range.min_length; l < range.max_length; l += slice_length) {
-      for (double d = range.min_distance; d < range.max_distance; d += slice_width) {
-        ref.min_length = l;
-        ref.max_length = l + slice_length;
-        ref.min_distance = d;
-        ref.max_distance = d + slice_width;
-        // EXPECT_NE(std::find_if(slices.begin(), slices.end(), equal_range), slices.end());
-      }
-    }
   }
 }

planning/behavior_velocity_planner/test/src/utils.hpp

@@ -33,6 +33,36 @@ namespace test
 {
 // Default grid parameter such that UNKNOWN cells have a value of 50
 const behavior_velocity_planner::grid_utils::GridParam grid_param = {10, 51};
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using ConstDpair = const std::pair<double, double>;
+
+/* lanelet
+        0 1 2 3 4 5
+      0 x
+      1   x
+      2     x
+      3       x
+      4         x
+      5           x
+      */
+inline lanelet::ConstLanelet createLanelet(
+  ConstDpair & start = {0, 0}, ConstDpair & end = {5, 5}, int nb_points = 6)
+{
+  const double interval =
+    std::hypot(end.first - start.first, end.second - start.second) / (nb_points - 1);
+  const double norm_x = (end.first - start.first) / interval;
+  const double norm_y = (end.second - start.second) / interval;
+  lanelet::Points3d path_points;
+  for (int i = 0; i < nb_points; i++) {
+    const double x = start.first + norm_x * i;
+    const double y = start.second + norm_y * i;
+    path_points.emplace_back(lanelet::InvalId, x, y, 0);
+  }
+  lanelet::LineString3d centerline(lanelet::InvalId, path_points);
+  lanelet::Lanelet path_lanelet(lanelet::InvalId);
+  path_lanelet.setCenterline(centerline);
+  return lanelet::ConstLanelet(path_lanelet);
+}
 
 /* Horizontal lanelet
         0 1 2 3 4 5 6