@@ -144,6 +144,19 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
144
144
{
145
145
BehaviorModuleOutput output;
146
146
147
+ if (isAbortState () && abort_path_) {
148
+ output.path = abort_path_->path ;
149
+ output.reference_path = prev_module_reference_path_;
150
+ output.turn_signal_info = prev_turn_signal_info_;
151
+ extendOutputDrivableArea (output);
152
+ const auto current_seg_idx = planner_data_->findEgoSegmentIndex (output.path .points );
153
+ output.turn_signal_info = planner_data_->turn_signal_decider .use_prior_turn_signal (
154
+ output.path , getEgoPose (), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info ,
155
+ planner_data_->parameters .ego_nearest_dist_threshold ,
156
+ planner_data_->parameters .ego_nearest_yaw_threshold );
157
+ return output;
158
+ }
159
+
147
160
const auto current_lanes = getCurrentLanes ();
148
161
if (current_lanes.empty ()) {
149
162
RCLCPP_WARN (logger_, " Current lanes not found!!! Something wrong." );
@@ -157,7 +170,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
157
170
const auto terminal_path =
158
171
calcTerminalLaneChangePath (current_lanes, getLaneChangeLanes (current_lanes, direction_));
159
172
if (!terminal_path) {
160
- RCLCPP_WARN (logger_, " Terminal path not found!!!" );
173
+ RCLCPP_DEBUG (logger_, " Terminal path not found!!!" );
161
174
output.path = prev_module_path_;
162
175
output.reference_path = prev_module_reference_path_;
163
176
output.drivable_area_info = prev_drivable_area_info_;
@@ -1528,7 +1541,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
1528
1541
-(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal));
1529
1542
1530
1543
if (!lane_changing_start_pose) {
1531
- RCLCPP_WARN (logger_, " Reject: lane changing start pose not found!!!" );
1544
+ RCLCPP_DEBUG (logger_, " Reject: lane changing start pose not found!!!" );
1532
1545
return {};
1533
1546
}
1534
1547
@@ -1537,7 +1550,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
1537
1550
1538
1551
// Check if the lane changing start point is not on the lanes next to target lanes,
1539
1552
if (target_length_from_lane_change_start_pose > 0.0 ) {
1540
- RCLCPP_WARN (logger_, " lane change start getEgoPose() is behind target lanelet!" );
1553
+ RCLCPP_DEBUG (logger_, " lane change start getEgoPose() is behind target lanelet!" );
1541
1554
return {};
1542
1555
}
1543
1556
@@ -1555,7 +1568,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
1555
1568
minimum_lane_changing_velocity, next_lane_change_buffer);
1556
1569
1557
1570
if (target_segment.points .empty ()) {
1558
- RCLCPP_WARN (logger_, " Reject: target segment is empty!! something wrong..." );
1571
+ RCLCPP_DEBUG (logger_, " Reject: target segment is empty!! something wrong..." );
1559
1572
return {};
1560
1573
}
1561
1574
@@ -1584,7 +1597,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
1584
1597
lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity;
1585
1598
1586
1599
if (!is_valid_start_point) {
1587
- RCLCPP_WARN (
1600
+ RCLCPP_DEBUG (
1588
1601
logger_,
1589
1602
" Reject: lane changing points are not inside of the target preferred lanes or its "
1590
1603
" neighbors" );
@@ -1599,7 +1612,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
1599
1612
next_lane_change_buffer);
1600
1613
1601
1614
if (target_lane_reference_path.points .empty ()) {
1602
- RCLCPP_WARN (logger_, " Reject: target_lane_reference_path is empty!!" );
1615
+ RCLCPP_DEBUG (logger_, " Reject: target_lane_reference_path is empty!!" );
1603
1616
return {};
1604
1617
}
1605
1618
@@ -1857,31 +1870,25 @@ bool NormalLaneChange::calcAbortPath()
1857
1870
1858
1871
ShiftedPath shifted_path;
1859
1872
// offset front side
1860
- // bool offset_back = false;
1861
1873
if (!path_shifter.generate (&shifted_path)) {
1862
1874
RCLCPP_ERROR (logger_, " failed to generate abort shifted path." );
1863
1875
}
1864
1876
1865
- const auto arc_position = lanelet::utils::getArcCoordinates (
1866
- reference_lanelets, shifted_path.path .points .at (abort_return_idx).point .pose );
1867
- const PathWithLaneId reference_lane_segment = std::invoke ([&]() {
1868
- const double s_start = arc_position.length ;
1869
- double s_end = std::max (lanelet::utils::getLaneletLength2d (reference_lanelets), s_start);
1870
-
1871
- if (
1872
- !reference_lanelets.empty () &&
1873
- route_handler->isInGoalRouteSection (reference_lanelets.back ())) {
1874
- const auto goal_arc_coordinates =
1875
- lanelet::utils::getArcCoordinates (reference_lanelets, route_handler->getGoalPose ());
1876
- const double forward_length = std::max (goal_arc_coordinates.length , s_start);
1877
- s_end = std::min (s_end, forward_length);
1877
+ PathWithLaneId reference_lane_segment = prev_module_path_;
1878
+ {
1879
+ const auto terminal_path =
1880
+ calcTerminalLaneChangePath (reference_lanelets, selected_path.info .target_lanes );
1881
+ if (terminal_path) {
1882
+ reference_lane_segment = terminal_path->path ;
1878
1883
}
1879
- PathWithLaneId ref = route_handler->getCenterLinePath (reference_lanelets, s_start, s_end, true );
1880
- ref.points .back ().point .longitudinal_velocity_mps = std::min (
1881
- ref.points .back ().point .longitudinal_velocity_mps ,
1882
- static_cast <float >(lane_change_parameters_->minimum_lane_changing_velocity ));
1883
- return ref;
1884
- });
1884
+ const auto return_pose = shifted_path.path .points .at (abort_return_idx).point .pose ;
1885
+ const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints (
1886
+ reference_lane_segment.points , return_pose, common_param.ego_nearest_dist_threshold ,
1887
+ common_param.ego_nearest_yaw_threshold );
1888
+ reference_lane_segment.points = motion_utils::cropPoints (
1889
+ reference_lane_segment.points , return_pose.position , seg_idx,
1890
+ common_param.forward_path_length , 0.0 );
1891
+ }
1885
1892
1886
1893
auto abort_path = selected_path;
1887
1894
abort_path.shifted_path = shifted_path;
0 commit comments