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 de3802b070326..95ff106eb4c00 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 @@ -15,6 +15,10 @@ longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 + # side walk parked vehicle + object_check_min_road_shoulder_width: 0.5 # [m] + object_shiftable_ratio_threshold: 0.6 + # turn signal min_length_for_turn_signal_activation: 10.0 # [m] length_ratio_for_turn_signal_deactivation: 0.8 # ratio (desired end position) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 85a77df4c1e0d..4def596f78746 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -34,6 +34,10 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // parked vehicle + double object_check_min_road_shoulder_width{0.5}; + double object_shiftable_ratio_threshold{0.6}; + // turn signal double min_length_for_turn_signal_activation{10.0}; double length_ratio_for_turn_signal_deactivation{0.8}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 7853f712f3fac..ed41d90b7b6cf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -180,5 +180,17 @@ PredictedPath convertToPredictedPath( const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose, const size_t nearest_seg_idx, const double duration, const double resolution, const double prepare_time, const double prepare_acc, const double lane_changing_acc); + +bool passParkedObject( + const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const PathWithLaneId & current_lane_path, const PredictedObjects & objects, + const std::vector & target_lane_obj_indices, const double minimum_lane_change_length, + const bool is_goal_in_route, const double object_check_min_road_shoulder_width, + const double object_shiftable_ratio_threshold); + +boost::optional getLeadingStaticObjectIdx( + const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const PredictedObjects & objects, const std::vector & obj_indices, + const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ 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 430fbd20793f2..b44339ecc01b5 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -763,6 +763,12 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lateral_acc_sampling_num = declare_parameter(parameter("lateral_acceleration_sampling_num")); + // parked vehicle detection + p.object_check_min_road_shoulder_width = + declare_parameter(parameter("object_check_min_road_shoulder_width")); + p.object_shiftable_ratio_threshold = + declare_parameter(parameter("object_shiftable_ratio_threshold")); + // turn signal p.min_length_for_turn_signal_activation = declare_parameter(parameter("min_length_for_turn_signal_activation")); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index ab9cb75503219..0ce4659d4a6dc 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -477,10 +477,10 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); - const auto shift_intervals = - route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back()); - const double next_lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); + const double lane_change_buffer = utils::calcMinimumLaneChangeLength( + common_parameter, route_handler.getLateralIntervalsToPreferredLane(original_lanelets.back())); + const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( + common_parameter, route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back())); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), original_lanelets); @@ -680,6 +680,20 @@ bool NormalLaneChange::getLaneChangePaths( {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, getEgoPose(), common_parameter.forward_path_length, *lane_change_parameters_, lateral_buffer); + + const double object_check_min_road_shoulder_width = + lane_change_parameters_->object_check_min_road_shoulder_width; + const double object_shiftable_ratio_threshold = + lane_change_parameters_->object_shiftable_ratio_threshold; + const auto current_lane_path = route_handler.getCenterLinePath( + original_lanelets, 0.0, std::numeric_limits::max()); + const bool pass_parked_object = utils::lane_change::passParkedObject( + route_handler, *candidate_path, current_lane_path, *dynamic_objects, + dynamic_object_indices.target_lane, lane_change_buffer, is_goal_in_route, + object_check_min_road_shoulder_width, object_shiftable_ratio_threshold); + if (pass_parked_object) { + return false; + } } candidate_paths->push_back(*candidate_path); diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index aeaee91159fa6..6f03720038163 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1067,4 +1067,109 @@ PredictedPath convertToPredictedPath( return predicted_path; } + +bool passParkedObject( + const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const PathWithLaneId & current_lane_path, const PredictedObjects & objects, + const std::vector & target_lane_obj_indices, const double minimum_lane_change_length, + const bool is_goal_in_route, const double object_check_min_road_shoulder_width, + const double object_shiftable_ratio_threshold) +{ + const auto & path = lane_change_path.path; + + if (target_lane_obj_indices.empty() || path.points.empty() || current_lane_path.points.empty()) { + return false; + } + + const auto leading_obj_idx = getLeadingStaticObjectIdx( + route_handler, lane_change_path, objects, target_lane_obj_indices, + object_check_min_road_shoulder_width, object_shiftable_ratio_threshold); + if (!leading_obj_idx) { + return false; + } + + const auto & leading_obj = objects.objects.at(*leading_obj_idx); + const auto & leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj); + if (leading_obj_poly.outer().empty()) { + return false; + } + + const auto & current_path_end = current_lane_path.points.back().point.pose.position; + double min_dist_to_end_of_current_lane = std::numeric_limits::max(); + for (const auto & point : leading_obj_poly.outer()) { + const auto obj_p = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0); + const double dist = + motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end); + min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); + if (is_goal_in_route) { + const auto goal_pose = route_handler.getGoalPose(); + const double dist_to_goal = + motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, goal_pose.position); + min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); + } + } + + // If there are still enough length after the target object, we delay the lane change + if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { + return true; + } + + return false; +} + +boost::optional getLeadingStaticObjectIdx( + const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const PredictedObjects & objects, const std::vector & obj_indices, + const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) +{ + const auto & path = lane_change_path.path; + + if (path.points.empty() || obj_indices.empty()) { + return {}; + } + + const auto & lane_change_start = lane_change_path.lane_changing_start; + const auto & path_end = path.points.back(); + + double dist_lc_start_to_leading_obj = 0.0; + boost::optional leading_obj_idx = boost::none; + for (const auto & obj_idx : obj_indices) { + const auto & obj = objects.objects.at(obj_idx); + const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose; + + // ignore non-static object + // TODO(shimizu): parametrize threshold + if (obj.kinematics.initial_twist_with_covariance.twist.linear.x > 1.0) { + continue; + } + + // ignore non-parked object + if (!isParkedObject( + path, route_handler, obj, object_check_min_road_shoulder_width, + object_shiftable_ratio_threshold)) { + continue; + } + + const double dist_back_to_obj = motion_utils::calcSignedArcLength( + path.points, path_end.point.pose.position, obj_pose.position); + if (dist_back_to_obj > 0.0) { + // object is not on the lane change path + continue; + } + + const double dist_lc_start_to_obj = + motion_utils::calcSignedArcLength(path.points, lane_change_start.position, obj_pose.position); + if (dist_lc_start_to_obj < 0.0) { + // object is on the lane changing path or behind it. It will be detected in safety check + continue; + } + + if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) { + dist_lc_start_to_leading_obj = dist_lc_start_to_obj; + leading_obj_idx = obj_idx; + } + } + + return leading_obj_idx; +} } // namespace behavior_path_planner::utils::lane_change