From 8f814b4244a67ecb04d87c151d71b39da9b1d88d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 13 Aug 2023 19:02:37 +0900 Subject: [PATCH] fix(start_planner): fix geometric parallel parking lane ids (#4603) * fix(start_planner): fix geometric parallel parking lane ids Signed-off-by: kosuke55 * fix stop path drivable lanes Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.hpp | 2 +- .../start_planner/start_planner_module.cpp | 38 ++++++++++++--- .../geometric_parallel_parking.cpp | 48 ++++++++++++------- 3 files changed, 64 insertions(+), 24 deletions(-) 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 1405e3cd1f10e..3214de2c37bd7 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 @@ -143,7 +143,7 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; - lanelet::ConstLanelets getPathLanes(const PathWithLaneId & path) const; + lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); static bool isOverlappedWithLane( 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 32398c7944435..fb471a4675fa7 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 @@ -534,19 +534,23 @@ PathWithLaneId StartPlannerModule::generateStopPath() const return path; } -lanelet::ConstLanelets StartPlannerModule::getPathLanes(const PathWithLaneId & path) const +lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId & path) const { + const auto & route_handler = planner_data_->route_handler; + const auto & lanelet_layer = route_handler->getLaneletMapPtr()->laneletLayer; + std::vector lane_ids; for (const auto & p : path.points) { for (const auto & id : p.lane_ids) { + if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) { + continue; + } if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { lane_ids.push_back(id); } } } - const auto & lanelet_layer = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer; - lanelet::ConstLanelets path_lanes; path_lanes.reserve(lane_ids.size()); for (const auto & id : lane_ids) { @@ -561,7 +565,22 @@ lanelet::ConstLanelets StartPlannerModule::getPathLanes(const PathWithLaneId & p std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { - return utils::generateDrivableLanesWithShoulderLanes(getPathLanes(path), status_.pull_out_lanes); + const auto path_road_lanes = getPathRoadLanes(path); + + if (!path_road_lanes.empty()) { + return utils::generateDrivableLanesWithShoulderLanes( + getPathRoadLanes(path), status_.pull_out_lanes); + } + + // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes + std::vector drivable_lanes; + for (const auto & lane : status_.pull_out_lanes) { + DrivableLanes drivable_lane; + drivable_lane.right_lane = lane; + drivable_lane.left_lane = lane; + drivable_lanes.push_back(drivable_lane); + } + return drivable_lanes; } void StartPlannerModule::updatePullOutStatus() @@ -702,10 +721,15 @@ bool StartPlannerModule::hasFinishedPullOut() const const auto current_pose = planner_data_->self_odometry->pose.pose; // check that ego has passed pull out end point - const auto path_lanes = getPathLanes(getFullPath()); - const auto arclength_current = lanelet::utils::getArcCoordinates(path_lanes, current_pose); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + const auto arclength_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose); const auto arclength_pull_out_end = - lanelet::utils::getArcCoordinates(path_lanes, status_.pull_out_path.end_pose); + lanelet::utils::getArcCoordinates(current_lanes, status_.pull_out_path.end_pose); // offset to not finish the module before engage constexpr double offset = 0.1; diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 678e19e616f26..4f8c7ff9a94b4 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -368,7 +368,8 @@ std::vector GeometricParallelParking::planOneTrial( { clearPaths(); - const auto common_params = planner_data_->parameters; + const auto & common_params = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); const double self_yaw = tf2::getYaw(start_pose.orientation); @@ -435,34 +436,49 @@ std::vector GeometricParallelParking::planOneTrial( PathWithLaneId path_turn_left = generateArcPath( Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), arc_path_interval, is_forward, is_forward); - path_turn_left.header = planner_data_->route_handler->getRouteHeader(); + path_turn_left.header = route_handler->getRouteHeader(); PathWithLaneId path_turn_right = generateArcPath( Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, arc_path_interval, !is_forward, is_forward); - path_turn_right.header = planner_data_->route_handler->getRouteHeader(); + path_turn_right.header = route_handler->getRouteHeader(); - auto setLaneIds = [lanes](PathPointWithLaneId & p) { - for (const auto & lane : lanes) { - p.lane_ids.push_back(lane.id()); + // Need to add straight path to last right_turning for parking in parallel + if (std::abs(end_pose_offset) > 0) { + PathPointWithLaneId straight_point{}; + straight_point.point.pose = goal_pose; + // setLaneIds(straight_point); + path_turn_right.points.push_back(straight_point); + } + + // Populate lane ids for a given path. + // It checks if each point in the path is within a lane + // and if its ID hasn't been added yet, it appends the ID to the container. + std::vector path_lane_ids; + const auto populateLaneIds = [&](const auto & path) { + for (const auto & p : path.points) { + for (const auto & lane : lanes) { + if ( + lanelet::utils::isInLanelet(p.point.pose, lane) && + std::find(path_lane_ids.begin(), path_lane_ids.end(), lane.id()) == path_lane_ids.end()) { + path_lane_ids.push_back(lane.id()); + } + } } }; - auto setLaneIdsToPath = [setLaneIds](PathWithLaneId & path) { + populateLaneIds(path_turn_left); + populateLaneIds(path_turn_right); + + // Set lane ids to each point in a given path. + // It assigns the accumulated lane ids from path_lane_ids to each point's lane_ids member. + const auto setLaneIdsToPath = [&](PathWithLaneId & path) { for (auto & p : path.points) { - setLaneIds(p); + p.lane_ids = path_lane_ids; } }; setLaneIdsToPath(path_turn_left); setLaneIdsToPath(path_turn_right); - // Need to add straight path to last right_turning for parking in parallel - if (std::abs(end_pose_offset) > 0) { - PathPointWithLaneId straight_point{}; - straight_point.point.pose = goal_pose; - setLaneIds(straight_point); - path_turn_right.points.push_back(straight_point); - } - // generate arc path vector paths_.push_back(path_turn_left); paths_.push_back(path_turn_right);