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 731bb9059768f..3e421e05ba8ba 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 7e0beea10a91e..8fa19e2bc58ec 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 1d2cb00fafe2a..a1973d426f1f8 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 @@ -194,6 +194,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( @@ -304,6 +306,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 adbf9ea52364e..dfb5b7a94a0da 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 @@ -552,7 +552,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); - 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); return; } @@ -1022,8 +1024,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; @@ -1042,12 +1042,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 e30e5c4ff2f83..841e1c7bb5b99 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 @@ -1101,19 +1101,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 5ca308d0d4abb..61c19ea504d2e 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 @@ -209,6 +209,28 @@ TEST_F(TestNormalLaneChange, testGetPathWhenInvalid) ASSERT_FALSE(lc_status.is_valid_path); } +TEST_F(TestNormalLaneChange, testFilteredObjects) +{ + constexpr auto is_approved = true; + ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0); + planner_data_->self_odometry = set_odometry(ego_pose_); + set_previous_approved_path(); + + normal_lane_change_->update_lanes(!is_approved); + normal_lane_change_->update_filtered_objects(); + + const auto & filtered_objects = get_filtered_objects(); + + 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(), 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(), 1); +} + TEST_F(TestNormalLaneChange, testGetPathWhenValid) { constexpr auto is_approved = true;