Skip to content

Commit bfb765a

Browse files
feat(lane_change): combine terminal lane change path at waiting approval (autowarefoundation#6176)
* saved Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feat(lane_change): fix drivable area info Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feat(lane_change): update filter object Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feat(lane_change): fix turn signal Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(lane_change): fix typo Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(lane_change): remove updateLaneChangeStatus() Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(lane_change): remove redundant process Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(lane_change): calculate distance to goal Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(lane_change): add empty guard Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> --------- Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
1 parent 65503db commit bfb765a

File tree

6 files changed

+241
-16
lines changed

6 files changed

+241
-16
lines changed

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

+6
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase
5353

5454
LaneChangePath getLaneChangePath() const override;
5555

56+
BehaviorModuleOutput getTerminalLaneChangePath() const override;
57+
5658
BehaviorModuleOutput generateOutput() override;
5759

5860
void extendOutputDrivableArea(BehaviorModuleOutput & output) const override;
@@ -148,6 +150,10 @@ class NormalLaneChange : public LaneChangeBase
148150
const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
149151
const bool check_safety = true) const override;
150152

153+
std::optional<LaneChangePath> calcTerminalLaneChangePath(
154+
const lanelet::ConstLanelets & current_lanes,
155+
const lanelet::ConstLanelets & target_lanes) const;
156+
151157
TurnSignalInfo calcTurnSignalInfo() const override;
152158

153159
bool isValidPath(const PathWithLaneId & path) const override;

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,8 @@ class LaneChangeBase
8989

9090
virtual LaneChangePath getLaneChangePath() const = 0;
9191

92+
virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0;
93+
9294
virtual bool isEgoOnPreparePhase() const = 0;
9395

9496
virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0;

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -115,6 +115,10 @@ ShiftLine getLaneChangingShiftLine(
115115
const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
116116
const PathWithLaneId & reference_path, const double shift_length);
117117

118+
ShiftLine getLaneChangingShiftLine(
119+
const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose,
120+
const PathWithLaneId & reference_path, const double shift_length);
121+
118122
PathWithLaneId getReferencePathFromTargetLane(
119123
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
120124
const Pose & lane_changing_start_pose, const double target_lane_length,

planning/behavior_path_lane_change_module/src/interface.cpp

+6-12
Original file line numberDiff line numberDiff line change
@@ -132,15 +132,12 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
132132
{
133133
RCLCPP_DEBUG(logger_, "%s", __func__);
134134
*prev_approved_path_ = getPreviousModuleOutput().path;
135-
module_type_->insertStopPoint(
136-
module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_);
137135

138136
BehaviorModuleOutput out;
139-
out.path = *prev_approved_path_;
140-
out.reference_path = getPreviousModuleOutput().reference_path;
141-
out.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
142-
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
143-
out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info);
137+
out = module_type_->getTerminalLaneChangePath();
138+
module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path);
139+
out.turn_signal_info =
140+
getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info);
144141

145142
const auto & lane_change_debug = module_type_->getDebugData();
146143
for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) {
@@ -451,16 +448,13 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
451448
const double buffer =
452449
next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front;
453450
const double path_length = motion_utils::calcArcLength(path.points);
454-
const auto & front_point = path.points.front().point.pose.position;
455451
const size_t & current_nearest_seg_idx =
456452
motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
457453
path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold);
458-
const double length_front_to_ego = motion_utils::calcSignedArcLength(
459-
path.points, front_point, static_cast<size_t>(0), current_pose.position,
460-
current_nearest_seg_idx);
454+
const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes);
461455
const auto start_pose =
462456
motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0));
463-
if (path_length - length_front_to_ego < buffer && start_pose) {
457+
if (dist_to_terminal - base_to_front < buffer && start_pose) {
464458
// modify turn signal
465459
current_turn_signal_info.desired_start_point = *start_pose;
466460
current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end;

planning/behavior_path_lane_change_module/src/scene.cpp

+211
Original file line numberDiff line numberDiff line change
@@ -143,6 +143,46 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const
143143
return status_.lane_change_path;
144144
}
145145

146+
BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
147+
{
148+
BehaviorModuleOutput output;
149+
150+
const auto current_lanes = getCurrentLanes();
151+
if (current_lanes.empty()) {
152+
RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong.");
153+
output.path = prev_module_path_;
154+
output.reference_path = prev_module_reference_path_;
155+
output.drivable_area_info = prev_drivable_area_info_;
156+
output.turn_signal_info = prev_turn_signal_info_;
157+
return output;
158+
}
159+
160+
const auto terminal_path =
161+
calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_));
162+
if (!terminal_path) {
163+
RCLCPP_WARN(logger_, "Terminal path not found!!!");
164+
output.path = prev_module_path_;
165+
output.reference_path = prev_module_reference_path_;
166+
output.drivable_area_info = prev_drivable_area_info_;
167+
output.turn_signal_info = prev_turn_signal_info_;
168+
return output;
169+
}
170+
171+
output.path = terminal_path->path;
172+
output.reference_path = prev_module_reference_path_;
173+
output.turn_signal_info = updateOutputTurnSignal();
174+
175+
extendOutputDrivableArea(output);
176+
177+
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
178+
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
179+
output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
180+
planner_data_->parameters.ego_nearest_dist_threshold,
181+
planner_data_->parameters.ego_nearest_yaw_threshold);
182+
183+
return output;
184+
}
185+
146186
BehaviorModuleOutput NormalLaneChange::generateOutput()
147187
{
148188
BehaviorModuleOutput output;
@@ -913,6 +953,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
913953
const auto current_pose = getEgoPose();
914954
const auto & route_handler = *getRouteHandler();
915955
const auto & common_parameters = planner_data_->parameters;
956+
const auto shift_intervals =
957+
route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
958+
const double minimum_lane_change_length =
959+
utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
916960

917961
// Guard
918962
if (objects.objects.empty()) {
@@ -982,6 +1026,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
9821026
return std::abs(lateral) > (common_parameters.vehicle_width / 2);
9831027
};
9841028

1029+
// ignore object that are ahead of terminal lane change start
1030+
{
1031+
double distance_to_terminal_from_object = std::numeric_limits<double>::max();
1032+
for (const auto & polygon_p : obj_polygon_outer) {
1033+
const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
1034+
Pose polygon_pose;
1035+
polygon_pose.position = obj_p;
1036+
polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation;
1037+
const double dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes);
1038+
if (dist < distance_to_terminal_from_object) {
1039+
distance_to_terminal_from_object = dist;
1040+
}
1041+
}
1042+
if (minimum_lane_change_length > distance_to_terminal_from_object) {
1043+
continue;
1044+
}
1045+
}
1046+
9851047
// ignore static object that are behind the ego vehicle
9861048
if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) {
9871049
continue;
@@ -1491,6 +1553,151 @@ bool NormalLaneChange::getLaneChangePaths(
14911553
return false;
14921554
}
14931555

1556+
std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
1557+
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const
1558+
{
1559+
if (current_lanes.empty() || target_lanes.empty()) {
1560+
RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected.");
1561+
return {};
1562+
}
1563+
const auto & route_handler = *getRouteHandler();
1564+
const auto & common_parameters = planner_data_->parameters;
1565+
1566+
const auto forward_path_length = common_parameters.forward_path_length;
1567+
const auto minimum_lane_changing_velocity =
1568+
lane_change_parameters_->minimum_lane_changing_velocity;
1569+
1570+
const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back());
1571+
1572+
const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(
1573+
*lane_change_parameters_,
1574+
route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()));
1575+
const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(
1576+
*lane_change_parameters_,
1577+
route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()));
1578+
1579+
const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes);
1580+
1581+
const auto sorted_lane_ids =
1582+
utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes);
1583+
1584+
const auto target_neighbor_preferred_lane_poly_2d =
1585+
utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_);
1586+
if (target_neighbor_preferred_lane_poly_2d.empty()) {
1587+
RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected.");
1588+
return {};
1589+
}
1590+
1591+
// lane changing start getEgoPose() is at the end of prepare segment
1592+
const auto current_lane_terminal_point =
1593+
lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back());
1594+
1595+
double distance_to_terminal_from_goal = 0;
1596+
if (is_goal_in_route) {
1597+
distance_to_terminal_from_goal =
1598+
utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes);
1599+
}
1600+
1601+
const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose(
1602+
prev_module_path_.points, current_lane_terminal_point,
1603+
-(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal));
1604+
1605+
if (!lane_changing_start_pose) {
1606+
RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!");
1607+
return {};
1608+
}
1609+
1610+
const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet(
1611+
current_lanes, target_lanes.front(), lane_changing_start_pose.value());
1612+
1613+
// Check if the lane changing start point is not on the lanes next to target lanes,
1614+
if (target_length_from_lane_change_start_pose > 0.0) {
1615+
RCLCPP_WARN(logger_, "lane change start getEgoPose() is behind target lanelet!");
1616+
return {};
1617+
}
1618+
1619+
const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(
1620+
target_lanes, lane_changing_start_pose.value());
1621+
1622+
const auto [min_lateral_acc, max_lateral_acc] =
1623+
lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity);
1624+
1625+
const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
1626+
shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc);
1627+
1628+
const auto target_segment = getTargetSegment(
1629+
target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer,
1630+
minimum_lane_changing_velocity, next_lane_change_buffer);
1631+
1632+
if (target_segment.points.empty()) {
1633+
RCLCPP_WARN(logger_, "Reject: target segment is empty!! something wrong...");
1634+
return {};
1635+
}
1636+
1637+
const lanelet::BasicPoint2d lc_start_point(
1638+
lane_changing_start_pose->position.x, lane_changing_start_pose->position.y);
1639+
1640+
const auto target_lane_polygon =
1641+
lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits<double>::max());
1642+
const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon();
1643+
1644+
const auto is_valid_start_point =
1645+
boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) ||
1646+
boost::geometry::covered_by(lc_start_point, target_lane_poly_2d);
1647+
1648+
LaneChangeInfo lane_change_info;
1649+
lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0};
1650+
lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time};
1651+
lane_change_info.velocity =
1652+
LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity};
1653+
lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer};
1654+
lane_change_info.current_lanes = current_lanes;
1655+
lane_change_info.target_lanes = target_lanes;
1656+
lane_change_info.lane_changing_start = lane_changing_start_pose.value();
1657+
lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
1658+
lane_change_info.lateral_acceleration = max_lateral_acc;
1659+
lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity;
1660+
1661+
if (!is_valid_start_point) {
1662+
RCLCPP_WARN(
1663+
logger_,
1664+
"Reject: lane changing points are not inside of the target preferred lanes or its "
1665+
"neighbors");
1666+
return {};
1667+
}
1668+
1669+
const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(
1670+
lane_change_buffer, minimum_lane_changing_velocity);
1671+
const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane(
1672+
route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length,
1673+
lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route,
1674+
next_lane_change_buffer);
1675+
1676+
if (target_lane_reference_path.points.empty()) {
1677+
RCLCPP_WARN(logger_, "Reject: target_lane_reference_path is empty!!");
1678+
return {};
1679+
}
1680+
1681+
lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
1682+
lane_changing_start_pose.value(), target_segment.points.front().point.pose,
1683+
target_lane_reference_path, shift_length);
1684+
1685+
auto reference_segment = prev_module_path_;
1686+
const double length_to_lane_changing_start = motion_utils::calcSignedArcLength(
1687+
reference_segment.points, reference_segment.points.front().point.pose.position,
1688+
lane_changing_start_pose->position);
1689+
utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0);
1690+
// remove terminal points because utils::clipPathLength() calculates extra long path
1691+
reference_segment.points.pop_back();
1692+
reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity;
1693+
1694+
const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath(
1695+
lane_change_info, reference_segment, target_segment, target_lane_reference_path,
1696+
sorted_lane_ids);
1697+
1698+
return terminal_lane_change_path;
1699+
}
1700+
14941701
PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
14951702
{
14961703
const auto & path = status_.lane_change_path;
@@ -1543,6 +1750,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const
15431750

15441751
TurnSignalInfo turn_signal_info{};
15451752

1753+
if (path.path.points.empty()) {
1754+
return turn_signal_info;
1755+
}
1756+
15461757
// desired start pose = prepare start pose
15471758
turn_signal_info.desired_start_point = std::invoke([&]() {
15481759
const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time;

planning/behavior_path_lane_change_module/src/utils/utils.cpp

+12-4
Original file line numberDiff line numberDiff line change
@@ -412,17 +412,17 @@ PathWithLaneId getReferencePathFromTargetLane(
412412
return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer);
413413
});
414414

415-
if (s_end - s_start < lane_changing_length) {
416-
return PathWithLaneId();
417-
}
418-
419415
RCLCPP_DEBUG(
420416
rclcpp::get_logger("behavior_path_planner")
421417
.get_child("lane_change")
422418
.get_child("util")
423419
.get_child("getReferencePathFromTargetLane"),
424420
"start: %f, end: %f", s_start, s_end);
425421

422+
if (s_end - s_start < lane_changing_length) {
423+
return PathWithLaneId();
424+
}
425+
426426
const auto lane_changing_reference_path =
427427
route_handler.getCenterLinePath(target_lanes, s_start, s_end);
428428

@@ -437,6 +437,14 @@ ShiftLine getLaneChangingShiftLine(
437437
const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose;
438438
const Pose & lane_changing_end_pose = target_segment.points.front().point.pose;
439439

440+
return getLaneChangingShiftLine(
441+
lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length);
442+
}
443+
444+
ShiftLine getLaneChangingShiftLine(
445+
const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose,
446+
const PathWithLaneId & reference_path, const double shift_length)
447+
{
440448
ShiftLine shift_line;
441449
shift_line.end_shift_length = shift_length;
442450
shift_line.start = lane_changing_start_pose;

0 commit comments

Comments
 (0)