feat(start_planner): move collision check to plan method (autowarefoundation#6564)

* move collision check to a member function of planner base

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* refactor

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add empty polygon and point checks

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp

@@ -38,7 +38,7 @@ class FreespacePullOut : public PullOutPlannerBase
     rclcpp::Node & node, const StartPlannerParameters & parameters,
     const vehicle_info_util::VehicleInfo & vehicle_info);
 
-  PlannerType getPlannerType() override { return PlannerType::FREESPACE; }
+  PlannerType getPlannerType() const override { return PlannerType::FREESPACE; }
 
   std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & end_pose) override;
 

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp

@@ -34,7 +34,7 @@ class GeometricPullOut : public PullOutPlannerBase
     rclcpp::Node & node, const StartPlannerParameters & parameters,
     const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
 
-  PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; };
+  PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; };
   std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   GeometricParallelParking planner_;

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp

@@ -16,8 +16,10 @@
 #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_
 
 #include "behavior_path_planner_common/data_manager.hpp"
+#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_start_planner_module/data_structs.hpp"
 #include "behavior_path_start_planner_module/pull_out_path.hpp"
+#include "behavior_path_start_planner_module/util.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -54,14 +56,44 @@ class PullOutPlannerBase
     planner_data_ = planner_data;
   }
 
-  virtual PlannerType getPlannerType() = 0;
+  void setCollisionCheckMargin(const double collision_check_margin)
+  {
+    collision_check_margin_ = collision_check_margin;
+  };
+  virtual PlannerType getPlannerType() const = 0;
   virtual std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) = 0;
 
 protected:
+  bool isPullOutPathCollided(
+    behavior_path_planner::PullOutPath & pull_out_path,
+    double collision_check_distance_from_end) const
+  {
+    // check for collisions
+    const auto & dynamic_objects = planner_data_->dynamic_object;
+    const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
+      planner_data_,
+      planner_data_->parameters.backward_path_length + parameters_.max_back_distance);
+    const auto & vehicle_footprint = vehicle_info_.createFootprint();
+    // extract stop objects in pull out lane for collision check
+    const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
+      *dynamic_objects, parameters_.th_moving_object_velocity);
+    auto [pull_out_lane_stop_objects, others] =
+      utils::path_safety_checker::separateObjectsByLanelets(
+        stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+    utils::path_safety_checker::filterObjectsByClass(
+      pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation);
+
+    return utils::checkCollisionBetweenPathFootprintsAndObjects(
+      vehicle_footprint_,
+      behavior_path_planner::start_planner_utils::extractCollisionCheckSection(
+        pull_out_path, collision_check_distance_from_end),
+      pull_out_lane_stop_objects, collision_check_margin_);
+  };
   std::shared_ptr<const PlannerData> planner_data_;
   vehicle_info_util::VehicleInfo vehicle_info_;
   LinearRing2d vehicle_footprint_;
   StartPlannerParameters parameters_;
+  double collision_check_margin_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp

@@ -36,7 +36,7 @@ class ShiftPullOut : public PullOutPlannerBase
     rclcpp::Node & node, const StartPlannerParameters & parameters,
     std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);
 
-  PlannerType getPlannerType() override { return PlannerType::SHIFT; };
+  PlannerType getPlannerType() const override { return PlannerType::SHIFT; };
   std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   std::vector<PullOutPath> calcPullOutPaths(

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp

@@ -19,6 +19,7 @@
 #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
+#include "behavior_path_start_planner_module/pull_out_path.hpp"
 
 #include <route_handler/route_handler.hpp>
 
@@ -50,6 +51,8 @@ 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);
+PathWithLaneId extractCollisionCheckSection(
+  const PullOutPath & path, const double collision_check_distance_from_end);
 }  // namespace behavior_path_planner::start_planner_utils
 
 #endif  // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_

planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp

@@ -114,6 +114,10 @@ std::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, const
   output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose;
   output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose;
 
+  if (isPullOutPathCollided(output, parameters_.geometric_collision_check_distance_from_end)) {
+    return {};
+  }
+
   return output;
 }
 }  // namespace behavior_path_planner

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -114,6 +114,10 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
     shift_path.points = cropped_path.points;
     shift_path.header = planner_data_->route_handler->getRouteHeader();
 
