diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 86d8d1c0c1413..5a6c22a731bf9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -886,6 +886,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | | `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | +| `enable_stopped_vehicle_buffer` | [-] | bool | If true, will keep enough distance from current lane front stopped object to perform lane change when possible | true | | `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 | | `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index bf892b3058a16..61e41b4ea3ea4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -5,6 +5,7 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] backward_length_from_intersection: 5.0 # [m] + enable_stopped_vehicle_buffer: true # turn signal min_length_for_turn_signal_activation: 10.0 # [m] diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 719bbbbf3057a..94020e8a08279 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -149,6 +149,7 @@ struct Parameters double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; double backward_length_from_intersection{5.0}; + bool enable_stopped_vehicle_buffer{false}; // parked vehicle double object_check_min_road_shoulder_width{0.5}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 1bb41069140e7..9f7a811770b5d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -195,6 +195,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); p.backward_length_from_intersection = getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); + p.enable_stopped_vehicle_buffer = + getOrDeclareParameter(*node, parameter("enable_stopped_vehicle_buffer")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -305,6 +307,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorbackward_length_buffer_for_blocking_object); updateParam( parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer); + updateParam( + parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer); updateParam( parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 7f0e6d7f575f6..e2543c602cbfa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -515,7 +515,9 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path"; - if (filtered_objects_.current_lane.empty()) { + if ( + filtered_objects_.current_lane.empty() || + !lane_change_parameters_->enable_stopped_vehicle_buffer) { set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } @@ -948,8 +950,6 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const filtered_objects.target_lane_trailing.reserve(reserve_size); filtered_objects.others.reserve(reserve_size); - const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - for (const auto & object : objects.objects) { auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_); const auto & ext_obj_pose = ext_object.initial_pose; @@ -968,12 +968,8 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const continue; } - // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior - const auto is_moving = velocity_filter( - ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits::max()); - if ( - ahead_of_ego && is_moving && is_before_terminal && + ahead_of_ego && is_before_terminal && !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { // check only the objects that are in front of the ego vehicle filtered_objects.current_lane.push_back(ext_object); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 147ef3856ac4e..9360435891632 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -920,19 +920,17 @@ double get_min_dist_to_current_lanes_obj( continue; } - // calculate distance from path front to the stationary object polygon on the ego lane. - for (const auto & polygon_p : object.initial_polygon.outer()) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); - - // ignore if the point is not on ego path - if (std::abs(lateral_fp) > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { - continue; - } - - const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp); - min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); + // check if object is on ego path + const auto obj_half_width = object.shape.dimensions.y / 2; + const auto obj_lat_dist_to_path = + std::abs(motion_utils::calcLateralOffset(path_points, object.initial_pose.position)) - + obj_half_width; + if (obj_lat_dist_to_path > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { + continue; } + + min_dist_to_obj = std::min(min_dist_to_obj, dist_to_obj); + break; } return min_dist_to_obj; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 114a38a77876c..5fb445788672c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -242,15 +242,14 @@ TEST_F(TestNormalLaneChange, testFilteredObjects) const auto & filtered_objects = get_filtered_objects(); - // Note: There's 1 stopping object in current lanes, however, it was filtered out. const auto filtered_size = filtered_objects.current_lane.size() + filtered_objects.target_lane_leading.size() + filtered_objects.target_lane_trailing.size() + filtered_objects.others.size(); EXPECT_EQ(filtered_size, planner_data_->dynamic_object->objects.size()); - EXPECT_EQ(filtered_objects.current_lane.size(), 0); + EXPECT_EQ(filtered_objects.current_lane.size(), 1); EXPECT_EQ(filtered_objects.target_lane_leading.size(), 2); EXPECT_EQ(filtered_objects.target_lane_trailing.size(), 0); - EXPECT_EQ(filtered_objects.others.size(), 2); + EXPECT_EQ(filtered_objects.others.size(), 1); } TEST_F(TestNormalLaneChange, testGetPathWhenValid)