From 2a1ad67b12f89f8e6b10995616b37890581cec98 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 16 Nov 2023 03:54:21 +0900 Subject: [PATCH] fix(goal_planner): fix extending current lanes (#5595) * fix(goal_planner): fix extending current lanes Signed-off-by: kosuke55 * fix build error Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 6b363f206204c..a0a84adfcf6f8 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1064,9 +1064,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, common_parameters.backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); if (current_lanes.empty()) { return PathWithLaneId{}; @@ -1152,9 +1153,10 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const const auto & common_parameters = planner_data_->parameters; // generate stop reference path - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, common_parameters.backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); + 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; @@ -1621,10 +1623,10 @@ bool GoalPlannerModule::isSafePath() const planner_data_->self_odometry->twist.twist.linear.y); const auto & dynamic_object = planner_data_->dynamic_object; const auto & route_handler = planner_data_->route_handler; - const double backward_path_length = planner_data_->parameters.backward_path_length; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); + 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 pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length);