fix(behavior_path_planner): fix cutOverlappedLanes (autowarefoundatio…

…n#2916) (autowarefoundation#286) * fix(behavior_path_planner): fix pull over deceleration before search area start (autowarefoundation#2898) (autowarefoundation#283) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix(behavior_path_planner): fix cutOverlappedLanes (autowarefoundation#2916) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
soblin · Mar 6, 2023 · 948e936 · 948e936
1 parent d9ee918
commit 948e936
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 16 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
@@ -470,18 +470,18 @@ BehaviorModuleOutput PullOverModule::plan()
       break;
     }
 
-    // Decelerate before the minimum shift distance from the goal search area.
+    // decelerate before the search area start
     if (status_.is_safe) {
-      auto & first_path = status_.pull_over_path.partial_paths.front();
       const auto search_start_pose = calcLongitudinalOffsetPose(
-        first_path.points, refined_goal_pose_.position,
+        status_.pull_over_path.getFullPath().points, refined_goal_pose_.position,
         -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front);
-      const Pose deceleration_pose =
-        search_start_pose ? *search_start_pose : first_path.points.front().point.pose;
-      constexpr double deceleration_buffer = 15.0;
-      first_path = util::setDecelerationVelocity(
-        first_path, parameters_.pull_over_velocity, deceleration_pose, -deceleration_buffer,
-        parameters_.deceleration_interval);
+      if (search_start_pose) {
+        auto & first_path = status_.pull_over_path.partial_paths.front();
+        constexpr double deceleration_buffer = 15.0;
+        first_path = util::setDecelerationVelocity(
+          first_path, parameters_.pull_over_velocity, *search_start_pose, -deceleration_buffer,
+          parameters_.deceleration_interval);
+      }
     }
 
     // generate drivable area for each partial path

diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
@@ -1055,8 +1055,12 @@ boost::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes>
 
     for (const auto & lanelet : lanelets) {
       for (const auto & target_lanelet : target_lanelets) {
-        if (boost::geometry::intersects(
-              lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon())) {
+        std::vector<Point2d> intersections{};
+        boost::geometry::intersection(
+          lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon(),
+          intersections);
+        // if only one point intersects, it is assumed not to be overlapped
+        if (intersections.size() > 1) {
           return true;
         }
       }
@@ -1118,7 +1122,7 @@ std::vector<DrivableLanes> cutOverlappedLanes(
         std::find(removed_lane_ids.begin(), removed_lane_ids.end(), lane_id) !=
         removed_lane_ids.end()) {
         path.points.erase(path.points.begin() + i, path.points.end());
-        break;
+        return shorten_lanes;
       }
     }
   }
@@ -1252,10 +1256,11 @@ void generateDrivableArea(
     calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length);
   const auto right_goal_point =
     calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length);
-  const size_t left_goal_idx =
-    findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point);
-  const size_t right_goal_idx =
-    findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point);
+  const size_t left_goal_idx = std::max(
+    goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point));
+  const size_t right_goal_idx = std::max(
+    goal_right_start_idx,
+    findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point));
 
   // Store Data
   path.left_bound.clear();