feat(start_planner): keep distance against front objects (#5983)

* refactor extractCollisionCheckPath

Signed-off-by: kyoichi-sugahara <[email protected]>

---------

Signed-off-by: kyoichi-sugahara <[email protected]>
Co-authored-by: Kosuke Takeuchi <[email protected]>

planning/behavior_path_start_planner_module/README.md

@@ -75,7 +75,8 @@ PullOutPath --o PullOutPlannerBase
 | intersection_search_length                                  | [m]   | double | check if intersections exist within this length                             | 30.0          |
 | length_ratio_for_turn_signal_deactivation_near_intersection | [m]   | double | deactivate turn signal of this module near intersection                     | 0.5           |
 | collision_check_margin                                      | [m]   | double | Obstacle collision check margin                                             | 1.0           |
-| collision_check_distance_from_end                           | [m]   | double | collision check distance from end point. currently only for pull out        | 15.0          |
+| collision_check_distance_from_end                           | [m]   | double | collision check distance from end shift end pose                            | 1.0           |
+| collision_check_margin_from_front_object                    | [m]   | double | collision check margin from front object                                    | 5.0           |
 | center_line_path_interval                                   | [m]   | double | reference center line path point interval                                   | 1.0           |
 
 ## Safety check with static obstacles

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -7,6 +7,7 @@
       th_stopped_time: 1.0
       collision_check_margin: 1.0
       collision_check_distance_from_end: 1.0
+      collision_check_margin_from_front_object: 5.0
       th_moving_object_velocity: 1.0
       th_distance_to_middle_of_the_road: 0.5
       center_line_path_interval: 1.0

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -174,7 +174,7 @@ class StartPlannerModule : public SceneModuleInterface
     const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
     const Pose & refined_start_pose, const Pose & goal_pose);
 
-  PathWithLaneId extractCollisionCheckPath(const PullOutPath & path);
+  PathWithLaneId extractCollisionCheckSection(const PullOutPath & path);
   void updateStatusWithCurrentPath(
     const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
     const behavior_path_planner::PlannerType & planner_type);

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp

@@ -44,6 +44,7 @@ struct StartPlannerParameters
   double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
   double collision_check_margin{0.0};
   double collision_check_distance_from_end{0.0};
+  double collision_check_margin_from_front_object{0.0};
   double th_moving_object_velocity{0.0};
   double center_line_path_interval{0.0};
 

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -46,6 +46,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   p.collision_check_margin = node->declare_parameter<double>(ns + "collision_check_margin");
   p.collision_check_distance_from_end =
     node->declare_parameter<double>(ns + "collision_check_distance_from_end");
+  p.collision_check_margin_from_front_object =
+    node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
   p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
   p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
   // shift pull out

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -645,7 +645,7 @@ bool StartPlannerModule::findPullOutPath(
 
   // check collision
   if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-        vehicle_footprint, extractCollisionCheckPath(*pull_out_path), pull_out_lane_stop_objects,
+        vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects,
         parameters_->collision_check_margin)) {
     return false;
   }
@@ -660,7 +660,7 @@ bool StartPlannerModule::findPullOutPath(
   return true;
 }
 
-PathWithLaneId StartPlannerModule::extractCollisionCheckPath(const PullOutPath & path)
+PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path)
 {
   PathWithLaneId combined_path;
   for (const auto & partial_path : path.partial_paths) {
@@ -912,6 +912,10 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
     pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity,
     backward_path_length, std::numeric_limits<double>::max());
 
+  const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
+    pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0,
+    std::numeric_limits<double>::max());
+
   // Set the maximum backward distance less than the distance from the vehicle's base_link to the
   // lane's rearmost point to prevent lane departure.
   const double current_arc_length =
@@ -924,9 +928,12 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
        back_distance += parameters_->backward_search_resolution) {
     const auto backed_pose = calcLongitudinalOffsetPose(
       back_path_from_start_pose.points, start_pose.position, -back_distance);
-    if (!backed_pose) {
+    if (!backed_pose) continue;
+
+    if (utils::checkCollisionBetweenFootprintAndObjects(
+          local_vehicle_footprint, *backed_pose, front_stop_objects_in_pull_out_lanes,
+          parameters_->collision_check_margin_from_front_object))
       continue;
-    }
 
     const double backed_pose_arc_length =
       lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length;