@@ -143,6 +143,46 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const
143
143
return status_.lane_change_path ;
144
144
}
145
145
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
+
146
186
BehaviorModuleOutput NormalLaneChange::generateOutput ()
147
187
{
148
188
BehaviorModuleOutput output;
@@ -913,6 +953,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
913
953
const auto current_pose = getEgoPose ();
914
954
const auto & route_handler = *getRouteHandler ();
915
955
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);
916
960
917
961
// Guard
918
962
if (objects.objects .empty ()) {
@@ -982,6 +1026,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
982
1026
return std::abs (lateral) > (common_parameters.vehicle_width / 2 );
983
1027
};
984
1028
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
+
985
1047
// ignore static object that are behind the ego vehicle
986
1048
if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0 ) {
987
1049
continue ;
@@ -1491,6 +1553,151 @@ bool NormalLaneChange::getLaneChangePaths(
1491
1553
return false ;
1492
1554
}
1493
1555
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
+
1494
1701
PathSafetyStatus NormalLaneChange::isApprovedPathSafe () const
1495
1702
{
1496
1703
const auto & path = status_.lane_change_path ;
@@ -1543,6 +1750,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const
1543
1750
1544
1751
TurnSignalInfo turn_signal_info{};
1545
1752
1753
+ if (path.path .points .empty ()) {
1754
+ return turn_signal_info;
1755
+ }
1756
+
1546
1757
// desired start pose = prepare start pose
1547
1758
turn_signal_info.desired_start_point = std::invoke ([&]() {
1548
1759
const auto blinker_start_duration = planner_data_->parameters .turn_signal_search_time ;
0 commit comments