feat(start_planner): keep distance against front objects (autowarefou…

…ndation#5983) * refactor extractCollisionCheckPath Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]> Co-authored-by: Kosuke Takeuchi <[email protected]>
tier4 · Jan 11, 2024 · f9dcafb · f9dcafb
1 parent 912bfd7
commit f9dcafb
Show file tree

Hide file tree

Showing 6 changed files with 18 additions and 6 deletions.
diff --git a/planning/behavior_path_start_planner_module/README.md b/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

diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/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

diff --git a/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/..._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);

diff --git a/...rt_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/...rt_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};
 

diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/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

diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/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;