Skip to content

Commit

Permalink
refactor(start_planner): remove redundant calculation in shift pull o…
Browse files Browse the repository at this point in the history 
…ut (autowarefoundation#8623)

* fix redundant calculation

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

* fix unneccesary modification for comment

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

---------

Signed-off-by: Go Sakayori <gsakayori@gmail.com>
Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
  • Loading branch information
@go-sakayori @a-maumau
go-sakayori authored and a-maumau committed Sep 2, 2024
1 parent 6f24c3d commit 70d6635
Showing 1 changed file with 33 additions and 32 deletions.
65 changes: 33 additions & 32 deletions .../behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,8 @@ std::optional<PullOutPath> ShiftPullOut::plan(
return std::nullopt;
}

const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();

// get safe path
for (auto & pull_out_path : pull_out_paths) {
universe_utils::ScopedTimeTrack st("get safe path", *time_keeper_);
Expand All @@ -79,8 +81,6 @@ std::optional<PullOutPath> ShiftPullOut::plan(
shift_path.points.begin() + pull_out_end_idx + 1);
}

const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();

// if lane departure check override is true, and if the initial pose is not fully within a lane,
// cancel lane departure check
const bool is_lane_departure_check_required = std::invoke([&]() -> bool {
Expand Down Expand Up @@ -269,26 +269,38 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
});

bool has_non_shifted_path = false;

// if shift length is too short, add non sifted path
constexpr double MINIMUM_SHIFT_LENGTH = 0.01;
const double shift_length = arc_position_start.distance;
const bool is_smaller_than_minimum = std::abs(shift_length) < MINIMUM_SHIFT_LENGTH;

if (is_smaller_than_minimum) {
candidate_paths.push_back(non_shifted_path);
has_non_shifted_path = true;
}

// calculate pull out distance, longitudinal acc, terminal velocity
const size_t shift_start_idx =
findNearestIndex(road_lane_reference_path.points, start_pose.position);
const double road_velocity =
road_lane_reference_path.points.at(shift_start_idx).point.longitudinal_velocity_mps;

// clip from ego pose
PathWithLaneId road_lane_reference_path_from_ego = road_lane_reference_path;
road_lane_reference_path_from_ego.points.erase(
road_lane_reference_path_from_ego.points.begin(),
road_lane_reference_path_from_ego.points.begin() + shift_start_idx);

const auto curvatures_and_segment_lengths =
autoware::motion_utils::calcCurvatureAndSegmentLength(road_lane_reference_path_from_ego.points);

for (double lateral_acc = minimum_lateral_acc; lateral_acc <= maximum_lateral_acc;
lateral_acc += acc_resolution) {
PathShifter path_shifter{};

path_shifter.setPath(road_lane_reference_path);

// if shift length is too short, add non sifted path
constexpr double MINIMUM_SHIFT_LENGTH = 0.01;
const double shift_length = getArcCoordinates(road_lanes, start_pose).distance;
if (std::abs(shift_length) < MINIMUM_SHIFT_LENGTH && !has_non_shifted_path) {
candidate_paths.push_back(non_shifted_path);
has_non_shifted_path = true;
continue;
}

// calculate pull out distance, longitudinal acc, terminal velocity
const size_t shift_start_idx =
findNearestIndex(road_lane_reference_path.points, start_pose.position);
const double road_velocity =
road_lane_reference_path.points.at(shift_start_idx).point.longitudinal_velocity_mps;
const double shift_time =
PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc);
const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0);
Expand All @@ -297,17 +309,12 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
minimum_shift_pull_out_distance);
const double terminal_velocity = longitudinal_acc * shift_time;

// clip from ego pose
PathWithLaneId road_lane_reference_path_from_ego = road_lane_reference_path;
road_lane_reference_path_from_ego.points.erase(
road_lane_reference_path_from_ego.points.begin(),
road_lane_reference_path_from_ego.points.begin() + shift_start_idx);
// before means distance on road lane
// Note: the pull_out_distance is the required distance on the shifted path. Now we need to
// calculate the distance on the center line used for the shift path generation. However, since
// the calcBeforeShiftedArcLength is an approximate conversion from center line to center line
// (not shift path to centerline), the conversion result may too long or short. To prevent too
// short length, take maximum with the original distance.
// calculate the distance on the center line used for the shift path_shifter generation.
// However, since the calcBeforeShiftedArcLength is an approximate conversion from center line
// to center line (not shift path to centerline), the conversion result may too long or short.
// To prevent too short length, take maximum with the original distance.
// TODO(kosuke55): update the conversion function and get rid of the comparison with original
// distance.
const double pull_out_distance_converted = std::max(
Expand All @@ -326,10 +333,6 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
double pull_out_distance = pull_out_distance_converted;
double min_curvature_after_distance_converted = std::numeric_limits<double>::max();

const auto curvatures_and_segment_lengths =
autoware::motion_utils::calcCurvatureAndSegmentLength(
road_lane_reference_path_from_ego.points);

const auto update_arc_length = [&](size_t i, const auto & segment_length) {
arc_length += (i == curvatures_and_segment_lengths.size() - 1)
? segment_length.first + segment_length.second
Expand Down Expand Up @@ -374,9 +377,7 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(

// get shift end pose
const auto shift_end_pose_ptr = std::invoke([&]() {
const auto arc_position_shift_start =
lanelet::utils::getArcCoordinates(road_lanes, start_pose);
const double s_start = arc_position_shift_start.length + before_shifted_pull_out_distance;
const double s_start = arc_position_start.length + before_shifted_pull_out_distance;
const double s_end = s_start + std::numeric_limits<double>::epsilon();
const auto path = route_handler.getCenterLinePath(road_lanes, s_start, s_end, true);
return path.points.empty()
Expand Down

0 comments on commit 70d6635

Please sign in to comment.