Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(start_planner): fix path cut by U turn goal #4109

Merged
merged 2 commits into from
Jun 29, 2023
Merged

fix(start_planner): fix path cut by U turn goal #4109

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3c5bc4d
fix(start_planner): fix path cut by U turn goal
kosuke55 Jun 28, 2023
01be12f
cut path
kosuke55 Jun 28, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 16 additions & 3 deletions planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,12 @@
tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose));

// Check if ego is not out of lanes
const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
// const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
const double backward_path_length =
planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
const auto current_lanes =
utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());

const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);
auto lanes = current_lanes;
lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
Expand Down Expand Up @@ -363,7 +368,12 @@
return output;
}

const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
// const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
const double backward_path_length =
planner_data_->parameters.backward_path_length + parameters_->max_back_distance;

Check warning on line 373 in planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp#L373 

Added line #L373 was not covered by tests
const auto current_lanes =
utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());

Check warning on line 376 in planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

StartPlannerModule::planWaitingApproval increases from 77 to 80 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);
auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
const auto drivable_lanes =
Expand Down Expand Up @@ -600,7 +610,10 @@
const auto & current_pose = planner_data_->self_odometry->pose.pose;
const auto & goal_pose = planner_data_->route_handler->getGoalPose();

status_.current_lanes = utils::getExtendedCurrentLanes(planner_data_);
const double backward_path_length =
planner_data_->parameters.backward_path_length + parameters_->max_back_distance;

Check warning on line 614 in planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp#L614 

Added line #L614 was not covered by tests
status_.current_lanes =
utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());

Check warning on line 616 in planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp#L616 

Added line #L616 was not covered by tests
status_.pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);

// combine road and shoulder lanes
Expand Down
6 changes: 5 additions & 1 deletion planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,11 @@
PullOutPath output;

// combine road lane and shoulder lane
const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_);
const double backward_path_length =
planner_data_->parameters.backward_path_length + parameters_.max_back_distance;

Check warning on line 44 in planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp#L44 

Added line #L44 was not covered by tests
const auto road_lanes =
utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());

const auto shoulder_lanes = getPullOutLanes(planner_data_);
auto lanes = road_lanes;
lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end());
Expand Down
21 changes: 13 additions & 8 deletions planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,16 @@
const auto & route_handler = planner_data_->route_handler;
const auto & common_parameters = planner_data_->parameters;
const auto & dynamic_objects = planner_data_->dynamic_object;
const auto & road_lanes = utils::getExtendedCurrentLanes(planner_data_);
const auto & shoulder_lanes = getPullOutLanes(planner_data_);
const auto shoulder_lanes = getPullOutLanes(planner_data_);

Check warning on line 47 in planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp#L47 

Added line #L47 was not covered by tests
if (shoulder_lanes.empty()) {
return boost::none;
}

const double backward_path_length =
planner_data_->parameters.backward_path_length + parameters_.max_back_distance;

Check warning on line 53 in planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp#L53 

Added line #L53 was not covered by tests
const auto road_lanes =
utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());

// find candidate paths
auto pull_out_paths = calcPullOutPaths(
*route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters,
Expand Down Expand Up @@ -135,6 +139,7 @@
}

// rename parameter
const double forward_path_length = common_parameter.forward_path_length;

Check warning on line 142 in planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp#L142 

Added line #L142 was not covered by tests
const double backward_path_length = common_parameter.backward_path_length;
const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance;
const double lateral_jerk = parameter.lateral_jerk;
Expand All @@ -150,13 +155,13 @@
const auto arc_position_start = getArcCoordinates(road_lanes, start_pose);
const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0);
const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose);
const double road_lanes_length = std::accumulate(
road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) {
return sum + lanelet::utils::getLaneletLength2d(lane);
});
// if goal is behind start pose,

// if goal is behind start pose, use path with forward_path_length
const bool goal_is_behind = arc_position_goal.length < s_start;
const double s_end = goal_is_behind ? road_lanes_length : arc_position_goal.length;
const double s_forward_length = s_start + forward_path_length;

Check warning on line 161 in planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp#L161 

Added line #L161 was not covered by tests
const double s_end =
goal_is_behind ? s_forward_length : std::min(arc_position_goal.length, s_forward_length);

constexpr double RESAMPLE_INTERVAL = 1.0;
PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(
route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL);
Expand Down
Loading