File tree 2 files changed +13
-6
lines changed
planning/behavior_path_lane_change_module/src
2 files changed +13
-6
lines changed Original file line number Diff line number Diff line change @@ -1516,9 +1516,16 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
1516
1516
// lane changing start getEgoPose() is at the end of prepare segment
1517
1517
const auto current_lane_terminal_point =
1518
1518
lanelet::utils::conversion::toGeomMsgPt (current_lanes.back ().centerline3d ().back ());
1519
- auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose (
1519
+
1520
+ double distance_to_terminal_from_goal = 0 ;
1521
+ if (is_goal_in_route) {
1522
+ distance_to_terminal_from_goal =
1523
+ utils::getDistanceToEndOfLane (route_handler.getGoalPose (), current_lanes);
1524
+ }
1525
+
1526
+ const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose (
1520
1527
prev_module_path_.points , current_lane_terminal_point,
1521
- -(lane_change_buffer + next_lane_change_buffer));
1528
+ -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal ));
1522
1529
1523
1530
if (!lane_changing_start_pose) {
1524
1531
RCLCPP_WARN (logger_, " Reject: lane changing start pose not found!!!" );
Original file line number Diff line number Diff line change @@ -412,17 +412,17 @@ PathWithLaneId getReferencePathFromTargetLane(
412
412
return std::min (dist_from_lc_start, target_lane_length - next_lane_change_buffer);
413
413
});
414
414
415
- if (s_end - s_start < lane_changing_length) {
416
- return PathWithLaneId ();
417
- }
418
-
419
415
RCLCPP_DEBUG (
420
416
rclcpp::get_logger (" behavior_path_planner" )
421
417
.get_child (" lane_change" )
422
418
.get_child (" util" )
423
419
.get_child (" getReferencePathFromTargetLane" ),
424
420
" start: %f, end: %f" , s_start, s_end);
425
421
422
+ if (s_end - s_start < lane_changing_length) {
423
+ return PathWithLaneId ();
424
+ }
425
+
426
426
const auto lane_changing_reference_path =
427
427
route_handler.getCenterLinePath (target_lanes, s_start, s_end);
428
428
You can’t perform that action at this time.
0 commit comments