diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp index f38279d172009..75e55d6eab410 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/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 plan(const Pose & start_pose, const Pose & end_pose) override; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 68a5d8100a271..138aaaddd6981 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/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); - PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; + PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 0144fdd45ae50..92baa8191b5fd 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/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 #include @@ -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 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 planner_data_; vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; StartPlannerParameters parameters_; + double collision_check_margin_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 2d50ae189dc13..f831eeda9778d 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/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 & lane_departure_checker); - PlannerType getPlannerType() override { return PlannerType::SHIFT; }; + PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 147632329d926..eb8d1b4c46fe2 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/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 @@ -50,6 +51,8 @@ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & 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_ diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index a5aeb5b257d39..0f7587fdff476 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -114,6 +114,10 @@ std::optional 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 diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 274fe19e04618..c6a56139e63d2 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -114,6 +114,10 @@ std::optional 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; } 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 index bb47182d953cc..176f804db203c 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/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 & 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 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) diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index b34398083a95c..ae78e68c9d8c3 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/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