diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 91121598cb3ae..6fb20a37fcebc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -176,7 +176,7 @@ class StartPlannerModule : public SceneModuleInterface std::mutex mutex_; PathWithLaneId getFullPath() const; - PathWithLaneId calcStartPoseCandidatesBackwardPath() const; + PathWithLaneId calcBackwardPathFromStartPose() const; std::vector searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const; std::shared_ptr lane_departure_checker_; @@ -194,9 +194,8 @@ class StartPlannerModule : public SceneModuleInterface std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); - static bool isOverlappedWithLane( - const lanelet::ConstLanelet & candidate_lanelet, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint); + PredictedObjects filterStopObjectsInPullOutLanes( + const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; bool hasFinishedPullOut() const; bool isBackwardDrivingComplete() const; bool isStopped(); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 77a290835fb31..287a543cfbbad 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -619,7 +619,7 @@ PathWithLaneId StartPlannerModule::generateStopPath() const PathPointWithLaneId p{}; p.point.pose = pose; p.point.longitudinal_velocity_mps = 0.0; - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); lanelet::Lanelet closest_lanelet; @@ -667,7 +667,7 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); const auto path_road_lanes = getPathRoadLanes(path); @@ -718,7 +718,7 @@ void StartPlannerModule::updatePullOutStatus() // refine start pose with pull out lanes. // 1) backward driving is not allowed: use refined pose just as start pose. // 2) backward driving is allowed: use refined pose to check if backward driving is needed. - const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath(); + const PathWithLaneId start_pose_candidates_path = calcBackwardPathFromStartPose(); const auto refined_start_pose = calcLongitudinalOffsetPose( start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); if (!refined_start_pose) return; @@ -734,11 +734,12 @@ void StartPlannerModule::updatePullOutStatus() planWithPriority( start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); if (isBackwardDrivingComplete()) { updateStatusAfterBackwardDriving(); + // should be moved to transition state current_state_ = ModuleStatus::SUCCESS; // for breaking loop } else { status_.backward_path = start_planner_utils::getBackwardPath( @@ -760,24 +761,27 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() } } -PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const +PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - // get backward shoulder path - const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose); - const double check_distance = parameters_->max_back_distance + 30.0; // buffer - auto path = planner_data_->route_handler->getCenterLinePath( - pull_out_lanes, arc_position_pose.length - check_distance, - arc_position_pose.length + check_distance); - - // lateral shift to current_pose - const double distance_from_center_line = arc_position_pose.distance; - for (auto & p : path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); + const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose); + + // common buffer distance for both front and back + static constexpr double buffer = 30.0; + const double check_distance = parameters_->max_back_distance + buffer; + + const double start_distance = arc_position_pose.length - check_distance; + const double end_distance = arc_position_pose.length + buffer; + + auto path = + planner_data_->route_handler->getCenterLinePath(pull_out_lanes, start_distance, end_distance); + + // shift all path points laterally to align with the start pose + for (auto & path_point : path.points) { + path_point.point.pose = calcOffsetPose(path_point.point.pose, 0, arc_position_pose.distance, 0); } return path; @@ -786,41 +790,30 @@ PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const std::vector StartPlannerModule::searchPullOutStartPoses( const PathWithLaneId & start_pose_candidates) const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - std::vector pull_out_start_pose{}; + const auto & start_pose = planner_data_->route_handler->getOriginalStartPose(); - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - // filter pull out lanes stop objects - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, pull_out_lanes, - utils::path_safety_checker::isPolygonOverlapLanelet); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_->th_moving_object_velocity); + const auto stop_objects_in_pull_out_lanes = + filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity); - // 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 s_current = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose).length; + const double s_current = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length; const double max_back_distance = std::clamp( s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); - // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); for (double back_distance = 0.0; back_distance <= max_back_distance; back_distance += parameters_->backward_search_resolution) { - const auto backed_pose = calcLongitudinalOffsetPose( - start_pose_candidates.points, current_pose.position, -back_distance); + const auto backed_pose = + calcLongitudinalOffsetPose(start_pose_candidates.points, start_pose.position, -back_distance); if (!backed_pose) { continue; } - // check the back pose is near the lane end const double length_to_backed_pose = 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); }); @@ -833,7 +826,7 @@ std::vector StartPlannerModule::searchPullOutStartPoses( } if (utils::checkCollisionBetweenFootprintAndObjects( - local_vehicle_footprint, *backed_pose, pull_out_lane_stop_objects, + local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, parameters_->collision_check_margin)) { break; // poses behind this has a collision, so break. } @@ -843,17 +836,18 @@ std::vector StartPlannerModule::searchPullOutStartPoses( return pull_out_start_pose; } -bool StartPlannerModule::isOverlappedWithLane( - const lanelet::ConstLanelet & candidate_lanelet, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint) +PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( + const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const { - for (const auto & point : vehicle_footprint) { - if (boost::geometry::within(point, candidate_lanelet.polygon2d().basicPolygon())) { - return true; - } - } + // filter for objects located in pull_out_lanes and moving at a speed below the threshold + const auto [objects_in_pull_out_lanes, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, pull_out_lanes, + utils::path_safety_checker::isPolygonOverlapLanelet); + const auto stop_objects_in_pull_out_lanes = utils::path_safety_checker::filterObjectsByVelocity( + objects_in_pull_out_lanes, velocity_threshold); - return false; + return stop_objects_in_pull_out_lanes; } bool StartPlannerModule::hasFinishedPullOut() const