+    if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) {
+      continue;
+    }
+
     return pull_out_path;
   }
 

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -767,22 +767,11 @@ bool StartPlannerModule::findPullOutPath(
   const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
   const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin)
 {
-  const auto & dynamic_objects = planner_data_->dynamic_object;
-  const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
-    planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
-  const auto & vehicle_footprint = vehicle_info_.createFootprint();
-  // extract stop objects in pull out lane for collision check
-  const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
-    *dynamic_objects, parameters_->th_moving_object_velocity);
-  auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets(
-    stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
-  utils::path_safety_checker::filterObjectsByClass(
-    pull_out_lane_stop_objects, parameters_->object_types_to_check_for_path_generation);
-
   // if start_pose_candidate is far from refined_start_pose, backward driving is necessary
   const bool backward_is_unnecessary =
     tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01;
 
+  planner->setCollisionCheckMargin(collision_check_margin);
   planner->setPlannerData(planner_data_);
   const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose);
 
@@ -791,13 +780,6 @@ bool StartPlannerModule::findPullOutPath(
     return false;
   }
 
-  // check collision
-  if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-        vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()),
-        pull_out_lane_stop_objects, collision_check_margin)) {
-    return false;
-  }
-
   if (backward_is_unnecessary) {
     updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType());
     return true;
@@ -808,49 +790,6 @@ bool StartPlannerModule::findPullOutPath(
   return true;
 }
 
-PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
-  const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type)
-{
-  const std::map<PlannerType, double> collision_check_distances = {
-    {behavior_path_planner::PlannerType::SHIFT,
-     parameters_->shift_collision_check_distance_from_end},
-    {behavior_path_planner::PlannerType::GEOMETRIC,
-     parameters_->geometric_collision_check_distance_from_end}};
-
-  const double collision_check_distance_from_end = collision_check_distances.at(planner_type);
-
-  PathWithLaneId full_path;
-  for (const auto & partial_path : path.partial_paths) {
-    full_path.points.insert(
-      full_path.points.end(), partial_path.points.begin(), partial_path.points.end());
-  }
-
-  // Find the start index for collision check section based on the shift start pose
-  const auto shift_start_idx =
-    motion_utils::findNearestIndex(full_path.points, path.start_pose.position);
-
-  // Find the end index for collision check section based on the end pose and collision check
-  // distance
-  const auto collision_check_end_idx = [&]() -> size_t {
-    const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose(
-      full_path.points, path.end_pose.position, collision_check_distance_from_end);
-
-    return end_pose_offset
-             ? motion_utils::findNearestIndex(full_path.points, end_pose_offset->position)
-             : full_path.points.size() - 1;  // Use the last point if offset pose is not calculable
-  }();
-
-  // Extract the collision check section from the full path
-  PathWithLaneId collision_check_section;
-  if (shift_start_idx < collision_check_end_idx) {
-    collision_check_section.points.assign(
-      full_path.points.begin() + shift_start_idx,
-      full_path.points.begin() + collision_check_end_idx + 1);
-  }
-
-  return collision_check_section;
-}
-
 void StartPlannerModule::updateStatusWithCurrentPath(
   const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
   const behavior_path_planner::PlannerType & planner_type)

planning/behavior_path_start_planner_module/src/util.cpp

@@ -104,4 +104,39 @@ lanelet::ConstLanelets getPullOutLanes(
     /*forward_only_in_route*/ true);
 }
 
+PathWithLaneId extractCollisionCheckSection(
+  const PullOutPath & path, const double collision_check_distance_from_end)
+{
+  PathWithLaneId full_path;
+  for (const auto & partial_path : path.partial_paths) {
+    full_path.points.insert(
+      full_path.points.end(), partial_path.points.begin(), partial_path.points.end());
+  }
+
+  // Find the start index for collision check section based on the shift start pose
+  const auto shift_start_idx =
+    motion_utils::findNearestIndex(full_path.points, path.start_pose.position);
+
+  // Find the end index for collision check section based on the end pose and collision check
+  // distance
+  const auto collision_check_end_idx = [&]() -> size_t {
+    const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose(
+      full_path.points, path.end_pose.position, collision_check_distance_from_end);
+
+    return end_pose_offset
+             ? motion_utils::findNearestIndex(full_path.points, end_pose_offset->position)
+             : full_path.points.size() - 1;  // Use the last point if offset pose is not calculable
+  }();
+
+  // Extract the collision check section from the full path
+  PathWithLaneId collision_check_section;
+  if (shift_start_idx < collision_check_end_idx) {
+    collision_check_section.points.assign(
+      full_path.points.begin() + shift_start_idx,
+      full_path.points.begin() + collision_check_end_idx + 1);
+  }
+
+  return collision_check_section;
+}
+
 }  // namespace behavior_path_planner::start_planner_utils