Skip to content

Commit

Permalink
fix(lane_change): stop point activated too early (autowarefoundation#…
Browse files Browse the repository at this point in the history
…5402)

* fix(lane_change): stop point activated too early

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* refactor

Signed-off-by: Zulfaqar Azmi <[email protected]>

* fix dead node

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
zulfaqar-azmi-t4 authored and TomohitoAndo committed Nov 22, 2023
1 parent f30f4c8 commit 3fa5ca2
Showing 1 changed file with 33 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -218,6 +218,23 @@ void NormalLaneChange::insertStopPoint(
const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;

const auto is_valid_start_point = std::invoke([&]() -> bool {
auto lc_start_point = lanelet::utils::conversion::toLaneletPoint(
status_.lane_change_path.info.lane_changing_start.position);
const auto target_neighbor_preferred_lane_poly_2d =
utils::lane_change::getTargetNeighborLanesPolygon(
*route_handler, status_.current_lanes, type_);
return boost::geometry::covered_by(
lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d);
});

if (!is_valid_start_point) {
const auto stop_point = utils::insertStopPoint(stopping_distance, path);
setStopPose(stop_point.point.pose);

return;
}

// calculate minimum distance from path front to the stationary object on the ego lane.
const auto distance_to_ego_lane_obj = [&]() -> double {
double distance_to_obj = distance_to_terminal;
Expand Down Expand Up @@ -642,7 +659,8 @@ double NormalLaneChange::calcMaximumLaneChangeLength(
const auto shift_intervals =
getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet);
return utils::lane_change::calcMaximumLaneChangeLength(
getEgoVelocity(), getCommonParam(), shift_intervals, max_acc);
std::max(getCommonParam().minimum_lane_changing_velocity, getEgoVelocity()), getCommonParam(),
shift_intervals, max_acc);
}

std::vector<double> NormalLaneChange::sampleLongitudinalAccValues(
Expand Down Expand Up @@ -1247,6 +1265,20 @@ bool NormalLaneChange::getLaneChangePaths(
boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) ||
boost::geometry::covered_by(lc_start_point, target_lane_poly_2d);

LaneChangeInfo lane_change_info;
lane_change_info.longitudinal_acceleration =
LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
lane_change_info.velocity =
LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
lane_change_info.current_lanes = current_lanes;
lane_change_info.target_lanes = target_lanes;
lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
lane_change_info.lateral_acceleration = lateral_acc;
lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;

if (!is_valid_start_point) {
debug_print(
"Reject: lane changing points are not inside of the target preferred lanes or its "
Expand All @@ -1266,21 +1298,8 @@ bool NormalLaneChange::getLaneChangePaths(
continue;
}

LaneChangeInfo lane_change_info;
lane_change_info.longitudinal_acceleration =
LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
lane_change_info.velocity =
LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
lane_change_info.current_lanes = current_lanes;
lane_change_info.target_lanes = target_lanes;
lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
prepare_segment, target_segment, target_lane_reference_path, shift_length);
lane_change_info.lateral_acceleration = lateral_acc;
lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;

const auto candidate_path = utils::lane_change::constructCandidatePath(
lane_change_info, prepare_segment, target_segment, target_lane_reference_path,
Expand Down

0 comments on commit 3fa5ca2

Please sign in to comment.