Skip to content

Commit 08492dd

Browse files
committed
fix(lane_change): calculate distance to goal
Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
1 parent 604e6a9 commit 08492dd

File tree

2 files changed

+13
-6
lines changed

2 files changed

+13
-6
lines changed

planning/behavior_path_lane_change_module/src/scene.cpp

+9-2
Original file line numberDiff line numberDiff line change
@@ -1516,9 +1516,16 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
15161516
// lane changing start getEgoPose() is at the end of prepare segment
15171517
const auto current_lane_terminal_point =
15181518
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(
15201527
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));
15221529

15231530
if (!lane_changing_start_pose) {
15241531
RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!");

planning/behavior_path_lane_change_module/src/utils/utils.cpp

+4-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

0 commit comments

Comments
 (0)