feat(behavior_path_side_shift): add drivable area check to prevent lane departure#12068
feat(behavior_path_side_shift): add drivable area check to prevent lane departure#12068k-uta wants to merge 15 commits intoautowarefoundation:mainfrom
Conversation
…ment lateral offset calculation Signed-off-by: Uta Kawakami <[email protected]>
… update shift status handling Signed-off-by: Uta Kawakami <[email protected]>
…d enhance drivable area checks Signed-off-by: Uta Kawakami <[email protected]>
…ution Signed-off-by: Uta Kawakami <[email protected]>
Apply code review suggestion to remove unnecessary index usage. Co-authored-by: Taiki Yamada <[email protected]> Signed-off-by: Uta Kawakami <[email protected]>
Set the default mode to 1 (current lane only) as it is the safest option. Signed-off-by: Uta Kawakami <[email protected]>
…dd lane limit helper functions Signed-off-by: Uta Kawakami <[email protected]>
…tor SideShift module Signed-off-by: Uta Kawakami <[email protected]>
Signed-off-by: Uta Kawakami <[email protected]>
Signed-off-by: Uta Kawakami <[email protected]>
…e_area_margin Signed-off-by: Uta Kawakami <[email protected]>
k-uta
requested review from
TomohitoAndo,
kyoichi-sugahara,
rej55,
satoshi-ota,
shmpwk and
tkimura4
as code owners
github-actions
bot
added
the
component:planning
|
Thank you for contributing to the Autoware project! 🚧 If your pull request is in progress, switch it to draft mode. Please ensure:
|
There was a problem hiding this comment.
Pull request overview
This PR adds a drivable area boundary check mechanism to the side_shift module to prevent the ego vehicle from departing from valid lane boundaries when executing lateral offset requests. Previously, the module would execute shift requests without checking lane boundaries, which could lead to unsafe lane departures.
Changes:
- Added configurable boundary checking with three modes (0: disabled, 1: current lane only, 2: current + adjacent lanes)
- Implemented
calcMaxLateralOffsetfunction to compute safe shift limits based on vehicle geometry, margins, and lane boundaries - Enhanced error handling with better fallback mechanisms when path generation fails
- Improved module state management to keep the module running when there's an active shift
Reviewed changes
Copilot reviewed 5 out of 5 changed files in this pull request and generated 6 comments.
Show a summary per file
| File | Description |
|---|---|
| src/scene.cpp | Core implementation of boundary checking logic, shift calculation improvements, and enhanced error handling |
| src/manager.cpp | Parameter initialization and dynamic parameter update support for new boundary checking features |
| include/.../scene.hpp | Header declarations for new boundary checking helper functions and data structures |
| include/.../data_structs.hpp | Added drivable_area_check_mode and min_drivable_area_margin parameters |
| config/side_shift.param.yaml | Configuration for boundary checking mode and margin with defaults |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| double SideShiftModule::calcMaxLateralOffset(const double requested_offset) const | ||
| { | ||
| // special for avoidance: take behind distance upt ot shift-start-point if it exist. | ||
| const auto longest_dist_to_shift_point = [&]() { | ||
| double max_dist = 0.0; | ||
| for (const auto & pnt : path_shifter_.getShiftLines()) { | ||
| max_dist = std::max(max_dist, calc_distance2d(getEgoPose(), pnt.start)); | ||
| // Mode 0: No boundary check, return requested offset directly | ||
| if (parameters_->drivable_area_check_mode == 0) { | ||
| return requested_offset; | ||
| } | ||
|
|
||
| const auto & route_handler = planner_data_->route_handler; | ||
| const auto & p = planner_data_->parameters; | ||
| const double margin = parameters_->min_drivable_area_margin; | ||
| const double vehicle_half_width = p.vehicle_width / 2.0; | ||
|
|
||
| const size_t nearest_idx = planner_data_->findEgoIndex(reference_path_.points); | ||
| const auto ego_speed = std::abs(planner_data_->self_odometry->twist.twist.linear.x); | ||
|
|
||
| // Calculate distance to start shifting to skip checking the path before shifting | ||
| const double dist_to_start = std::max( | ||
| parameters_->min_distance_to_start_shifting, ego_speed * parameters_->time_to_start_shifting); | ||
|
|
||
| // Estimate shifting distance to determine check horizon | ||
| const double shift_length = std::abs(requested_offset); | ||
| const double jerk_shifting_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( | ||
| shift_length, parameters_->shifting_lateral_jerk, | ||
| std::max(ego_speed, parameters_->min_shifting_speed)); | ||
| const double shifting_distance = | ||
| std::max(jerk_shifting_distance, parameters_->min_shifting_distance); | ||
|
|
||
| // Check up to end of shift + buffer (e.g. 20m or 2s) to avoid restricting the shift based on | ||
| // far-future road conditions | ||
| const double check_distance = dist_to_start + shifting_distance + std::max(20.0, ego_speed * 2.0); | ||
|
|
||
| const size_t start_idx = utils::getIdxByArclength(reference_path_, nearest_idx, dist_to_start); | ||
| const size_t end_idx = utils::getIdxByArclength(reference_path_, nearest_idx, check_distance); | ||
|
|
||
| LaneLimitInfo limits; | ||
|
|
||
| // Check range: from shift start point to end of check distance | ||
| for (size_t i = start_idx; i < std::min(end_idx, reference_path_.points.size()); ++i) { | ||
| const auto & pt = reference_path_.points.at(i); | ||
| const auto & pose = pt.point.pose; | ||
|
|
||
| lanelet::ConstLanelet lane; | ||
| if (!route_handler->getClosestLaneletWithinRoute(pose, &lane)) { | ||
| continue; | ||
| } | ||
| return max_dist; | ||
| }(); | ||
|
|
||
| const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. | ||
| const auto backward_length = std::max( | ||
| planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); | ||
| const lanelet::BasicPoint2d target_point(pose.position.x, pose.position.y); | ||
| const auto adj_info = getAdjacentLaneInfo(lane); | ||
|
|
||
| const auto & prev_reference = getPreviousModuleOutput().path; | ||
| const size_t orig_ego_idx = findNearestIndex(original_path.points, getEgoPose().position); | ||
| const size_t prev_ego_idx = findNearestSegmentIndex( | ||
| prev_reference.points, get_point(original_path.points.at(orig_ego_idx))); | ||
| // Determine which lane the reference point is in | ||
| const auto lane_pos = determineLanePosition(lane, target_point, adj_info); | ||
|
|
||
| size_t clip_idx = 0; | ||
| for (size_t i = 0; i < prev_ego_idx; ++i) { | ||
| if (backward_length > calcSignedArcLength(prev_reference.points, clip_idx, prev_ego_idx)) { | ||
| break; | ||
| if (lane_pos.is_outside_all) { | ||
| updateLaneLimitsForOutsidePoint(lane, target_point, vehicle_half_width, margin, limits); | ||
| continue; | ||
| } | ||
| clip_idx = i; | ||
|
|
||
| // Calculate available space based on the lane the point is in | ||
| updateLaneLimitsForInsidePoint( | ||
| lane_pos.check_lane, target_point, adj_info.allow_left, adj_info.allow_right, | ||
| lane_pos.is_in_adjacent, adj_info.left_lane, adj_info.right_lane, vehicle_half_width, margin, | ||
| limits); | ||
| } | ||
|
|
||
| if (!limits.found_valid_limit) { | ||
| RCLCPP_WARN_THROTTLE( | ||
| getLogger(), *clock_, 1000, "SideShift: No valid lane found for limit check. Returning 0.0"); | ||
| return 0.0; | ||
| } | ||
|
|
||
| return clampOffsetToLimits(requested_offset, limits.safe_left_limit, limits.safe_right_limit); | ||
| } |
There was a problem hiding this comment.
The new calcMaxLateralOffset function and related boundary checking logic lack test coverage. This is a critical safety feature that prevents lane departures, so it should have comprehensive unit tests covering different scenarios such as: mode 0/1/2 behavior, points inside/outside lanes, adjacent lane checking, and edge cases like invalid lanelets or insufficient space.
.../behavior_path_planner/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml
Show resolved
Hide resolved
| // Check if the shift was aborted (indicated by zero-length shift with same start/end index) | ||
| const bool shift_aborted = | ||
| (new_sl.start_idx == new_sl.end_idx) && (std::abs(new_sl.end_shift_length) < 0.01); |
There was a problem hiding this comment.
The abort detection logic checks for start_idx == end_idx AND end_shift_length < 0.01, but in calcShiftLine (line 467), when the shift span is too short, only end_shift_length is set to 0.0 without ensuring start_idx == end_idx. This could cause the abort detection to miss cases where the shift was intentionally aborted due to insufficient span. Consider also setting end_idx = start_idx when aborting in calcShiftLine to make the abort signal more explicit and reliable.
| // Check if the shift was aborted (indicated by zero-length shift with same start/end index) | |
| const bool shift_aborted = | |
| (new_sl.start_idx == new_sl.end_idx) && (std::abs(new_sl.end_shift_length) < 0.01); | |
| // Check if the shift was aborted (indicated by zero-length shift from calcShiftLine) | |
| const bool shift_aborted = std::abs(new_sl.end_shift_length) < 0.01; |
| const double dist_to_left = lanelet::geometry::distance2d(left_bound, target_point); | ||
| const double dist_to_right = lanelet::geometry::distance2d(right_bound, target_point); | ||
|
|
||
| const double extra_buffer = 0.05; |
There was a problem hiding this comment.
The extra_buffer constant is hard-coded here without explanation. Consider making this a parameter or adding a comment explaining why 0.05m is chosen. This magic number could make the code harder to maintain and tune for different scenarios.
There was a problem hiding this comment.
Thanks for flagging this!
I've replaced the local magic number with a named constexpr kLaneBoundaryDiscretizationBuffer at the namespace scope, accompanied by a detailed comment explaining its derivation.
Specifically, the lanelet boundary polyline discretization error is bounded by segment_length² / (8 × radius), which is ~0.013 m for typical maps. The 0.05 m value provides ~4× headroom to safely absorb floating-point and coordinate transform rounding errors.
I chose not to make it a tunable parameter since it's a numerical guard tied strictly to map representation accuracy, rather than a design-level margin. The min_drivable_area_margin parameter already serves the purpose of a tunable safety boundary.
| if (allow_left && !point_in_adjacent) { | ||
| const double dist_to_far_left = | ||
| lanelet::geometry::distance2d(left_lane_val.leftBound2d(), target_point); | ||
| current_max_left = dist_to_far_left - vehicle_half_width - margin - extra_buffer; |
There was a problem hiding this comment.
When allow_left or allow_right is true but the lane values (left_lane_val or right_lane_val) are accessed, there's no validation that these lanelets are valid/initialized. If getAdjacentLaneInfo sets allow_left to true but left_lane is somehow invalid, calling left_lane_val.leftBound2d() could cause undefined behavior. Consider adding validation that the lane is valid before accessing its bounds.
| if (allow_right && !point_in_adjacent) { | ||
| const double dist_to_far_right = | ||
| lanelet::geometry::distance2d(right_lane_val.rightBound2d(), target_point); | ||
| current_max_right = dist_to_far_right - vehicle_half_width - margin - extra_buffer; |
There was a problem hiding this comment.
Same issue as above - when allow_right is true and point_in_adjacent is false, right_lane_val.rightBound2d() is called without validating that the lanelet is initialized. This could lead to undefined behavior if the lanelet is not properly set.
k-uta
force-pushed
the
feat/side-shift-boundary-check
branch
from
d06647e to
7e97c2a
Compare
…and document math Signed-off-by: Uta Kawakami <[email protected]>
1 participant
Description
This PR implements a boundary check mechanism in the
side_shiftmodule to prevent the ego vehicle from deviating from the drivable area (Lanelet) when a lateral offset is requested.Previously, the module would execute the requested shift amount regardless of lane boundaries, which could lead to unsafe lane departures. With this update, the module calculates the maximum allowable lateral offset based on the distance to the lanelet boundaries and clamps the requested value to ensure the vehicle stays within the safe area.
Changes
calcMaxLateralOffsetfunction: Calculates the safe shift limit based on the vehicle width, configured margin, and distance to lane boundaries.drivable_area_check_modeandmin_drivable_area_margin.SideShiftModuleto use the clampedfinal_shift_lengthinstead of the raw requested offset.Interface changes
ROS Parameters
The following parameters have been added to
side_shift.param.yaml:drivable_area_check_mode11: Check current lane only
2: Check current and same-direction neighbor lanes
min_drivable_area_margin0.3Effects on system behavior
How was this PR tested?
drivable_area_check_modeto1or2.Command: