From 7450b1952f7a6ad960add7603f1f39c69e5c4489 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Wed, 17 Jan 2024 20:37:18 +0900 Subject: [PATCH 1/5] Add expanded drivable lanes to ShiftPullOut constructor Signed-off-by: kyoichi-sugahara --- .../shift_pull_out.hpp | 3 +- .../start_planner_module.hpp | 1 + .../src/shift_pull_out.cpp | 51 +++++-------------- .../src/start_planner_module.cpp | 32 +++++++++++- 4 files changed, 46 insertions(+), 41 deletions(-) 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..2376f24f5bbd1 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 @@ -34,7 +34,7 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker); + std::shared_ptr & lane_departure_checker, const lanelet::ConstLanelets & expanded_drivable_lanes); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; @@ -51,6 +51,7 @@ class ShiftPullOut : public PullOutPlannerBase const double longitudinal_acc, const double lateral_acc); std::shared_ptr lane_departure_checker_; + lanelet::ConstLanelets expanded_drivable_lanes_; private: // Calculate longitudinal distance based on the acceleration limit, curvature limit, and the diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 7b463f4fadf80..c54c396948f3d 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -242,6 +242,7 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; + lanelet::ConstLanelets createExpandedDrivableLanes(); // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; 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 51673410199b8..2671219479539 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 @@ -37,8 +37,8 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker) -: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker} + std::shared_ptr & lane_departure_checker, const lanelet::ConstLanelets & expanded_drivable_lanes) +: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}, expanded_drivable_lanes_{expanded_drivable_lanes} { } @@ -49,14 +49,11 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_.max_back_distance; - const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - if (pull_out_lanes.empty()) { - return std::nullopt; - } - const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); + + // find candidate paths auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); if (pull_out_paths.empty()) { @@ -69,40 +66,18 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos pull_out_path.partial_paths.front(); // shift path is not separate but only one. // check lane_departure with path between pull_out_start to pull_out_end - PathWithLaneId path_start_to_end{}; + PathWithLaneId path_shift_start_to_end{}; { const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); + const size_t pull_out_end_idx = + findNearestIndex(shift_path.points, pull_out_path.end_pose.position); - // calculate collision check end idx - const size_t collision_check_end_idx = std::invoke([&]() { - const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - shift_path.points, pull_out_path.end_pose.position, - parameters_.collision_check_distance_from_end); - - if (collision_check_end_pose) { - return findNearestIndex(shift_path.points, collision_check_end_pose->position); - } else { - return shift_path.points.size() - 1; - } - }); - path_start_to_end.points.insert( - path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, - shift_path.points.begin() + collision_check_end_idx + 1); + path_shift_start_to_end.points.insert( + path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + pull_out_end_idx + 1); } - // extract shoulder lanes from pull out lanes - lanelet::ConstLanelets shoulder_lanes; - std::copy_if( - pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), - [&route_handler](const auto & pull_out_lane) { - return route_handler->isShoulderLanelet(pull_out_lane); - }); - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip)); + // crop backward path // removes points which are out of lanes up to the start pose. @@ -117,7 +92,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); const bool is_out_of_lane = - LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); + LaneDepartureChecker::isOutOfLane(expanded_drivable_lanes_, transformed_vehicle_footprint); if (i <= start_segment_idx) { if (!is_out_of_lane) { cropped_path.points.push_back(shift_path.points.at(i)); @@ -131,7 +106,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(expanded_drivable_lanes_, path_shift_start_to_end)) { continue; } 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 9ad7f72d6af4e..b87c28332a5f1 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 @@ -61,8 +61,8 @@ StartPlannerModule::StartPlannerModule( // set enabled planner if (parameters_->enable_shift_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_)); + start_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker_, createExpandedDrivableLanes())); } if (parameters_->enable_geometric_pull_out) { start_planners_.push_back(std::make_shared(node, *parameters)); @@ -1325,6 +1325,34 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } +lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes() +{ + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + // if (pull_out_lanes.empty()) { + // return std::nullopt; + // } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [this](const auto & pull_out_lane) { + return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); + }); + const auto drivable_lanes = + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; + const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip)); + return expanded_lanes; +} + void StartPlannerModule::setDebugData() { using marker_utils::addFootprintMarker; From 3444108fde4b24ef3f2603943d1c29b0d28a6513 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 17 Jan 2024 11:42:43 +0000 Subject: [PATCH 2/5] style(pre-commit): autofix --- .../shift_pull_out.hpp | 3 ++- .../src/shift_pull_out.cpp | 13 +++++++------ 2 files changed, 9 insertions(+), 7 deletions(-) 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 2376f24f5bbd1..74c71b7394584 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 @@ -34,7 +34,8 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, const lanelet::ConstLanelets & expanded_drivable_lanes); + std::shared_ptr & lane_departure_checker, + const lanelet::ConstLanelets & expanded_drivable_lanes); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; 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 2671219479539..e6b69c923b043 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 @@ -37,8 +37,11 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, const lanelet::ConstLanelets & expanded_drivable_lanes) -: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker}, expanded_drivable_lanes_{expanded_drivable_lanes} + std::shared_ptr & lane_departure_checker, + const lanelet::ConstLanelets & expanded_drivable_lanes) +: PullOutPlannerBase{node, parameters}, + lane_departure_checker_{lane_departure_checker}, + expanded_drivable_lanes_{expanded_drivable_lanes} { } @@ -53,7 +56,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - // find candidate paths auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); if (pull_out_paths.empty()) { @@ -77,8 +79,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } - - // crop backward path // removes points which are out of lanes up to the start pose. // this ensures that the backward_path stays within the drivable area when starting from a @@ -106,7 +106,8 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_drivable_lanes_, path_shift_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane( + expanded_drivable_lanes_, path_shift_start_to_end)) { continue; } From fd075accb6d83a8fe2a9b5dce35b230f0cef5f26 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 18 Jan 2024 13:00:38 +0900 Subject: [PATCH 3/5] Refactor ShiftPullOut class and update expanded drivable lanes Signed-off-by: kyoichi-sugahara --- .../shift_pull_out.hpp | 11 +++++-- .../start_planner_module.hpp | 3 +- .../src/shift_pull_out.cpp | 30 +++++++++++-------- .../src/start_planner_module.cpp | 26 ++++++++++++---- 4 files changed, 48 insertions(+), 22 deletions(-) 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 74c71b7394584..318dadac2b5f7 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 @@ -34,8 +34,7 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, - const lanelet::ConstLanelets & expanded_drivable_lanes); + std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; @@ -51,8 +50,12 @@ class ShiftPullOut : public PullOutPlannerBase ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, const double longitudinal_acc, const double lateral_acc); + void setExpandedDrivableLanes(const lanelet::ConstLanelets & expanded_drivable_lanes) + { + expanded_drivable_lanes_ = expanded_drivable_lanes; + } + std::shared_ptr lane_departure_checker_; - lanelet::ConstLanelets expanded_drivable_lanes_; private: // Calculate longitudinal distance based on the acceleration limit, curvature limit, and the @@ -60,6 +63,8 @@ class ShiftPullOut : public PullOutPlannerBase double calcPullOutLongitudinalDistance( const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; + + lanelet::ConstLanelets expanded_drivable_lanes_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index c54c396948f3d..bf3874ee4f25d 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -242,7 +242,8 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; - lanelet::ConstLanelets createExpandedDrivableLanes(); + void updateExpandedDrivableLanes(); + lanelet::ConstLanelets createExpandedDrivableLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; 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 e6b69c923b043..63fb57e8ea5f7 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 @@ -37,11 +37,8 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, - const lanelet::ConstLanelets & expanded_drivable_lanes) -: PullOutPlannerBase{node, parameters}, - lane_departure_checker_{lane_departure_checker}, - expanded_drivable_lanes_{expanded_drivable_lanes} + std::shared_ptr & lane_departure_checker) +: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker} { } @@ -55,7 +52,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - // find candidate paths auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); if (pull_out_paths.empty()) { @@ -68,15 +64,25 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos pull_out_path.partial_paths.front(); // shift path is not separate but only one. // check lane_departure with path between pull_out_start to pull_out_end - PathWithLaneId path_shift_start_to_end{}; + PathWithLaneId path_start_to_end{}; { const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); - const size_t pull_out_end_idx = - findNearestIndex(shift_path.points, pull_out_path.end_pose.position); - path_shift_start_to_end.points.insert( - path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, - shift_path.points.begin() + pull_out_end_idx + 1); + // calculate collision check end idx + const size_t collision_check_end_idx = std::invoke([&]() { + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + shift_path.points, pull_out_path.end_pose.position, + parameters_.collision_check_distance_from_end); + + if (collision_check_end_pose) { + return findNearestIndex(shift_path.points, collision_check_end_pose->position); + } else { + return shift_path.points.size() - 1; + } + }); + path_start_to_end.points.insert( + path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + collision_check_end_idx + 1); } // crop backward 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 b87c28332a5f1..1939d1a322336 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 @@ -61,8 +61,8 @@ StartPlannerModule::StartPlannerModule( // set enabled planner if (parameters_->enable_shift_pull_out) { - start_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker_, createExpandedDrivableLanes())); + start_planners_.push_back( + std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { start_planners_.push_back(std::make_shared(node, *parameters)); @@ -127,6 +127,7 @@ void StartPlannerModule::initVariables() debug_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(start_planner_data_.collision_check); + updateExpandedDrivableLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -559,6 +560,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; + updateExpandedDrivableLanes(); } void StartPlannerModule::incrementPathIndex() @@ -1325,15 +1327,27 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } -lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes() +void StartPlannerModule::updateExpandedDrivableLanes() +{ + const auto expanded_drivable_lanes = createExpandedDrivableLanes(); + for (auto & planner : start_planners_) { + auto shift_pull_out = std::dynamic_pointer_cast(planner); + + if (shift_pull_out) { + shift_pull_out->setExpandedDrivableLanes(expanded_drivable_lanes); + } + } +} + +lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes() const { const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - // if (pull_out_lanes.empty()) { - // return std::nullopt; - // } + if (pull_out_lanes.empty()) { + return lanelet::ConstLanelets{}; + } const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); From 9421314e9c8e8bd5442e2bb572df87ea3c30308e Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 18 Jan 2024 15:10:52 +0900 Subject: [PATCH 4/5] Refactor drivable lane functions Signed-off-by: kyoichi-sugahara --- .../shift_pull_out.hpp | 6 ++--- .../start_planner_module.hpp | 4 ++-- .../src/shift_pull_out.cpp | 5 ++--- .../src/start_planner_module.cpp | 22 ++++++++----------- 4 files changed, 16 insertions(+), 21 deletions(-) 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 318dadac2b5f7..fef9a4aa8ebfc 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 @@ -50,9 +50,9 @@ class ShiftPullOut : public PullOutPlannerBase ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, const double longitudinal_acc, const double lateral_acc); - void setExpandedDrivableLanes(const lanelet::ConstLanelets & expanded_drivable_lanes) + void setDrivableLanes(const lanelet::ConstLanelets & drivable_lanes) { - expanded_drivable_lanes_ = expanded_drivable_lanes; + drivable_lanes_ = drivable_lanes; } std::shared_ptr lane_departure_checker_; @@ -64,7 +64,7 @@ class ShiftPullOut : public PullOutPlannerBase const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; - lanelet::ConstLanelets expanded_drivable_lanes_; + lanelet::ConstLanelets drivable_lanes_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index bf3874ee4f25d..d82521b4833c0 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -242,8 +242,8 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; - void updateExpandedDrivableLanes(); - lanelet::ConstLanelets createExpandedDrivableLanes() const; + void updateDrivableLanes(); + lanelet::ConstLanelets createDrivableLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; 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 63fb57e8ea5f7..40836229243a6 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 @@ -98,7 +98,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); const bool is_out_of_lane = - LaneDepartureChecker::isOutOfLane(expanded_drivable_lanes_, transformed_vehicle_footprint); + LaneDepartureChecker::isOutOfLane(drivable_lanes_, transformed_vehicle_footprint); if (i <= start_segment_idx) { if (!is_out_of_lane) { cropped_path.points.push_back(shift_path.points.at(i)); @@ -112,8 +112,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane( - expanded_drivable_lanes_, path_shift_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_start_to_end)) { continue; } 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 1939d1a322336..9ecc2d4d8d679 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 @@ -127,7 +127,7 @@ void StartPlannerModule::initVariables() debug_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(start_planner_data_.collision_check); - updateExpandedDrivableLanes(); + updateDrivableLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -560,7 +560,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; - updateExpandedDrivableLanes(); + updateDrivableLanes(); } void StartPlannerModule::incrementPathIndex() @@ -1327,19 +1327,19 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } -void StartPlannerModule::updateExpandedDrivableLanes() +void StartPlannerModule::updateDrivableLanes() { - const auto expanded_drivable_lanes = createExpandedDrivableLanes(); + const auto drivable_lanes = createDrivableLanes(); for (auto & planner : start_planners_) { auto shift_pull_out = std::dynamic_pointer_cast(planner); if (shift_pull_out) { - shift_pull_out->setExpandedDrivableLanes(expanded_drivable_lanes); + shift_pull_out->setDrivableLanes(drivable_lanes); } } } -lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes() const +lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const { const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; @@ -1358,13 +1358,9 @@ lanelet::ConstLanelets StartPlannerModule::createExpandedDrivableLanes() const [this](const auto & pull_out_lane) { return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); }); - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip)); - return expanded_lanes; + const auto drivable_lanes = utils::transformToLanelets( + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); + return drivable_lanes; } void StartPlannerModule::setDebugData() From 61a08512506e6bd6cc5216997e98be9eb5dd7981 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 19 Jan 2024 16:33:43 +0900 Subject: [PATCH 5/5] Fix lane departure check in ShiftPullOut::plan() Signed-off-by: kyoichi-sugahara --- .../behavior_path_start_planner_module/src/shift_pull_out.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9d02fdabffd6d..3b4d08b65923c 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 @@ -102,7 +102,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_shift_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_shift_start_to_end)) { continue; }