diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index f7e3413f8f40c..7c47caf2dc90d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -118,7 +118,7 @@ class PullOutModule : public SceneModuleInterface const std::vector & start_pose_candidates, const Pose & goal_pose); void planWithPriorityOnShortBackDistance( const std::vector & start_pose_candidates, const Pose & goal_pose); - void generateStopPath(); + PathWithLaneId generateStopPath() const; void updatePullOutStatus(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 4d375afd3658d..9157079831ddc 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -176,6 +176,9 @@ BehaviorModuleOutput PullOutModule::plan() if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); + const PathWithLaneId stop_path = generateStopPath(); + output.path = std::make_shared(stop_path); + path_candidate_ = std::make_shared(stop_path); return output; } @@ -430,11 +433,11 @@ void PullOutModule::planWithPriorityOnShortBackDistance( } } -void PullOutModule::generateStopPath() +PathWithLaneId PullOutModule::generateStopPath() const { const auto & current_pose = planner_data_->self_pose->pose; constexpr double dummy_path_distance = 1.0; - const auto & moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); + const auto moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); // convert Pose to PathPointWithLaneId with 0 velocity. auto toPathPointWithLaneId = [this](const Pose & pose) { @@ -452,9 +455,15 @@ void PullOutModule::generateStopPath() path.points.push_back(toPathPointWithLaneId(current_pose)); path.points.push_back(toPathPointWithLaneId(moved_pose)); - status_.pull_out_path.partial_paths.push_back(path); - status_.pull_out_path.start_pose = current_pose; - status_.pull_out_path.end_pose = current_pose; + // generate drivable area + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); + const auto expanded_lanes = util::expandLanelets( + shorten_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + util::generateDrivableArea( + path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + + return path; } void PullOutModule::updatePullOutStatus() @@ -498,7 +507,9 @@ void PullOutModule::updatePullOutStatus() RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); status_.back_finished = true; // no need to drive backward - generateStopPath(); + status_.pull_out_path.partial_paths.push_back(generateStopPath()); + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; } checkBackFinished(); @@ -546,8 +557,10 @@ std::vector PullOutModule::searchBackedPoses() // check the back pose is near the lane end const double length_to_backed_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; - const double length_to_lane_end = - lanelet::utils::getLaneletLength2d(status_.pull_out_lanes.back()); + double length_to_lane_end = 0.0; + for (const auto & lane : status_.pull_out_lanes) { + length_to_lane_end += lanelet::utils::getLaneletLength2d(lane); + } const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; if (distance_from_lane_end < parameters_.ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE(