feat(behavior_path_planner): ignore pull out start near lane end (aut…

…owarefoundation#2675) (autowarefoundation#248) * feat(behavior_path_planner): ignore pull out start near lane end Signed-off-by: kosuke55 <[email protected]> * update warn message Signed-off-by: kosuke55 <[email protected]> Signed-off-by: kosuke55 <[email protected]> Signed-off-by: kosuke55 <[email protected]> Co-authored-by: Kosuke Takeuchi <[email protected]>
zulfaqar-azmi-t4 · Jan 18, 2023 · f646431 · f646431
1 parent f5936bd
commit f646431
Show file tree

Hide file tree

Showing 5 changed files with 33 additions and 18 deletions.
diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md
@@ -440,6 +440,7 @@ If a safe path cannot be generated from the current position, search backwards f
 | max_back_distance                                                          | [m]            | double | maximum back distance                                                                                                 | 15.0          |
 | backward_search_resolution                                                 | [m]            | double | distance interval for searching backward pull out start point                                                         | 2.0           |
 | backward_path_update_duration                                              | [s]            | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0           |
+| ignore_distance_from_lane_end                                              | [m]            | double | distance from end of pull out lanes for ignoring start candidates                                                     | 15.0          |
 
 #### Unimplemented parts / limitations for pull put
 

diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml
@@ -28,3 +28,4 @@
       max_back_distance: 15.0
       backward_search_resolution: 2.0
       backward_path_update_duration: 3.0
+      ignore_distance_from_lane_end: 15.0
diff --git a/..._path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/..._path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp
@@ -49,6 +49,7 @@ struct PullOutParameters
   double max_back_distance;
   double backward_search_resolution;
   double backward_path_update_duration;
+  double ignore_distance_from_lane_end;
   // drivable area expansion
   double drivable_area_right_bound_offset;
   double drivable_area_left_bound_offset;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -537,6 +537,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam()
   p.max_back_distance = dp("max_back_distance", 15.0);
   p.backward_search_resolution = dp("backward_search_resolution", 2.0);
   p.backward_path_update_duration = dp("backward_path_update_duration", 3.0);
+  p.ignore_distance_from_lane_end = dp("ignore_distance_from_lane_end", 15.0);
   // drivable area
   p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0);
   p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0);

diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
@@ -174,7 +174,8 @@ BehaviorModuleOutput PullOutModule::plan()
 
   BehaviorModuleOutput output;
   if (!status_.is_safe) {
-    RCLCPP_INFO(getLogger(), "not found safe path");
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path");
     return output;
   }
 
@@ -376,12 +377,13 @@ void PullOutModule::planWithPriorityOnEfficientPath(
 
   // plan with each planner
   for (const auto & planner : pull_out_planners_) {
-    for (size_t i = 0; i < start_pose_candidates.size(); ++i) {
-      // pull out start pose is current_pose
-      if (i == 0) {
-        status_.back_finished = true;
-      }
-      const auto & pull_out_start_pose = start_pose_candidates.at(i);
+    for (const Pose & pull_out_start_pose : start_pose_candidates) {
+      // check pull_out_start_pose is current_pose
+      const double distance_from_current_pose = tier4_autoware_utils::calcDistance2d(
+        pull_out_start_pose.position, planner_data_->self_pose->pose.position);
+      constexpr double epsilon = 0.1;
+      status_.back_finished = distance_from_current_pose < epsilon;
+
       planner->setPlannerData(planner_data_);
       const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose);
       if (pull_out_path) {  // found safe path
@@ -391,8 +393,6 @@ void PullOutModule::planWithPriorityOnEfficientPath(
         status_.planner_type = planner->getPlannerType();
         break;
       }
-      // pull out start pose is not current_pose(index > 0), so need back.
-      status_.back_finished = false;
     }
     if (status_.is_safe) {
       break;
@@ -406,12 +406,12 @@ void PullOutModule::planWithPriorityOnShortBackDistance(
   status_.is_safe = false;
   status_.planner_type = PlannerType::NONE;
 
-  for (size_t i = 0; i < start_pose_candidates.size(); ++i) {
-    // pull out start pose is current_pose
-    if (i == 0) {
-      status_.back_finished = true;
-    }
-    const auto & pull_out_start_pose = start_pose_candidates.at(i);
+  for (const Pose & pull_out_start_pose : start_pose_candidates) {
+    // check pull_out_start_pose is current_pose
+    const double distance_from_current_pose = tier4_autoware_utils::calcDistance2d(
+      pull_out_start_pose.position, planner_data_->self_pose->pose.position);
+    constexpr double epsilon = 0.1;
+    status_.back_finished = distance_from_current_pose < epsilon;
     // plan with each planner
     for (const auto & planner : pull_out_planners_) {
       planner->setPlannerData(planner_data_);
@@ -427,8 +427,6 @@ void PullOutModule::planWithPriorityOnShortBackDistance(
     if (status_.is_safe) {
       break;
     }
-    // pull out start pose is not current_pose(index > 0), so need back.
-    status_.back_finished = false;
   }
 }
 
@@ -518,7 +516,7 @@ void PullOutModule::updatePullOutStatus()
 // make this class?
 std::vector<Pose> PullOutModule::searchBackedPoses()
 {
-  const auto current_pose = planner_data_->self_pose->pose;
+  const Pose & current_pose = planner_data_->self_pose->pose;
 
   // get backward shoulder path
   const auto arc_position_pose =
@@ -545,6 +543,19 @@ std::vector<Pose> PullOutModule::searchBackedPoses()
       continue;
     }
 
+    // check the back pose is near the lane end
+    const double length_to_backed_pose =
+      lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length;
+    const double length_to_lane_end =
+      lanelet::utils::getLaneletLength2d(status_.pull_out_lanes.back());
+    const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose;
+    if (distance_from_lane_end < parameters_.ignore_distance_from_lane_end) {
+      RCLCPP_WARN_THROTTLE(
+        getLogger(), *clock_, 5000,
+        "the ego is too close to the lane end, so needs backward driving");
+      continue;
+    }
+
     if (util::checkCollisionBetweenFootprintAndObjects(
           local_vehicle_footprint, *backed_pose, *(planner_data_->dynamic_object),
           parameters_.collision_check_margin)) {