From 8f66df0505d4dbfa34230084ed24fe370fefeffa Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 15 Dec 2023 01:38:22 +0900 Subject: [PATCH] feat(avoidance/goal_planner): execute avoidance and pull over simultaneously Signed-off-by: kosuke55 tmp Signed-off-by: kosuke55 tmp Signed-off-by: kosuke55 tmp Signed-off-by: kosuke55 tmp tmp Signed-off-by: kosuke55 tmp Signed-off-by: kosuke55 fix tmp Signed-off-by: kosuke55 feat(mission_planner): check shoulder lanelets for check_goal_footprint Signed-off-by: kosuke55 tmp Signed-off-by: kosuke55 tmp tmp --- .../src/manager.cpp | 2 + .../behavior_path_avoidance_module/README.md | 5 +- .../config/avoidance.param.yaml | 6 +- .../data_structs.hpp | 12 +- .../parameter_helper.hpp | 4 +- .../src/shift_line_generator.cpp | 77 ++++++++-- .../src/utils.cpp | 22 +-- .../goal_planner_module.hpp | 4 + .../pull_over_planner_base.hpp | 7 + .../shift_pull_over.hpp | 5 + .../util.hpp | 12 ++ .../src/goal_planner_module.cpp | 131 +++++++++++++----- .../src/shift_pull_over.cpp | 122 ++++++++++++++-- .../src/util.cpp | 68 +++++++++ .../utils/utils.hpp | 4 + .../src/utils/utils.cpp | 64 +++++++++ .../src/lanelet2_plugins/default_planner.cpp | 6 +- .../lanelet2_plugins/utility_functions.cpp | 50 +++++-- .../lanelet2_plugins/utility_functions.hpp | 3 +- 19 files changed, 500 insertions(+), 104 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 3f810710ef37b..35aa14ccb56a6 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -119,6 +119,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_check_return_pose_distance = + getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); p.threshold_distance_object_is_on_center = getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = diff --git a/planning/behavior_path_avoidance_module/README.md b/planning/behavior_path_avoidance_module/README.md index 20c0985f0f333..399cf918f7c99 100644 --- a/planning/behavior_path_avoidance_module/README.md +++ b/planning/behavior_path_avoidance_module/README.md @@ -833,7 +833,10 @@ namespace: `avoidance.target_filtering.` | object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | | object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | | object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | +| object_check_goal_distance | [m] | double | If the distance between object and goal position is less than this parameter, the module do not | +| return center line target. | 20.0 | +| object_check_return_pose_distance | [m] | double | If the distance between object and return position is less than this parameter, the module do | +| // not return center line target. | 20.0 | | object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | | object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | | object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 2d38cebd591f5..5e2644de78f3f 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -122,7 +122,8 @@ motorcycle: true # [-] pedestrian: true # [-] # detection range - object_check_goal_distance: 20.0 # [m] + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] object_check_shiftable_ratio: 0.6 # [-] @@ -215,9 +216,6 @@ nominal_avoidance_speed: 8.33 # [m/s] # return dead line return_dead_line: - goal: - enable: true # [-] - buffer: 30.0 # [m] traffic_light: enable: true # [-] buffer: 3.0 # [m] diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 1d1494b7719c3..13dedc770fda8 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -164,10 +164,14 @@ struct AvoidanceParameters double object_check_backward_distance{0.0}; double object_check_yaw_deviation{0.0}; - // if the distance between object and goal position is less than this parameter, the module ignore - // the object. + // if the distance between object and goal position is less than this parameter, the module do not + // return center line. double object_check_goal_distance{0.0}; + // if the distance between object and return position is less than this parameter, the module do + // not return center line. + double object_check_return_pose_distance{0.0}; + // use in judge whether the vehicle is parking object on road shoulder double object_check_shiftable_ratio{0.0}; @@ -462,14 +466,14 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(AvoidLine avoid_line, AvoidLine return_line) + AvoidOutline(AvoidLine avoid_line, const std::optional return_line) : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)} { } AvoidLine avoid_line{}; - AvoidLine return_line{}; + std::optional return_line{}; AvoidLineArray middle_lines{}; }; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index b9af50ce76eb5..eb3ec34e15ac7 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -122,6 +122,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_check_return_pose_distance = + getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); p.threshold_distance_object_is_on_center = getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = @@ -270,10 +272,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // avoidance maneuver (return shift dead line) { const std::string ns = "avoidance.avoidance.return_dead_line."; - p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); p.enable_dead_line_for_traffic_light = getOrDeclareParameter(*node, ns + "traffic_light.enable"); - p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); p.dead_line_buffer_for_traffic_light = getOrDeclareParameter(*node, ns + "traffic_light.buffer"); } diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index dabb0d7257555..e697416c1ede5 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -81,7 +81,9 @@ AvoidLineArray toArray(const AvoidOutlines & outlines) AvoidLineArray ret{}; for (const auto & outline : outlines) { ret.push_back(outline.avoid_line); - ret.push_back(outline.return_line); + if (outline.return_line.has_value()) { + ret.push_back(outline.return_line.value()); + } std::for_each( outline.middle_lines.begin(), outline.middle_lines.end(), @@ -341,7 +343,20 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_return.object_on_right = utils::avoidance::isOnRight(o); } - if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + const auto goal_pose = data_->route_handler->getGoalPose(); + const double goal_longitudinal_distance = + motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position); + const bool return_to_goal = std::abs(al_return.end_longitudinal - goal_longitudinal_distance) < + parameters_->object_check_return_pose_distance; + + const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; + const bool is_near_goal = tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) < + parameters_->object_check_goal_distance; + + if (is_near_goal || return_to_goal) { + // if the object is close to goal or ego is about to return near GOAL, do not reuturn + outlines.emplace_back(al_avoid, std::nullopt); + } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { outlines.emplace_back(al_avoid, al_return); } else { o.reason = "InvalidShiftLine"; @@ -637,35 +652,43 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( auto & next_outline = outlines.at(i); const auto & return_line = last_outline.return_line; - const auto & avoid_line = next_outline.avoid_line; + if (!return_line.has_value()) { + ret.push_back(outlines.at(i)); + break; + } - if (no_conflict(return_line, avoid_line)) { + const auto & avoid_line = next_outline.avoid_line; + if (no_conflict(return_line.value(), avoid_line)) { ret.push_back(outlines.at(i)); continue; } - const auto merged_shift_line = merge(return_line, avoid_line, generateUUID()); + const auto merged_shift_line = merge(return_line.value(), avoid_line, generateUUID()); if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) { ret.push_back(outlines.at(i)); continue; } - if (same_side_shift(return_line, avoid_line)) { + if (same_side_shift(return_line.value(), avoid_line)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { + if ( + within(return_line.value(), avoid_line.end_idx) && + within(avoid_line, return_line->start_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { + if ( + within(return_line.value(), avoid_line.start_idx) && + within(avoid_line, return_line->end_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); @@ -686,7 +709,10 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( for (auto & outline : ret) { if (outline.middle_lines.empty()) { - const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID()); + const auto new_line = + outline.return_line.has_value() + ? fill(outline.avoid_line, outline.return_line.value(), generateUUID()) + : outline.avoid_line; outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -701,8 +727,11 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); - if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { - const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID()); + if ( + outline.return_line.has_value() && + outline.middle_lines.back().end_longitudinal < outline.return_line->start_longitudinal) { + const auto new_line = + fill(outline.middle_lines.back(), outline.return_line.value(), generateUUID()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -973,6 +1002,18 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const bool has_candidate_point = !ret.empty(); const bool has_registered_point = last_.has_value(); + const auto has_object_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + data_->route_handler->getGoalPose().position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_object_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "object near goal exists so skip adding return shift"); + return ret; + } + const auto exist_unavoidable_object = std::any_of( data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); @@ -1027,6 +1068,17 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( } } + // if last shift line is near the objects, do not add return shift. + if (std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + last_sl.end.position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + })) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "last shift line is near the objects"); + return ret; + } + // There already is a shift point candidates to go back to center line, but it could be too sharp // due to detection noise or timing. // Here the return-shift from ego is added for the in case. @@ -1070,8 +1122,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( return ret; } - const auto remaining_distance = std::min( - arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); + const auto remaining_distance = data.to_return_point; // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 83e96d83b995f..b9d083b28abdd 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -678,13 +678,6 @@ bool isSatisfiedWithCommonCondition( return false; } - if ( - object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > - to_goal_distance) { - object.reason = "TooNearToGoal"; - return false; - } - return true; } @@ -1663,7 +1656,9 @@ void fillAdditionalInfoFromLongitudinal( { for (auto & outline : outlines) { fillAdditionalInfoFromLongitudinal(data, outline.avoid_line); - fillAdditionalInfoFromLongitudinal(data, outline.return_line); + if (outline.return_line.has_value()) { + fillAdditionalInfoFromLongitudinal(data, outline.return_line.value()); + } std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) { fillAdditionalInfoFromLongitudinal(data, line); @@ -2145,17 +2140,6 @@ double calcDistanceToReturnDeadLine( } } - // dead line for goal - if (parameters->enable_dead_line_for_goal) { - if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { - const auto & ego_pos = planner_data->self_odometry->pose.pose.position; - const auto to_goal_distance = - calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); - distance_to_return_dead_line = std::min( - distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal); - } - } - return distance_to_return_dead_line; } diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index e90162c958be5..9fb9c17f1ca70 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -472,6 +472,10 @@ class GoalPlannerModule : public SceneModuleInterface // new turn signal TurnSignalInfo calcTurnSignalInfo() const; + std::optional last_previous_module_output_{}; + bool hasPreviousModulePathShapeChanged() const; + bool hasDeviatedFromLastPreviousModulePath() const; + // timer for generating pull over path candidates in a separate thread void onTimer(); void onFreespaceParkingTimer(); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ddd7c93998654..9c51623bbf24f 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -119,6 +119,11 @@ class PullOverPlannerBase } virtual ~PullOverPlannerBase() = default; + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + void setPlannerData(const std::shared_ptr planner_data) { planner_data_ = planner_data; @@ -132,6 +137,8 @@ class PullOverPlannerBase vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; GoalPlannerParameters parameters_; + + BehaviorModuleOutput previous_module_output_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index bd19dc2e87de0..e4700d1d32559 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -43,6 +43,11 @@ class ShiftPullOver : public PullOverPlannerBase protected: PathWithLaneId generateReferencePath( const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; + std::optional extendPrevModulePath( + const PathWithLaneId & road_lane_reference_path_to_shift_end, const Pose & shift_end_pose, + const PathWithLaneId & prev_module_path) const; + std::optional cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index a7aec66f64363..7b4715dbcbd18 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -51,6 +51,18 @@ PredictedObjects filterObjectsByLateralDistance( bool isAllowedGoalModification(const std::shared_ptr & route_handler); bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path); +bool isRerencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_thresh); + +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const double extend_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const Pose & extend_pose); // debug MarkerArray createPullOverAreaMarkerArray( diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 493e7ec57f063..2b85013ae8ef1 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -137,11 +137,34 @@ void GoalPlannerModule::updateOccupancyGrid() occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } +bool GoalPlannerModule::hasPreviousModulePathShapeChanged() const +{ + if (!last_previous_module_output_) { + return true; + } + + const auto current_path = getPreviousModuleOutput().path; + + // the terminal distance is far + return calcDistance2d( + last_previous_module_output_->path.points.back().point, + current_path.points.back().point) > 0.3; +} + +bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const +{ + if (!last_previous_module_output_) { + return true; + } + return std::abs(motion_utils::calcLateralOffset( + last_previous_module_output_->path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { - // already generated pull over candidate paths - if (!thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -156,10 +179,26 @@ void GoalPlannerModule::onTimer() return; } - if (getCurrentStatus() == ModuleStatus::IDLE) { + // check if new pull over path candidates are needed to be generated + const bool need_update = std::invoke([&]() { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + return true; + } + if (hasPreviousModulePathShapeChanged()) { + RCLCPP_ERROR(getLogger(), "has previous module path shape changed"); + return true; + } + if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) { + RCLCPP_ERROR(getLogger(), "has deviated from last previous module path"); + return true; + } + return false; + }); + if (!need_update) { return; } + const auto previous_module_output = getPreviousModuleOutput(); const auto goal_candidates = thread_safe_data_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose @@ -174,8 +213,9 @@ void GoalPlannerModule::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { planner->setPlannerData(planner_data_); + planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path && isCrossingPossible(*pull_over_path)) { + if (pull_over_path) { pull_over_path->goal_id = goal_candidate.id; pull_over_path->id = path_candidates.size(); path_candidates.push_back(*pull_over_path); @@ -189,9 +229,21 @@ void GoalPlannerModule::onTimer() } } }; + + // todo: currently non centerline input path is supported only by shift pull over + const bool is_center_line_input_path = goal_planner_utils::isRerencePath( + previous_module_output.reference_path, previous_module_output.path, 0.1); + RCLCPP_DEBUG( + getLogger(), "the input path of pull over planner is center line: %d", + is_center_line_input_path); + // plan candidate paths and set them to the member variable if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } @@ -199,6 +251,10 @@ void GoalPlannerModule::onTimer() } else if (parameters_->path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } planCandidatePaths(planner, goal_candidate); } } @@ -214,7 +270,10 @@ void GoalPlannerModule::onTimer() const std::lock_guard lock(mutex_); thread_safe_data_.set_pull_over_path_candidates(path_candidates); thread_safe_data_.set_closest_start_pose(closest_start_pose); + RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); } + + last_previous_module_output_ = previous_module_output; } void GoalPlannerModule::onFreespaceParkingTimer() @@ -355,7 +414,9 @@ bool GoalPlannerModule::isExecutionRequested() const // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const lanelet::ConstLanelets current_lanes = + utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); const bool goal_is_in_current_lanes = std::any_of( current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) { @@ -1154,11 +1215,15 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return PathWithLaneId{}; } - // generate reference path - const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; - const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; - auto reference_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + // extend previous module path to generate reference path for stop path + const auto reference_path = std::invoke([&]() -> PathWithLaneId { + const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; + const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); + const double s_end = s_current + common_parameters.forward_path_length; + return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + }); + const auto extended_prev_path = goal_planner_utils::extendPath( + getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length); // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible @@ -1166,7 +1231,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); const auto decel_pose = calcLongitudinalOffsetPose( - reference_path.points, closest_goal_candidate.goal_pose.position, + extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); // if not approved stop road lane. @@ -1194,7 +1259,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const } // if stop pose is closer than min_stop_distance, stop as soon as possible - const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, *stop_pose); + const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, *stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; @@ -1205,55 +1270,43 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const } // slow down for turn signal, insert stop point to stop_pose - decelerateForTurnSignal(*stop_pose, reference_path); + auto stop_path = extended_prev_path; + decelerateForTurnSignal(*stop_pose, stop_path); stop_pose_ = *stop_pose; // for debug wall marker // slow down before the search area. if (decel_pose) { - decelerateBeforeSearchStart(*decel_pose, reference_path); - return reference_path; + decelerateBeforeSearchStart(*decel_pose, stop_path); + return stop_path; } - // if already passed the decel pose, set pull_over_velocity to reference_path. + // if already passed the decel pose, set pull_over_velocity to stop_path. const auto min_decel_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, pull_over_velocity); - for (auto & p : reference_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); + for (auto & p : stop_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(stop_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { continue; } p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); } - return reference_path; + return stop_path; } PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const { - const auto & route_handler = planner_data_->route_handler; - const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & common_parameters = planner_data_->parameters; - - // generate stop reference path - const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); - - const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; - const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; - auto stop_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); - // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return stop_path; + return getPreviousModuleOutput().path; } // set stop point + auto stop_path = getPreviousModuleOutput().path; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { @@ -1895,6 +1948,14 @@ void GoalPlannerModule::setDebugData() add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } + // Visualize previous module output + add(createPathMarkerArray( + getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); + if (last_previous_module_output_.has_value()) { + add(createPathMarkerArray( + last_previous_module_output_.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); + } + // Visualize path and related pose if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 712da5aa03a4e..7d9a7da173798 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -46,10 +46,10 @@ std::optional ShiftPullOver::plan(const Pose & goal_pose) const int shift_sampling_num = parameters_.shift_sampling_num; const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; - // get road and shoulder lanes - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_search_length, forward_search_length, + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + previous_module_output_.path, planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, backward_search_length, forward_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { @@ -99,6 +99,68 @@ PathWithLaneId ShiftPullOver::generateReferencePath( return road_lane_reference_path; } +// generate path after previous module path terminal pose by adding clipped shifted road lane +std::optional ShiftPullOver::extendPrevModulePath( + const PathWithLaneId & road_lane_reference_path_to_shift_end, const Pose & shift_end_pose, + const PathWithLaneId & prev_module_path) const +{ + const auto & ref_path = road_lane_reference_path_to_shift_end; + const auto & prev_path = prev_module_path; + const auto & prev_terminal_pose = prev_path.points.back().point.pose; + + // generate clipped road lane reference path from previous module path terminal pose to shift end + const size_t prev_module_path_terminal_idx = + motion_utils::findNearestSegmentIndex(ref_path.points, prev_terminal_pose.position) + 1; + const double distance_to_terminal_to_shift_end = motion_utils::calcSignedArcLength( + ref_path.points, prev_module_path_terminal_idx, shift_end_pose.position); + PathWithLaneId clipped_path{}; + clipped_path.points = motion_utils::cropPoints( + ref_path.points, prev_terminal_pose.position, prev_module_path_terminal_idx, + distance_to_terminal_to_shift_end, 0.0); + + // shift clipped path to previous module path terminal pose + const double lateral_shift_from_reference_path = + motion_utils::calcLateralOffset(ref_path.points, prev_terminal_pose.position); + for (auto & p : clipped_path.points) { + p.point.pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); + } + + auto extended_prev_module_path = prev_module_path; + for (const auto & p : clipped_path.points) { + extended_prev_module_path.points.push_back(p); + } + extended_prev_module_path.points = + motion_utils::removeOverlapPoints(extended_prev_module_path.points); + + return extended_prev_module_path; +} + +std::optional ShiftPullOver::cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const +{ + // clip previous module path to shift end pose nearest segment index + const size_t shift_end_idx = + motion_utils::findNearestSegmentIndex(prev_module_path.points, shift_end_pose.position); + std::vector clipped_points{ + prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected shift end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength( + prev_module_path.points, shift_end_idx, shift_end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_prev_module_path = prev_module_path; + clipped_prev_module_path.points = clipped_points; + + return clipped_prev_module_path; +} + std::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const @@ -106,33 +168,55 @@ std::optional ShiftPullOver::generatePullOverPath( const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; + // shift end pose is longitudinal offset from goal pose to imporove parking angle accuracy const Pose shift_end_pose = tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); - // generate road lane reference path to shift end + // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval); + const auto prev_module_path = utils::resamplePathWithSpline( + previous_module_output_.path, parameters_.center_line_path_interval); + const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; + + // process previous module path for path shifter input path + // case1) extend path if shift end pose is behind of previous module path terminal pose + // case2) crop path if shift end pose is ahead of previous module path terminal pose + const auto processed_prev_module_path = std::invoke([&]() -> std::optional { + const bool extend_previous_module_path = + lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > + lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; + if (extend_previous_module_path) { // case1 + return extendPrevModulePath( + road_lane_reference_path_to_shift_end, shift_end_pose, prev_module_path); + } else { // case2 + return cropPrevModulePath(prev_module_path, shift_end_pose); + } + }); + if (!processed_prev_module_path || processed_prev_module_path->points.empty()) { + return {}; + } // calculate shift length - const Pose & shift_end_pose_road_lane = - road_lane_reference_path_to_shift_end.points.back().point.pose; + const Pose & shift_end_pose_prev_module_path = + processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane) + tier4_autoware_utils::inverseTransformPoint( + shift_end_pose.position, shift_end_pose_prev_module_path) .y; // calculate shift start pose on road lane const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( - road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance); + processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( - road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position, + processed_prev_module_path->points, shift_end_pose_prev_module_path.position, -before_shifted_pull_over_distance); - if (!shift_start_pose) return {}; // set path shifter and generate shifted path PathShifter path_shifter{}; - path_shifter.setPath(road_lane_reference_path_to_shift_end); + path_shifter.setPath(processed_prev_module_path.value()); ShiftLine shift_line{}; shift_line.start = *shift_start_pose; shift_line.end = shift_end_pose; @@ -140,7 +224,9 @@ std::optional ShiftPullOver::generatePullOverPath( path_shifter.addShiftLine(shift_line); ShiftedPath shifted_path{}; const bool offset_back = true; // offset front side from reference path - if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + if (!path_shifter.generate(&shifted_path, offset_back)) { + return {}; + } shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points); motion_utils::insertOrientation(shifted_path.path.points, true); @@ -208,8 +294,13 @@ std::optional ShiftPullOver::generatePullOverPath( pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; - pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); + pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); + pull_over_path.debug_poses.push_back(goal_pose); + pull_over_path.debug_poses.push_back(shift_end_pose); + pull_over_path.debug_poses.push_back( + road_lane_reference_path_to_shift_end.points.back().point.pose); + pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); // check if the parking path will leave lanes const auto drivable_lanes = @@ -218,8 +309,11 @@ std::optional ShiftPullOver::generatePullOverPath( const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); if (lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) { + utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) { + std::cerr << "ShiftPullOver: path will leave lane" << std::endl; return {}; } diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 1a1e66f85b403..19b7ba321d427 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -214,4 +214,72 @@ bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_h return false; } +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path) +{ + double lateral_deviation = 0.0; + for (const auto & target_point : target_path.points) { + const size_t nearest_index = + motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); + lateral_deviation = std::max( + lateral_deviation, + std::abs(tier4_autoware_utils::calcLateralDeviation( + reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); + } + return lateral_deviation; +} + +bool isRerencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_threshold) +{ + return calcLateralDeviationBetweenPaths(reference_path, target_path) < + lateral_deviation_threshold; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + + // generate clipped road lane reference path from previous module path terminal pose to shift end + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position) + 1; + + PathWithLaneId clipped_path{}; + clipped_path.points = motion_utils::cropPoints( + reference_path.points, target_terminal_pose.position, target_path_terminal_idx, extend_length, + 0.0); + + // shift clipped path to previous module path terminal pose + const double lateral_shift_from_reference_path = + motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); + for (auto & p : clipped_path.points) { + p.point.pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); + } + + auto extended_path = target_path; + for (const auto & p : clipped_path.points) { + extended_path.points.push_back(p); + } + extended_path.points = motion_utils::removeOverlapPoints(extended_path.points); + + return extended_path; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const Pose & extend_pose) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position) + 1; + const double extend_distance = motion_utils::calcSignedArcLength( + reference_path.points, target_path_terminal_idx, extend_pose.position); + + return extendPath(reference_path, target_path, extend_distance); +} + } // namespace behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index 01b2880f4362a..355e9078e2ea8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -303,6 +303,10 @@ lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, const double forward_length, const bool forward_only_in_route); +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route); + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length, diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index e6f9ce21cebf7..c65595e4eefe7 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1450,6 +1450,70 @@ lanelet::ConstLanelets getExtendedCurrentLanes( return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data)); } +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route) +{ + auto lanes = getCurrentLanesFromPath(path, planner_data); + + if (lanes.empty()) return lanes; + const auto start_lane = lanes.front(); + double forward_length_sum = 0.0; + double backward_length_sum = 0.0; + + while (backward_length_sum < backward_length) { + auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for backward_length, + // the extending process will not finish. + if (extended_lanes.front().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + backward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.front()); + } else { + break; // no more previous lanes to add + } + lanes = extended_lanes; + } + + while (forward_length_sum < forward_length) { + auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for forward_length, + // the extending process will not finish. + if (extended_lanes.back().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + forward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.back()); + } else { + break; // no more next lanes to add + } + + // stop extending when the lane outside of the route is reached + // if forward_length is a very large value, set it to true, + // as it may continue to extend forever. + if (forward_only_in_route) { + if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) { + return lanes; + } + } + + lanes = extended_lanes; + } + + return lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 5d1c0c68d8208..e98084204ba78 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -292,7 +292,8 @@ bool DefaultPlanner::check_goal_footprint( lanelet::ConstLanelets lanelets; lanelets.push_back(combined_prev_lanelet); lanelets.push_back(next_lane); - lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets); + lanelet::ConstLanelet combined_lanelets = + combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_); // if next lanelet length longer than vehicle longitudinal offset if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { @@ -351,7 +352,8 @@ bool DefaultPlanner::is_goal_valid( double next_lane_length = 0.0; // combine calculated route lanelets - lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets); + lanelet::ConstLanelet combined_prev_lanelet = + combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_); // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index 690a864fcdacd..79e08cf169fa9 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -54,7 +54,8 @@ void insert_marker_array( a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) +lanelet::ConstLanelet combine_lanelets_with_shoulder( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets) { lanelet::Points3d lefts; lanelet::Points3d rights; @@ -70,17 +71,48 @@ lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) } for (const auto & llt : lanelets) { - if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { - for (const auto & pt : llt.leftBound()) { - lefts.push_back(lanelet::Point3d(pt)); - } + // lamda to check if shoulder lane which share left bound with lanelets exist + auto find_bound_shared_shoulder = + [&llt, &shoulder_lanelets](const auto & lanelet_bound, const auto & get_shoulder_bound) { + return std::find_if( + shoulder_lanelets.begin(), shoulder_lanelets.end(), + [&lanelet_bound, &get_shoulder_bound](const auto & shoulder_llt) { + return lanelet_bound.id() == get_shoulder_bound(shoulder_llt).id(); + }); + }; + + // lamda to add bound to target_bound + auto add_bound = [](const auto & bound, auto & target_bound) { + std::transform( + bound.begin(), bound.end(), std::back_inserter(target_bound), + [](const auto & pt) { return lanelet::Point3d(pt); }); + }; + + // check if shoulder lanelats which has LEFT bound of lanelet exist + auto left_shared_shoulder_it = find_bound_shared_shoulder( + llt.leftBound(), [](const auto & shoulder_llt) { return shoulder_llt.rightBound(); }); + if (left_shared_shoulder_it != shoulder_lanelets.end()) { + // if exist, add left bound of SHOULDER lanelet to lefts + add_bound(left_shared_shoulder_it->leftBound(), lefts); + } else if ( + // if not exist, add left bound of lanelet to lefts + std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { + add_bound(llt.leftBound(), lefts); } - if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { - for (const auto & pt : llt.rightBound()) { - rights.push_back(lanelet::Point3d(pt)); - } + + // check if shoulder lanelats which has RIGHT bound of lanelet exist + auto right_shared_shoulder_it = find_bound_shared_shoulder( + llt.rightBound(), [](const auto & shoulder_llt) { return shoulder_llt.leftBound(); }); + if (right_shared_shoulder_it != shoulder_lanelets.end()) { + // if exist, add right bound of SHOULDER lanelet to rights + add_bound(right_shared_shoulder_it->rightBound(), rights); + } else if ( + // if not exist, add right bound of lanelet to rights + std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { + add_bound(llt.rightBound(), rights); } } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 3ea6237f38501..3287267a00dfe 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -49,7 +49,8 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); -lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets); +lanelet::ConstLanelet combine_lanelets_with_shoulder( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets); std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw);