feat(start_planner): keep distance back static objects from start pose

planning/behavior_path_start_planner_module/README.md

@@ -319,20 +319,21 @@ PullOutPath --o PullOutPlannerBase
 
 ## General parameters for start_planner
 
-| Name                                                        | Unit  | Type   | Description                                                                 | Default value        |
-| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- |
-| th_arrived_distance_m                                       | [m]   | double | distance threshold for arrival of path termination                          | 1.0                  |
-| th_distance_to_middle_of_the_road                           | [m]   | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1                  |
-| th_stopped_velocity_mps                                     | [m/s] | double | velocity threshold for arrival of path termination                          | 0.01                 |
-| th_stopped_time_sec                                         | [s]   | double | time threshold for arrival of path termination                              | 1.0                  |
-| th_turn_signal_on_lateral_offset                            | [m]   | double | lateral distance threshold for turning on blinker                           | 1.0                  |
-| 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_margins                                     | [m]   | double | Obstacle collision check margins list                                       | [2.0, 1.0, 0.5, 0.1] |
-| shift_collision_check_distance_from_end                     | [m]   | double | collision check distance from end shift end pose                            | -10.0                |
-| geometric_collision_check_distance_from_end                 | [m]   | double | collision check distance from end geometric end pose                        | 0.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                  |
+| Name                                                        | Unit  | Type     | Description                                                                                                         | Default value        |
+| :---------------------------------------------------------- | :---- | :------- | :------------------------------------------------------------------------------------------------------------------ | :------------------- |
+| th_arrived_distance_m                                       | [m]   | double   | distance threshold for arrival of path termination                                                                  | 1.0                  |
+| th_distance_to_middle_of_the_road                           | [m]   | double   | distance threshold to determine if the vehicle is on the middle of the road                                         | 0.1                  |
+| th_stopped_velocity_mps                                     | [m/s] | double   | velocity threshold for arrival of path termination                                                                  | 0.01                 |
+| th_stopped_time_sec                                         | [s]   | double   | time threshold for arrival of path termination                                                                      | 1.0                  |
+| th_turn_signal_on_lateral_offset                            | [m]   | double   | lateral distance threshold for turning on blinker                                                                   | 1.0                  |
+| 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                  |
+| objects_collision_check_margins                             | [m]   | [double] | obstacle collision check margins against static objects from the footprint on the trajectory in pull_out_lanes      | [2.0, 1.0, 0.5, 0.1] |
+| back_objects_collision_check_margin                         | [m]   | double   | obstacle collision check margin against back static objects from the footprint on the start pose in pull_out_lanes  | 3.0                  |
+| shift_collision_check_distance_from_end                     | [m]   | double   | collision check distance from end shift end pose                                                                    | -10.0                |
+| geometric_collision_check_distance_from_end                 | [m]   | double   | collision check distance from end geometric end pose                                                                | 0.0                  |
+| collision_check_margin_from_front_object                    | [m]   | double   | obstacle collision check margin against front static objects from the footprint on the start pose in pull_out_lanes | 5.0                  |
+| center_line_path_interval                                   | [m]   | double   | reference center line path point interval                                                                           | 1.0                  |
 
 ### **Ego vehicle's velocity planning**
 

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -5,7 +5,8 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
-      collision_check_margins: [2.0, 1.0, 0.5, 0.1]
+      objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1]
+      back_objects_collision_check_margin: 0.5
       collision_check_margin_from_front_object: 5.0
       th_moving_object_velocity: 1.0
       th_distance_to_middle_of_the_road: 0.5

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp

@@ -63,7 +63,8 @@ struct StartPlannerParameters
   double th_distance_to_middle_of_the_road{0.0};
   double intersection_search_length{0.0};
   double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
-  std::vector<double> collision_check_margins{};
+  std::vector<double> objects_collision_check_margins{};
+  double back_objects_collision_check_margin{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/include/behavior_path_start_planner_module/util.hpp

@@ -46,10 +46,34 @@ using route_handler::RouteHandler;
 PathWithLaneId getBackwardPath(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & current_pose, const Pose & backed_pose, const double velocity);
+
+/**
+ * @brief Get a sequence of lanelets for pulling out from the current position.
+ *
+ * If the ego vehicle's current position is on a shoulder lane, the function retrieves a sequence of
+ * shoulder lanelets. If it is on a road lane, the function returns a sequence of road lanelets.
+ *
+ */
 lanelet::ConstLanelets getPullOutLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length);
 Pose getBackedPose(
   const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance);
+
+/**
+ * @brief Calculate the minimum arc length distance from the ego vehicle to static objects within
+ the same lanelets.
+ *
+ * Calculates the minimum arc length distance between the ego vehicle and static objects in given
+ * lanelets. It compares each corner of the vehicle's transformed footprint with every corner of
+ * object polygons to find the shortest distance within the lanelet system.
+ */
+double calcMinArcLengthDistanceFromEgoToObjects(
+  const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose,
+  const lanelet::ConstLanelets & lanelets, const PredictedObjects & static_objects);
+
+double getArcLengthForPoint(
+  const lanelet::ConstLanelets & lanelets, const tier4_autoware_utils::Point2d & point);
+
 }  // namespace behavior_path_planner::start_planner_utils
 
 #endif  // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -43,8 +43,10 @@
   p.intersection_search_length = node->declare_parameter<double>(ns + "intersection_search_length");
   p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
     ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
