fix(behavior_path_planner): fix left/right split of the drivable area (…

…#4738)

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

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

@@ -165,8 +165,8 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
 {
   const auto original_left_bound = path.left_bound;
   const auto original_right_bound = path.right_bound;
-  const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) {
-    return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0;
+  const auto is_left_of_path = [&](const point_t & p) {
+    return motion_utils::calcLateralOffset(path.points, convert_point(p)) > 0.0;
   };
   // prepare delimiting lines: start and end of the original expanded drivable area
   const auto start_segment =
@@ -230,12 +230,10 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
             *it, *std::next(it), start_segment.first, start_segment.second);
     if (inter_start) {
       const auto dist = boost::geometry::distance(*inter_start, path_start_segment);
-      const auto is_left_of_path_start = is_left_of_segment(
-        convert_point(path.points[0].point.pose.position),
-        convert_point(path.points[1].point.pose.position), *inter_start);
-      if (is_left_of_path_start && dist < start_left.distance)
+      const auto is_left = is_left_of_path(*inter_start);
+      if (is_left && dist < start_left.distance)
         start_left.update(*inter_start, it, dist);
-      else if (!is_left_of_path_start && dist < start_right.distance)
+      else if (!is_left && dist < start_right.distance)
         start_right.update(*inter_start, it, dist);
     }
     const auto inter_end =
@@ -244,11 +242,10 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
         : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second);
     if (inter_end) {
       const auto dist = boost::geometry::distance(*inter_end, path_end_segment);
-      const auto is_left_of_path_end = is_left_of_segment(
-        convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end);
-      if (is_left_of_path_end && dist < end_left.distance)
+      const auto is_left = is_left_of_path(*inter_end);
+      if (is_left && dist < end_left.distance)
         end_left.update(*inter_end, it, dist);
-      else if (!is_left_of_path_end && dist < end_right.distance)
+      else if (!is_left && dist < end_right.distance)
         end_right.update(*inter_end, it, dist);
     }
   }

planning/behavior_path_planner/test/test_drivable_area_expansion.cpp

@@ -284,13 +284,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea)
   }
 
   // expanded left bound
-  ASSERT_EQ(path.left_bound.size(), 3ul);
+  ASSERT_EQ(path.left_bound.size(), 4ul);
   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, 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, 2.0, eps);
+  EXPECT_NEAR(path.left_bound[3].x, 2.0, eps);
+  EXPECT_NEAR(path.left_bound[3].y, 1.0, eps);
   // expanded right bound
   ASSERT_EQ(path.right_bound.size(), 3ul);
   EXPECT_NEAR(path.right_bound[0].x, 0.0, eps);