Properly erase successive duplicate points and fix unit tests

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -266,8 +266,11 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
   const auto point_cmp = [](const auto & p1, const auto & p2) {
     return p1.x == p2.x && p1.y == p2.y;
   };
-  std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp);
-  std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp);
+  path.left_bound.erase(
+    std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end());
+  path.right_bound.erase(
+    std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp),
+    path.right_bound.end());
   copy_z_over_arc_length(original_left_bound, path.left_bound);
   copy_z_over_arc_length(original_right_bound, path.right_bound);
 }

planning/behavior_path_planner/test/test_drivable_area_expansion.cpp

@@ -266,7 +266,6 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea)
     params.ego_right_offset = -2.0;
   }
   // we expect the drivable area to be expanded by 1m on each side
-  // BUT short paths, due to pruning at the edge of the driving area, there is no expansion
   drivable_area_expansion::expandDrivableArea(
     path, params, dynamic_objects, route_handler, path_lanes);
   // unchanged path points
@@ -280,16 +279,16 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea)
   ASSERT_EQ(path.left_bound.size(), 3ul);
   EXPECT_NEAR(path.left_bound[0].x, 0.0, eps);
   EXPECT_NEAR(path.left_bound[0].y, 1.0, eps);
-  EXPECT_NEAR(path.left_bound[1].x, 1.0, eps);
-  EXPECT_NEAR(path.left_bound[1].y, 1.0, eps);
+  EXPECT_NEAR(path.left_bound[1].x, 0.0, eps);
+  EXPECT_NEAR(path.left_bound[1].y, 2.0, eps);
   EXPECT_NEAR(path.left_bound[2].x, 2.0, eps);
-  EXPECT_NEAR(path.left_bound[2].y, 1.0, eps);
+  EXPECT_NEAR(path.left_bound[2].y, 2.0, eps);
   // expanded right bound
   ASSERT_EQ(path.right_bound.size(), 3ul);
   EXPECT_NEAR(path.right_bound[0].x, 0.0, eps);
-  EXPECT_NEAR(path.right_bound[0].y, -1.0, eps);
-  EXPECT_NEAR(path.right_bound[1].x, 1.0, eps);
-  EXPECT_NEAR(path.right_bound[1].y, -1.0, eps);
+  EXPECT_NEAR(path.right_bound[0].y, -2.0, eps);
+  EXPECT_NEAR(path.right_bound[1].x, 2.0, eps);
+  EXPECT_NEAR(path.right_bound[1].y, -2.0, eps);
   EXPECT_NEAR(path.right_bound[2].x, 2.0, eps);
   EXPECT_NEAR(path.right_bound[2].y, -1.0, eps);
 }