-  p.collision_check_margins =
-    node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
+  p.objects_collision_check_margins =
+    node->declare_parameter<std::vector<double>>(ns + "objects_collision_check_margins");
+  p.back_objects_collision_check_margin =
+    node->declare_parameter<double>(ns + "back_objects_collision_check_margin");
   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");

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -628,7 +628,7 @@
   const PriorityOrder order_priority =
     determinePriorityOrder(search_priority, start_pose_candidates.size());
 
-  for (const auto & collision_check_margin : parameters_->collision_check_margins) {
+  for (const auto & collision_check_margin : parameters_->objects_collision_check_margins) {
     for (const auto & [index, planner] : order_priority) {
       if (findPullOutPath(
             start_pose_candidates[index], planner, refined_start_pose, goal_pose,
@@ -974,50 +974,54 @@
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
 
-  const auto stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
-    pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity,
-    backward_path_length, std::numeric_limits<double>::max());
+  const auto back_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
+    pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0,
+    backward_path_length);
 
   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());
+    pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity,
+    std::numeric_limits<double>::max(), 0);
 
   // 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 =
     lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length;
   const double allowed_backward_distance = std::clamp(
     current_arc_length - planner_data_->parameters.base_link2rear, 0.0,
     parameters_->max_back_distance);
 
   for (double back_distance = 0.0; back_distance <= allowed_backward_distance;
        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) 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;
     const double length_to_lane_end = std::accumulate(
       std::begin(pull_out_lanes), std::end(pull_out_lanes), 0.0,
       [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); });
     const double distance_from_lane_end = length_to_lane_end - backed_pose_arc_length;
     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 (utils::checkCollisionBetweenFootprintAndObjects(
-          local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes,
-          parameters_->collision_check_margins.back())) {
-      break;  // poses behind this has a collision, so break.
+    if (
+      (start_planner_utils::calcMinArcLengthDistanceFromEgoToObjects(
+         local_vehicle_footprint, *backed_pose, pull_out_lanes,
+         back_stop_objects_in_pull_out_lanes) < parameters_->back_objects_collision_check_margin) ||
+      (utils::checkCollisionBetweenFootprintAndObjects(
+        local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes,
+        parameters_->objects_collision_check_margins.back()))) {
+      break;  // poses behind this is too close to back static object, so break.
     }
 
     pull_out_start_pose_candidates.push_back(*backed_pose);
@@ -1027,8 +1031,8 @@
 
 PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
   const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point,
-  const double velocity_threshold, const double object_check_forward_distance,
-  const double object_check_backward_distance) const
+  const double velocity_threshold, const double object_check_backward_distance,
+  const double object_check_forward_distance) const
 {
   const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     *planner_data_->dynamic_object, velocity_threshold);
@@ -1042,8 +1046,8 @@
     pull_out_lanes, object_check_backward_distance, object_check_forward_distance);
 
   utils::path_safety_checker::filterObjectsByPosition(
-    stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance,
-    object_check_backward_distance);
+    stop_objects_in_pull_out_lanes, path.points, current_point, object_check_backward_distance,
+    object_check_forward_distance);
 
   return stop_objects_in_pull_out_lanes;
 }

planning/behavior_path_start_planner_module/src/util.cpp

@@ -17,6 +17,7 @@
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <motion_utils/trajectory/path_with_lane_id.hpp>
@@ -104,4 +105,35 @@ lanelet::ConstLanelets getPullOutLanes(
     /*forward_only_in_route*/ true);
 }
 
+double calcMinArcLengthDistanceFromEgoToObjects(
+  const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose,
+  const lanelet::ConstLanelets & lanelets, const PredictedObjects & static_objects)
+{
+  double min_distance = std::numeric_limits<double>::max();
+  const auto vehicle_footprint =
+    transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose));
+  for (const auto & obj : static_objects.objects) {
+    const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj);
+    for (const auto & obj_outer_point : obj_polygon.outer()) {
+      const auto obj_pose_arc_length = getArcLengthForPoint(lanelets, obj_outer_point);
+      for (const auto & vehicle_corner_point : vehicle_footprint) {
+        const auto vehicle_pose_arc_length = getArcLengthForPoint(lanelets, vehicle_corner_point);
+        const double distance = std::abs(obj_pose_arc_length - vehicle_pose_arc_length);
+        min_distance = std::min(min_distance, distance);
+      }
+    }
+  }
+
+  return min_distance;
+}
+
+double getArcLengthForPoint(
+  const lanelet::ConstLanelets & lanelets, const tier4_autoware_utils::Point2d & point)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = point.x();
+  pose.position.y = point.y();
+  return lanelet::utils::getArcCoordinates(lanelets, pose).length;
+}
+
 }  // namespace behavior_path_planner::start_planner_utils