From a7e68a17ff0bf56d3acd9793d5a1d01e5843eea3 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 11 Oct 2024 17:26:24 +0900 Subject: [PATCH] fix(lane_change): change stopping logic (RT0-33761) (#1581) * RT0-33761 fix lane change stopping logic Signed-off-by: Zulfaqar Azmi * copied from awf main tested implementation Signed-off-by: Zulfaqar Azmi * doxygen comment Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../scene.hpp | 4 +- .../utils/utils.hpp | 49 ++++ .../src/scene.cpp | 214 ++++++++---------- .../src/utils/utils.cpp | 86 +++++++ 4 files changed, 238 insertions(+), 115 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 77efb658e28c3..04f21f5249240 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -60,6 +60,8 @@ class NormalLaneChange : public LaneChangeBase void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point_on_current_lanes(PathWithLaneId & path); + PathWithLaneId getReferencePath() const override; std::optional extendPath() override; @@ -178,7 +180,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_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index a84acf4decd8d..48b940093c2cd 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -289,5 +289,54 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol double calcPhaseLength( const double velocity, const double maximum_velocity, const double acceleration, const double time); + +/** + * @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 filtered_objects A collection of lane change target objects, including those in the + * current lane. + * @param bpp_param Parameters for the behavior path planner, such as vehicle dimensions. + * @param lc_param Parameters for the lane change process, including the velocity threshold for + * stopping. + * @param dist_to_target_lane_start The distance from the ego vehicle to the start of the target + * lane. + * @param ego_pose The current pose of 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 LaneChangeTargetObjects & filtered_objects, const BehaviorPathPlannerParameters & bpp_param, + const LaneChangeParameters & lc_param, const double dist_to_target_lane_start, const Pose & pose, + const PathWithLaneId & path); + +/** + * @brief Checks if there is any stationary object in the target lanes that would affect the lane + * change stop decision. + * + * This function determines whether there are any stationary objects in the target lanes that could + * impact the decision to insert a stop point for the ego vehicle. It checks each object's velocity, + * position relative to the ego vehicle, and overlap with the target lanes to identify if any object + * meets the criteria for being a blocking obstacle. + * + * @param target_lanes A collection of lanelets representing the target lanes for the lane change. + * @param filtered_objects A collection of lane change target objects, including those in the target + * lanes. + * @param lc_param Parameters for the lane change process, such as the stop velocity threshold. + * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. + * @param ego_pose The current pose of the ego vehicle. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return true if there is a stationary object in the target lanes that meets the criteria for + * being a blocking object; otherwise, false. + */ +bool has_blocking_target_object( + const lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects, + const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose, + const PathWithLaneId & path); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2fe72c8d964d8..284ea9409e924 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -165,8 +165,7 @@ 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(status_.target_lanes, output.path); } @@ -204,7 +203,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) void NormalLaneChange::insertStopPoint( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { - if (lanelets.empty()) { + if (lanelets.empty() || status_.current_lanes.empty()) { return; } @@ -214,133 +213,119 @@ void NormalLaneChange::insertStopPoint( return; } - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const double lane_change_buffer = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); + const auto & current_lanes = status_.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 shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); + const auto min_dist_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, shift_intervals, 0.0); + const auto backward_length_buffer = + lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const auto arc_length_to_stop_pose = + motion_utils::calcArcLength(path.points) - backward_length_buffer + min_dist_buffer; + set_stop_pose(arc_length_to_stop_pose, path); + return; + } - const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { - return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); - }; + insert_stop_point_on_current_lanes(path); +} - // 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); - } +void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +{ + const auto & path_front_pose = path.points.front().point.pose; + const auto & route_handler_ptr = getRouteHandler(); + const auto & current_lanes = status_.current_lanes; + const auto current_path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & center_line = current_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); + }; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); - double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; - - const auto is_valid_start_point = std::invoke([&]() -> bool { - auto lc_start_point = lanelet::utils::conversion::toLaneletPoint( - status_.lane_change_path.info.lane_changing_start.position); - const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon( - *route_handler, status_.current_lanes, type_); - return boost::geometry::covered_by( - lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); + const auto dist_to_terminal = std::invoke([&]() -> double { + const auto target_pose = route_handler_ptr->isInGoalRouteSection(current_lanes.back()) + ? route_handler_ptr->getGoalPose() + : center_line.back().point.pose; + return get_arc_length_along_lanelet(target_pose); }); - if (!is_valid_start_point) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); + const auto shift_intervals = + route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back()); + const auto min_dist_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); + const auto backward_length_buffer = + lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const auto dist_to_terminal_start = dist_to_terminal - min_dist_buffer - backward_length_buffer; + const auto filtered_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); + if (filtered_objects.current_lane.empty()) { + set_stop_pose(dist_to_terminal_start, 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.twist.linear.x); - if (obj_v > lane_change_parameters_->stop_velocity_threshold) { - continue; - } - - // calculate distance from path front to the stationary object polygon on the ego lane. - const auto polygon = - tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); - for (const auto & polygon_p : polygon) { - const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + const auto dist_to_target_lane_start = std::invoke([&]() -> double { + const auto & front_lane = status_.target_lanes.front(); + const auto & ll_front_pt = front_lane.centerline2d().front(); + const auto front_pt = lanelet::utils::conversion::toGeomMsgPt(ll_front_pt); + const auto yaw = lanelet::utils::getLaneletAngle(front_lane, front_pt); + Pose target_front; + target_front.position = front_pt; + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, yaw); + target_front.orientation = tf2::toMsg(tf_quat); - // ignore if the point is around the ego path - if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { - continue; - } + return get_arc_length_along_lanelet(target_front); + }); - const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( + filtered_objects, getCommonParam(), *lane_change_parameters_, dist_to_target_lane_start, + getEgoPose(), path); - // 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 double lane_change_buffer_for_blocking_object = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); - - const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - - 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.begin(), target_objects.target_lane.end(), [&](const auto & o) { - const auto v = std::abs(o.initial_twist.twist.linear.x); - if (v > lane_change_parameters_->stop_velocity_threshold) { - return false; - } + // margin with leading vehicle + // 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); - // target_objects includes objects out of target lanes, so filter them out - if (!boost::geometry::intersects( - tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(status_.target_lanes) - .polygon2d() - .basicPolygon())) { - return false; - } + const auto min_single_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, {shift_intervals.front()}, 0.0); + const auto stop_margin = min_single_lc_length + + lane_change_parameters_->backward_length_buffer_for_blocking_object + + rss_dist + getCommonParam().base_link2front; + const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin; - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); - return stopping_distance_for_obj < distance_to_target_lane_obj && - distance_to_target_lane_obj < distance_to_ego_lane_obj; - }); + if (stop_arc_length_behind_obj < dist_to_target_lane_start) { + set_stop_pose(dist_to_target_lane_start, path); + return; + } - if (!has_blocking_target_lane_obj) { - stopping_distance = stopping_distance_for_obj; - } + if (stop_arc_length_behind_obj > dist_to_terminal_start) { + set_stop_pose(dist_to_terminal_start, path); + return; } - if (stopping_distance > 0.0) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); + // 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( + status_.target_lanes, filtered_objects, *lane_change_parameters_, stop_arc_length_behind_obj, + getEgoPose(), path); + + if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { + set_stop_pose(dist_to_terminal_start, path); + return; } + + set_stop_pose(stop_arc_length_behind_obj, path); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -1842,9 +1827,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan return isVehicleStuck(current_lanes, detection_distance); } -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_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 60d7c699dc6bb..0d5b20db3c0c8 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1219,4 +1219,90 @@ double calcPhaseLength( const auto length_with_max_velocity = maximum_velocity * duration; return std::min(length_with_acceleration, length_with_max_velocity); } + +double get_min_dist_to_current_lanes_obj( + const LaneChangeTargetObjects & filtered_objects, const BehaviorPathPlannerParameters & bpp_param, + const LaneChangeParameters & lc_param, const double dist_to_target_lane_start, + const Pose & ego_pose, 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.twist.linear.x); + if (obj_v > lc_param.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.pose.position) - + (object.shape.dimensions.x / 2); + + const auto dist_to_ego = motion_utils::calcSignedArcLength( + path_points, ego_pose.position, object.initial_pose.pose.position) - + (object.shape.dimensions.x / 2); + + if (dist_to_obj < dist_to_target_lane_start || dist_to_obj < dist_to_ego) { + continue; + } + + // calculate distance from path front to the stationary object polygon on the ego lane. + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + for (const auto & polygon_p : obj_poly) { + const auto p_fp = tier4_autoware_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) > (bpp_param.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 lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects, + const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose, + const PathWithLaneId & path) +{ + const auto target_lane_poly = + lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon(); + return std::any_of( + filtered_objects.target_lane.begin(), filtered_objects.target_lane.end(), + [&](const auto & object) { + const auto v = std::abs(object.initial_twist.twist.linear.x); + if (v > lc_param.stop_velocity_threshold) { + return false; + } + + const auto arc_length_to_ego = + motion_utils::calcSignedArcLength( + path.points, ego_pose.position, object.initial_pose.pose.position) - + (object.shape.dimensions.x / 2); + + if (arc_length_to_ego < 0.0) { + return false; + } + + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + // filtered_objects includes objects out of target lanes, so filter them out + if (boost::geometry::disjoint(obj_poly, target_lane_poly)) { + return false; + } + + const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, object.initial_pose.pose.position); + const auto width_margin = object.shape.dimensions.x / 2; + return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; + }); +} } // namespace behavior_path_planner::utils::lane_change