diff --git a/planning/behavior_path_planner/behavior_path_planner_lane_change.md b/planning/behavior_path_planner/behavior_path_planner_lane_change.md index f8ebc709a52c3..ed23881cbc250 100644 --- a/planning/behavior_path_planner/behavior_path_planner_lane_change.md +++ b/planning/behavior_path_planner/behavior_path_planner_lane_change.md @@ -301,6 +301,14 @@ Finally minimum longitudinal distance for `d_rear` is added to compensate for ob std::max(longitudinal_distance_min_threshold, d_rear) < d_front + d_inter ``` +##### Collision check in prepare phase + +The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. + +![enable collision check at prepare phase](./image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png) + +The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. + #### If the lane is blocked and multiple lane changes When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance computation is as follows. @@ -370,18 +378,19 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`. The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | ------- | ------- | --------------------------------------------------------------------------------------- | ------------- | -| `lane_change_prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `lane_changing_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `minimum_lane_change_velocity` | [m/s] | double | Minimum speed during lane changing process. | 5.6 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `maximum_deceleration` | [m/s^2] | double | Ego vehicle maximum deceleration when performing lane change. | 1.0 | -| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | -| `enable_abort_lane_change` | [-] | boolean | Enable abort lane change. (\*1) | false | -| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | false | -| `lane_change_search_distance` | [m] | double | The turn signal activation distance during the lane change preparation phase. | 30.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------- | ------- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `lane_change_prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `lane_changing_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `minimum_lane_change_velocity` | [m/s] | double | Minimum speed during lane changing process. | 5.6 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `maximum_deceleration` | [m/s^2] | double | Ego vehicle maximum deceleration when performing lane change. | 1.0 | +| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | +| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | false | +| `lane_change_search_distance` | [m] | double | The turn signal activation distance during the lane change preparation phase. | 30.0 | +| `enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | false | +| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | ### Collision checks during lane change diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 865a1bcea19fb..cf375e14317a7 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -27,9 +27,9 @@ visualize_drivable_area_for_shared_linestrings_lanelet: true - lateral_distance_max_threshold: 5.0 + lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 - expected_front_deceleration: -1.0 + expected_front_deceleration: -0.5 expected_rear_deceleration: -1.0 rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 2.0 diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index a1ad81b61e931..da89dddc6bd8f 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -12,8 +12,9 @@ abort_lane_change_velocity_thresh: 0.5 abort_lane_change_angle_thresh: 10.0 # [deg] abort_lane_change_distance_thresh: 0.3 # [m] + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] enable_abort_lane_change: true - enable_collision_check_at_prepare_phase: true + enable_collision_check_at_prepare_phase: false use_predicted_path_outside_lanelet: true use_all_predicted_path: true publish_debug_marker: false diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png b/planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png new file mode 100644 index 0000000000000..b9acb71d95f4a Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png differ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index 3cd4546422a12..f9ce032760e45 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -25,25 +25,26 @@ namespace behavior_path_planner { struct LaneChangeParameters { - double lane_change_prepare_duration; - double lane_changing_duration; - double minimum_lane_change_prepare_distance; - double lane_change_finish_judge_buffer; - double minimum_lane_change_velocity; - double prediction_time_resolution; - double maximum_deceleration; - int lane_change_sampling_num; - double abort_lane_change_velocity_thresh; - double abort_lane_change_angle_thresh; - double abort_lane_change_distance_thresh; - bool enable_abort_lane_change; - bool enable_collision_check_at_prepare_phase; - bool use_predicted_path_outside_lanelet; - bool use_all_predicted_path; - bool publish_debug_marker; + double lane_change_prepare_duration{2.0}; + double lane_changing_duration{4.0}; + double minimum_lane_change_prepare_distance{4.0}; + double lane_change_finish_judge_buffer{3.0}; + double minimum_lane_change_velocity{5.6}; + double prediction_time_resolution{0.5}; + double maximum_deceleration{1.0}; + int lane_change_sampling_num{10}; + double abort_lane_change_velocity_thresh{0.5}; + double abort_lane_change_angle_thresh{0.174533}; + double abort_lane_change_distance_thresh{0.3}; + double prepare_phase_ignore_target_speed_thresh{0.1}; + bool enable_abort_lane_change{true}; + bool enable_collision_check_at_prepare_phase{true}; + bool use_predicted_path_outside_lanelet{false}; + bool use_all_predicted_path{false}; + bool publish_debug_marker{false}; // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; + double drivable_area_right_bound_offset{0.0}; + double drivable_area_left_bound_offset{0.0}; }; struct LaneChangeStatus diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 478f436e0135b..a09a99460e626 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -198,6 +198,9 @@ bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_p bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon); +bool calcObjectPolygon( + const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon); + PredictedPath resamplePredictedPath( const PredictedPath & input_path, const double resolution, const double duration); @@ -425,13 +428,13 @@ std::vector getPredictedPathFromObj( Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object); bool getEgoExpectedPoseAndConvertToPolygon( - const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose, + const Pose & current_pose, const PredictedPath & pred_path, tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time, - const VehicleInfo & ego_info); + const VehicleInfo & ego_info, Pose & expected_pose, std::string & failed_reason); bool getObjectExpectedPoseAndConvertToPolygon( - const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose, - Polygon2d & obj_polygon, const double & check_current_time); + const PredictedPath & pred_path, const PredictedObject & object, Polygon2d & obj_polygon, + const double & check_current_time, Pose & expected_pose, std::string & failed_reason); bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 71197f438d84c..1ece7892e4cae 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -227,10 +227,10 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 3.0); + p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 2.0); p.longitudinal_distance_min_threshold = declare_parameter("longitudinal_distance_min_threshold", 3.0); - p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -1.0); + p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -0.5); p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration", -1.0); p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 2.0); p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0); @@ -349,7 +349,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lane_changing_duration = dp("lane_changing_duration", 4.0); p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0); p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0); - p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 8.3); + p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 5.6); p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.lane_change_sampling_num = dp("lane_change_sampling_num", 10); @@ -357,6 +357,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.abort_lane_change_angle_thresh = dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0)); p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3); + p.prepare_phase_ignore_target_speed_thresh = dp("prepare_phase_ignore_target_speed_thresh", 0.1); p.enable_abort_lane_change = dp("enable_abort_lane_change", true); p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); @@ -733,7 +734,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( if (isForcedCandidatePath()) { for (auto & path_point : path_candidate->points) { - path_point.point.longitudinal_velocity_mps = 1.0; + path_point.point.longitudinal_velocity_mps = 0.0; } } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index f3c1a0038df23..91a13ffbb2044 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -367,13 +367,13 @@ bool isLaneChangePathSafe( const auto & enable_collision_check_at_prepare_phase = lane_change_parameters.enable_collision_check_at_prepare_phase; const auto & lane_changing_duration = lane_change_parameters.lane_changing_duration; - const double check_start_time = - (enable_collision_check_at_prepare_phase) ? 0.0 : lane_change_prepare_duration; const double check_end_time = lane_change_prepare_duration + lane_changing_duration; constexpr double ego_predicted_path_min_speed{1.0}; const auto vehicle_predicted_path = util::convertToPredictedPath( path, current_twist, current_pose, static_cast(current_seg_idx), check_end_time, time_resolution, acceleration, ego_predicted_path_min_speed); + const auto prepare_phase_ignore_target_speed_thresh = + lane_change_parameters.prepare_phase_ignore_target_speed_thresh; const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); @@ -416,6 +416,12 @@ bool isLaneChangePathSafe( for (const auto & i : current_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); + const auto object_speed = + util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); + const double check_start_time = (enable_collision_check_at_prepare_phase && + (object_speed > prepare_phase_ignore_target_speed_thresh)) + ? 0.0 + : lane_change_prepare_duration; auto current_debug_data = assignDebugData(obj); const auto predicted_paths = util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); @@ -449,6 +455,12 @@ bool isLaneChangePathSafe( const auto predicted_paths = util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); + const auto object_speed = + util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); + const double check_start_time = (enable_collision_check_at_prepare_phase && + (object_speed > prepare_phase_ignore_target_speed_thresh)) + ? 0.0 + : lane_change_prepare_duration; if (is_object_in_target) { for (const auto & obj_path : predicted_paths) { if (!util::isSafeInLaneletCollisionCheck( diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 6ea03789e20ce..c8c8ac5ec692f 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -541,6 +541,46 @@ bool lerpByTimeStamp(const PredictedPath & path, const double t_query, Pose * le return false; } +bool lerpByTimeStamp( + const PredictedPath & path, const double t_query, Pose * lerped_pt, std::string & failed_reason) +{ + const rclcpp::Duration time_step(path.time_step); + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + if (lerped_pt == nullptr) { + failed_reason = "nullptr_pt"; + return false; + } + if (path.path.empty()) { + failed_reason = "empty_path"; + return false; + } + + const double t_final = time_step.seconds() * static_cast(path.path.size() - 1); + if (t_query > t_final) { + failed_reason = "query_exceed_t_final"; + *lerped_pt = path.path.back(); + + return false; + } + + for (size_t i = 1; i < path.path.size(); i++) { + const auto & pt = path.path.at(i); + const auto & prev_pt = path.path.at(i - 1); + const double t = time_step.seconds() * static_cast(i); + if (t_query <= t) { + const double prev_t = time_step.seconds() * static_cast(i - 1); + const double duration = time_step.seconds(); + const double offset = t_query - prev_t; + const double ratio = offset / duration; + *lerped_pt = lerpByPose(prev_pt, pt, ratio); + return true; + } + } + + failed_reason = "unknown_failure"; + return false; +} + double getDistanceBetweenPredictedPaths( const PredictedPath & object_path, const PredictedPath & ego_path, const double start_time, const double end_time, const double resolution) @@ -832,6 +872,28 @@ bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygo return true; } +bool calcObjectPolygon( + const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon) +{ + if (object_shape.type == Shape::BOUNDING_BOX) { + const double & length_m = object_shape.dimensions.x / 2; + const double & width_m = object_shape.dimensions.y / 2; + *object_polygon = + convertBoundingBoxObjectToGeometryPolygon(object_pose, length_m, length_m, width_m); + + } else if (object_shape.type == Shape::CYLINDER) { + *object_polygon = convertCylindricalObjectToGeometryPolygon(object_pose, object_shape); + } else if (object_shape.type == Shape::POLYGON) { + *object_polygon = convertPolygonObjectToGeometryPolygon(object_pose, object_shape); + } else { + RCLCPP_WARN( + rclcpp::get_logger("behavior_path_planner").get_child("utilities"), "Object shape unknown!"); + return false; + } + + return true; +} + std::vector calcObjectsDistanceToPath( const PredictedObjects & objects, const PathWithLaneId & ego_path) { @@ -2341,13 +2403,12 @@ void getProjectedDistancePointFromPolygons( } bool getEgoExpectedPoseAndConvertToPolygon( - const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose, + const Pose & current_pose, const PredictedPath & pred_path, tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time, - const VehicleInfo & ego_info) + const VehicleInfo & ego_info, Pose & expected_pose, std::string & failed_reason) { - if (!util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose)) { - return false; - } + bool is_lerped = + util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose, failed_reason); expected_pose.orientation = current_pose.orientation; const auto & i = ego_info; @@ -2356,21 +2417,21 @@ bool getEgoExpectedPoseAndConvertToPolygon( const auto & back_m = i.rear_overhang_m; ego_polygon = - util::convertBoundingBoxObjectToGeometryPolygon(current_pose, front_m, back_m, width_m); + util::convertBoundingBoxObjectToGeometryPolygon(expected_pose, front_m, back_m, width_m); - return true; + return is_lerped; } bool getObjectExpectedPoseAndConvertToPolygon( - const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose, - Polygon2d & obj_polygon, const double & check_current_time) + const PredictedPath & pred_path, const PredictedObject & object, Polygon2d & obj_polygon, + const double & check_current_time, Pose & expected_pose, std::string & failed_reason) { - if (!util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose)) { - return false; - } + bool is_lerped = + util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose, failed_reason); expected_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; - return util::calcObjectPolygon(object, &obj_polygon); + is_lerped = util::calcObjectPolygon(object.shape, expected_pose, &obj_polygon); + return is_lerped; } std::vector getPredictedPathFromObj( @@ -2507,22 +2568,17 @@ bool isSafeInLaneletCollisionCheck( debug.lerped_path.reserve(static_cast(lerp_path_reserve)); } + Pose expected_obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; + Pose expected_ego_pose = ego_current_pose; for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { - Pose expected_obj_pose; tier4_autoware_utils::Polygon2d obj_polygon; - if (!util::getObjectExpectedPoseAndConvertToPolygon( - target_object_path, target_object, expected_obj_pose, obj_polygon, t)) { - debug.failed_reason = "get_obj_info_failed"; - continue; - } + [[maybe_unused]] const auto get_obj_info = util::getObjectExpectedPoseAndConvertToPolygon( + target_object_path, target_object, obj_polygon, t, expected_obj_pose, debug.failed_reason); - Pose expected_ego_pose; tier4_autoware_utils::Polygon2d ego_polygon; - if (!util::getEgoExpectedPoseAndConvertToPolygon( - ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, ego_info)) { - debug.failed_reason = "get_ego_info_failed"; - continue; - } + [[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon( + ego_current_pose, ego_predicted_path, ego_polygon, t, ego_info, expected_ego_pose, + debug.failed_reason); debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon; @@ -2560,14 +2616,12 @@ bool isSafeInFreeSpaceCollisionCheck( if (!util::calcObjectPolygon(target_object, &obj_polygon)) { return false; } + Pose expected_ego_pose = ego_current_pose; for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { - Pose expected_ego_pose; tier4_autoware_utils::Polygon2d ego_polygon; - if (!util::getEgoExpectedPoseAndConvertToPolygon( - ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, ego_info)) { - debug.failed_reason = "get_ego_info_failed"; - continue; - } + [[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon( + ego_current_pose, ego_predicted_path, ego_polygon, t, ego_info, expected_ego_pose, + debug.failed_reason); debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon;