From dbcf7aa9da79468ff28ee89483af5a2264384e0e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 11 Oct 2024 14:18:48 +0900 Subject: [PATCH] fix(lane_change): insert stop for current lanes object (RT0-33761) (#9070) * RT0-33761 fix lc insert stop for current lanes object Signed-off-by: Zulfaqar Azmi * fix wrong value used for comparison Signed-off-by: Zulfaqar Azmi * ignore current lane object that is not on ego's path Signed-off-by: Zulfaqar Azmi * remove print Signed-off-by: Zulfaqar Azmi * update readme Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> * revert is_within_vel_th removal Signed-off-by: Zulfaqar Azmi * fix flowchart too wide Signed-off-by: Zulfaqar Azmi * rename variable in has_blocking_target_object_for_stopping Signed-off-by: Zulfaqar Azmi * Add docstring and rename function Signed-off-by: Zulfaqar Azmi * change color Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../README.md | 91 +++++++- .../scene.hpp | 6 +- .../utils/base_class.hpp | 2 +- .../utils/utils.hpp | 42 ++++ .../src/interface.cpp | 2 +- .../src/scene.cpp | 205 ++++++++---------- .../src/utils/utils.cpp | 65 ++++++ 7 files changed, 286 insertions(+), 127 deletions(-) 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 02280f7ffa697..a52d842b5dcb9 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 @@ -195,7 +195,7 @@ if (max_lane_change_length > ego's distance to the end of the current lanes.) t stop endif -if ego is stuck in the current lanes then (yes) +if (ego is stuck in the current lanes) then (yes) :Return **sampled acceleration values**; stop else (no) @@ -540,14 +540,65 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) -#### Stopping position when an object exists ahead +### Stopping behavior -When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change. -The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case. +The stopping behavior of the ego vehicle is determined based on various factors, such as the number of lane changes required, the presence of obstacles, and the position of blocking objects in relation to the lane change plan. The objective is to choose a suitable stopping point that allows for a safe and effective lane change while adapting to different traffic scenarios. -##### When the ego vehicle is near the end of the lane change +The following flowchart and subsections explain the conditions for deciding where to insert a stop point when an obstacle is ahead. -Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Inserting Stop Point + +start +if (number of lane changes is zero?) then (YES) +stop +else (NO) +endif + +if (do we want to insert stop point in current lanes?) then (NO) +#LightPink:Insert stop point at next lane change terminal start.; +stop +else (YES) +endif + +if (Is there leading object in the current lanes that blocks ego's path?) then (NO) +#LightPink:Insert stop point at terminal stop.; +stop +else (YES) +endif + +if (Blocking object's position is after target lane's start position?) then (NO) +#LightPink:Insert stop at the target lane's start position.; +stop +else (YES) +endif + +if (Blocking object's position is before terminal stop position?) then (NO) +#LightPink:Insert stop at the terminal stop position; +stop +else (YES) +endif + +if (Are there target lane objects between the ego and the blocking object?) then (NO) +#LightGreen:Insert stop behind the blocking object; +stop +else (YES) +#LightPink:Insert stop at terminal stop; +stop +@enduml +``` + +#### Ego vehicle's stopping position when an object exists ahead + +When the ego vehicle encounters an obstacle ahead, it stops while maintaining a safe distance to prepare for a possible lane change. The exact stopping position depends on factors like whether the target lane is clear or if the lane change needs to be delayed. The following explains how different stopping scenarios are handled: + +##### When the near the end of the lane change + +Whether the target lane has obstacles or is clear, the ego vehicle stops while keeping a safe distance from the obstacle ahead, ensuring there is enough room for the lane change. ![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) @@ -555,20 +606,40 @@ Regardless of the presence or absence of objects in the lane change target lane, ##### When the ego vehicle is not near the end of the lane change -If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. +The ego vehicle stops while maintaining a safe distance from the obstacle ahead, ensuring there is enough space for a lane change. + +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) + +#### Ego vehicle's stopping position when an object exists in the lane changing section + +If there are objects within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without maintaining the usual distance for a lane change. + +##### When near the end of the lane change + +Regardless of whether there are obstacles in the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead, allowing for the lane change. + +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) + +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) + +##### When not near the end of the lane change + +If there are no obstacles in the lane change section of the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead to accommodate the lane change. ![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) -If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. +If there are obstacles within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without keeping the usual distance needed for a lane change. ![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) -##### When the target lane is far away +#### When the target lane is far away -When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. +If the target lane for the lane change is far away and not next to the current lane, the ego vehicle stops closer to the obstacle ahead, as maintaining the usual distance for a lane change is not necessary. ![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) + ### Lane Change When Stuck The ego vehicle is considered stuck if it is stopped and meets any of the following conditions: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index cc06d0b114d7b..3b414235a9f54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -70,7 +70,9 @@ class NormalLaneChange : public LaneChangeBase void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; - void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + + void insert_stop_point_on_current_lanes(PathWithLaneId & path); PathWithLaneId getReferencePath() const override; @@ -209,7 +211,7 @@ class NormalLaneChange : public LaneChangeBase std::pair calcCurrentMinMaxAcceleration() const; - void setStopPose(const Pose & stop_pose); + void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); void updateStopTime(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index c124353e2873a..aaf9df7144d74 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -127,7 +127,7 @@ class LaneChangeBase virtual void updateSpecialData() {} - virtual void insertStopPoint( + virtual void insert_stop_point( [[maybe_unused]] const lanelet::ConstLanelets & lanelets, [[maybe_unused]] PathWithLaneId & path) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 3219dc7e777f1..729eaa5ca8268 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -266,6 +266,48 @@ ExtendedPredictedObjects transform_to_extended_objects( double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, const bool ignore_intersection = false); + +/** + * @brief Calculates the minimum distance to a stationary object in the current lanes. + * + * This function determines the closest distance from the ego vehicle to a stationary object + * in the current lanes. It checks if the object is within the stopping criteria based on its + * velocity and calculates the distance while accounting for the object's size. Only objects + * positioned after the specified distance to the target lane's start are considered. + * + * @param common_data_ptr Pointer to the common data structure containing parameters for lane + * change. + * @param filtered_objects A collection of objects filtered by lanes, including those in the current + * lane. + * @param dist_to_target_lane_start The distance to the start of the target lane from the ego + * vehicle. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return The minimum distance to a stationary object in the current lanes. If no valid object is + * found, returns the maximum possible double value. + */ +double get_min_dist_to_current_lanes_obj( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double dist_to_target_lane_start, const PathWithLaneId & path); + +/** + * @brief Checks if there is an object in the target lane that influences the decision to insert a + * stop point. + * + * This function determines whether any objects exist in the target lane that would affect + * the decision to place a stop point behind a blocking object in the current lane. + * + * @param common_data_ptr Pointer to the common data structure containing parameters for the lane + * change. + * @param filtered_objects A collection of objects filtered by lanes, including those in the target + * lane. + * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return true if there is an object in the target lane that influences the stop point decision; + * otherwise, false. + */ +bool has_blocking_target_object( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double stop_arc_length, const PathWithLaneId & path); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 09550a135ba3c..8fb1459f62c98 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -154,7 +154,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() BehaviorModuleOutput out = getPreviousModuleOutput(); - module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); 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 5a9ee485320b8..532a99a67195b 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 @@ -370,14 +370,14 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); - insertStopPoint(get_current_lanes(), prev_module_output_.path); + insert_stop_point(get_current_lanes(), prev_module_output_.path); return prev_module_output_; } auto output = prev_module_output_; if (isAbortState() && abort_path_) { output.path = abort_path_->path; - insertStopPoint(get_current_lanes(), output.path); + insert_stop_point(get_current_lanes(), output.path); } else { output.path = status_.lane_change_path.path; @@ -395,10 +395,9 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path.points, output.path.points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); - setStopPose(stop_point.point.pose); + set_stop_pose(stop_dist + current_dist, output.path); } else { - insertStopPoint(get_target_lanes(), output.path); + insert_stop_point(get_target_lanes(), output.path); } } @@ -432,7 +431,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c current_drivable_area_info, prev_module_output_.drivable_area_info); } -void NormalLaneChange::insertStopPoint( +void NormalLaneChange::insert_stop_point( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -446,131 +445,109 @@ void NormalLaneChange::insertStopPoint( return; } - const auto [_, lanes_dist_buffer] = - calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, lanelets); + const auto & current_lanes = get_current_lanes(); + const auto is_current_lane = lanelets.front().id() == current_lanes.front().id() && + lanelets.back().id() == current_lanes.back().id(); + + // if input is not current lane, we can just insert the points at terminal. + if (!is_current_lane) { + const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) - + common_data_ptr_->transient_data.next_dist_buffer.min; + set_stop_pose(arc_length_to_stop_pose, path); + return; + } + + insert_stop_point_on_current_lanes(path); +} - const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { - return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); +void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +{ + const auto & path_front_pose = path.points.front().point.pose; + const auto & center_line = common_data_ptr_->current_lanes_path.points; + const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) { + return motion_utils::calcSignedArcLength( + center_line, path_front_pose.position, target.position); }; - // If lanelets.back() is in goal route section, get distance to goal. - // Otherwise, get distance to end of lane. - double distance_to_terminal = 0.0; - if (route_handler->isInGoalRouteSection(lanelets.back())) { - const auto goal = route_handler->getGoalPose(); - distance_to_terminal = getDistanceAlongLanelet(goal); - } else { - distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); - } + const auto & transient_data = common_data_ptr_->transient_data; + const auto & lanes_ptr = common_data_ptr_->lanes_ptr; + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = filterObjects(); - double stopping_distance = distance_to_terminal - lanes_dist_buffer.min - stop_point_buffer; + const auto dist_to_terminal = std::invoke([&]() -> double { + const auto target_pose = (lanes_ptr->current_lane_in_goal_section) + ? common_data_ptr_->route_handler_ptr->getGoalPose() + : center_line.back().point.pose; + return get_arc_length_along_lanelet(target_pose); + }); - const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); - if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { - const double distance_to_last_fit_width = - utils::lane_change::calculation::calc_dist_to_last_fit_width( - lanelets, path.points.front().point.pose, planner_data_->parameters); - stopping_distance = std::min(stopping_distance, distance_to_last_fit_width); - } + const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; + const auto min_dist_buffer = transient_data.current_dist_buffer.min; + const auto dist_to_terminal_start = + dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr); - const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start; + const auto distance_to_last_fit_width = std::invoke([&]() -> double { + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { + return utils::lane_change::calculation::calc_dist_to_last_fit_width( + lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + } + return std::numeric_limits::max(); + }); - if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); + const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + if (filtered_objects_.current_lane.empty()) { + set_stop_pose(dist_to_terminal_stop, path); return; } - // calculate minimum distance from path front to the stationary object on the ego lane. - const auto distance_to_ego_lane_obj = [&]() -> double { - double distance_to_obj = distance_to_terminal; - const double distance_to_ego = getDistanceAlongLanelet(getEgoPose()); - - for (const auto & object : target_objects.current_lane) { - // check if stationary - const auto obj_v = std::abs(object.initial_twist.linear.x); - if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { - continue; - } + const auto dist_to_target_lane_start = std::invoke([&]() -> double { + const auto & front_lane = lanes_ptr->target_neighbor.front(); + const auto target_front = + utils::to_geom_msg_pose(front_lane.centerline2d().front(), front_lane); + return get_arc_length_along_lanelet(target_front); + }); - // calculate distance from path front to the stationary object polygon on the ego lane. - const auto polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer(); - for (const auto & polygon_p : polygon) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); + const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( + common_data_ptr_, filtered_objects_, dist_to_target_lane_start, path); - // ignore if the point is around the ego path - if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { - continue; - } + // margin with leading vehicle + // consider rss distance when the LC need to avoid obstacles + const auto rss_dist = calcRssDistance( + 0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params_for_parked); - const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + const auto stop_margin = transient_data.lane_changing_length.min + + lc_param_ptr->backward_length_buffer_for_blocking_object + rss_dist + + bpp_param_ptr->base_link2front; + const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin; - // ignore backward object - if (current_distance_to_obj < distance_to_ego) { - continue; - } - distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); - } - } - return distance_to_obj; - }(); - - // Need to stop before blocking obstacle - if (distance_to_ego_lane_obj < distance_to_terminal) { - // consider rss distance when the LC need to avoid obstacles - const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); - - const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lanes_dist_buffer.min - - lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - - getCommonParam().base_link2front; - - // If the target lane in the lane change section is blocked by a stationary obstacle, there - // is no reason for stopping with a lane change margin. Instead, stop right behind the - // obstacle. - // ---------------------------------------------------------- - // [obj]> - // ---------------------------------------------------------- - // [ego]> | <--- lane change margin ---> [obj]> - // ---------------------------------------------------------- - const bool has_blocking_target_lane_obj = std::any_of( - target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), - [&](const auto & o) { - const auto v = std::abs(o.initial_twist.linear.x); - if (v > lane_change_parameters_->stopped_object_velocity_threshold) { - return false; - } + if (stop_arc_length_behind_obj < dist_to_target_lane_start) { + set_stop_pose(dist_to_target_lane_start, path); + return; + } - // target_objects includes objects out of target lanes, so filter them out - if (!boost::geometry::intersects( - autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(get_target_lanes()) - .polygon2d() - .basicPolygon())) { - return false; - } + if (stop_arc_length_behind_obj > dist_to_terminal_stop) { + set_stop_pose(dist_to_terminal_stop, path); + return; + } - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose); - return stopping_distance_for_obj < distance_to_target_lane_obj && - distance_to_target_lane_obj < distance_to_ego_lane_obj; - }); + // If the target lane in the lane change section is blocked by a stationary obstacle, there + // is no reason for stopping with a lane change margin. Instead, stop right behind the + // obstacle. + // ---------------------------------------------------------- + // [obj]> + // ---------------------------------------------------------- + // [ego]> | <--- stop margin ---> [obj]> + // ---------------------------------------------------------- + const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object( + common_data_ptr_, filtered_objects_, stop_arc_length_behind_obj, path); - if (!has_blocking_target_lane_obj) { - stopping_distance = stopping_distance_for_obj; - } + if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { + set_stop_pose(dist_to_terminal_stop, path); + return; } - if (stopping_distance > 0.0) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); - } + set_stop_pose(stop_arc_length_behind_obj, path); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -1109,6 +1086,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const }); } + // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior utils::path_safety_checker::filterObjects( filtered_by_lanes_objects.current_lane, [&](const PredictedObject & object) { const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); @@ -2182,9 +2160,10 @@ bool NormalLaneChange::is_valid_start_point( boost::geometry::covered_by(lc_start_point, target_lane_poly); } -void NormalLaneChange::setStopPose(const Pose & stop_pose) +void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) { - lane_change_stop_pose_ = stop_pose; + const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); + lane_change_stop_pose_ = stop_point.point.pose; } void NormalLaneChange::updateStopTime() 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 20bc982445b07..c4fe5fa9902c5 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 @@ -1250,4 +1250,69 @@ double get_distance_to_next_regulatory_element( return distance; } + +double get_min_dist_to_current_lanes_obj( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double dist_to_target_lane_start, const PathWithLaneId & path) +{ + const auto & path_points = path.points; + auto min_dist_to_obj = std::numeric_limits::max(); + for (const auto & object : filtered_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.linear.x); + if (obj_v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { + continue; + } + + // provide "estimation" based on size of object + const auto dist_to_obj = + motion_utils::calcSignedArcLength( + path_points, path_points.front().point.pose.position, object.initial_pose.position) - + (object.shape.dimensions.x / 2); + + if (dist_to_obj < dist_to_target_lane_start) { + 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); + } + } + return min_dist_to_obj; +} + +bool has_blocking_target_object( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double stop_arc_length, const PathWithLaneId & path) +{ + return std::any_of( + filtered_objects.target_lane_leading.begin(), filtered_objects.target_lane_leading.end(), + [&](const auto & object) { + const auto v = std::abs(object.initial_twist.linear.x); + if (v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { + return false; + } + + // filtered_objects includes objects out of target lanes, so filter them out + if (boost::geometry::disjoint( + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target.value())) { + return false; + } + + const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, object.initial_pose.position); + const auto width_margin = object.shape.dimensions.x / 2; + return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; + }); +} } // namespace autoware::behavior_path_planner::utils::lane_change