From bc555c57a7305004a5b4c06ea450af351d6ad433 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 20 Sep 2024 17:29:39 +0900 Subject: [PATCH 01/29] feat(lane_change): add checks to ensure the edge of vehicle do not exceed target lane boundary when changing lanes (#8750) * check if LC candidate path footprint exceeds target lane far bound Signed-off-by: mohammad alqudah * add parameter to enable/disable check Signed-off-by: mohammad alqudah * check only lane changing section of cadidate path Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * small refactoring Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../config/lane_change.param.yaml | 1 + .../utils/data_structs.hpp | 1 + .../utils/utils.hpp | 14 +++++-- .../src/manager.cpp | 2 + .../src/scene.cpp | 9 +++++ .../src/utils/utils.cpp | 38 +++++++++++++++++++ 6 files changed, 61 insertions(+), 4 deletions(-) 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 9fe9dfd62eece..bdde481cb6d25 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 @@ -34,6 +34,7 @@ # safety check safety_check: allow_loose_check_for_cancel: true + enable_target_lane_bound_check: true collision_check_yaw_diff_threshold: 3.1416 execution: expected_front_deceleration: -1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 194412dfe01b7..4ba2e1aef893c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -154,6 +154,7 @@ struct Parameters // safety check bool allow_loose_check_for_cancel{true}; + bool enable_target_lane_bound_check{true}; double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_parked{}; 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 a19508dd11b7f..463b3baecf934 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 @@ -45,6 +45,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVeloc using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; @@ -99,6 +100,10 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +bool pathFootprintExceedsTargetLaneBound( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, + const double margin = 0.1); + std::optional constructCandidatePath( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, @@ -216,8 +221,9 @@ rclcpp::Logger getLogger(const std::string & type); * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ -Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info); +Polygon2d getEgoCurrentFootprint(const Pose & ego_pose, const VehicleInfo & ego_info); + +Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, bool left); /** * @brief Checks if the given polygon is within an intersection area. @@ -303,8 +309,8 @@ namespace autoware::behavior_path_planner::utils::lane_change::debug geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset); + const VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, + double additional_lat_offset); } // namespace autoware::behavior_path_planner::utils::lane_change::debug #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ 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 69df2fe14317a..dd7382d8f7116 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 @@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // safety check p.allow_loose_check_for_cancel = getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.enable_target_lane_bound_check = + getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); p.collision_check_yaw_diff_threshold = getOrDeclareParameter( *node, parameter("safety_check.collision_check_yaw_diff_threshold")); 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 28e95580a4bc7..3f55ef1c28e32 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 @@ -1675,6 +1675,15 @@ bool NormalLaneChange::getLaneChangePaths( return false; } + if ( + lane_change_parameters_->enable_target_lane_bound_check && + utils::lane_change::pathFootprintExceedsTargetLaneBound( + common_data_ptr_, candidate_path.value().shifted_path.path, + common_parameters.vehicle_info)) { + debug_print_lat("Reject: Path footprint exceeds target lane boundary. Skip lane change."); + return false; + } + const auto is_safe = std::invoke([&]() { constexpr size_t decel_sampling_num = 1; const auto safety_check_with_normal_rss = isLaneChangePathSafe( 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 a063919a1db5a..747bd0796dd27 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 @@ -241,6 +241,36 @@ bool isPathInLanelets( return true; } +bool pathFootprintExceedsTargetLaneBound( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, + const double margin) +{ + if (common_data_ptr->direction == Direction::NONE || path.points.empty()) { + return false; + } + + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const bool is_left = common_data_ptr->direction == Direction::LEFT; + + const auto combined_target_lane = lanelet::utils::combineLaneletsShape(target_lanes); + + for (const auto & path_point : path.points) { + const auto & pose = path_point.point.pose; + const auto front_vertex = getEgoFrontVertex(pose, ego_info, is_left); + + const auto sign = is_left ? -1.0 : 1.0; + const auto dist_to_boundary = + sign * utils::getSignedDistanceFromLaneBoundary(combined_target_lane, front_vertex, is_left); + + if (dist_to_boundary < margin) { + RCLCPP_DEBUG(get_logger(), "Path footprint exceeds target lane boundary"); + return true; + } + } + + return false; +} + std::optional constructCandidatePath( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, @@ -1048,6 +1078,14 @@ Polygon2d getEgoCurrentFootprint( return autoware::universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); } +Point getEgoFrontVertex( + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info, bool left) +{ + const double lon_offset = ego_info.wheel_base_m + ego_info.front_overhang_m; + const double lat_offset = 0.5 * (left ? ego_info.vehicle_width_m : -ego_info.vehicle_width_m); + return autoware::universe_utils::calcOffsetPose(ego_pose, lon_offset, lat_offset, 0.0).position; +} + bool isWithinIntersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) From 9beabb6e3a63bae8ea995b47d6431661ecaafbc2 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 16 Aug 2024 16:26:53 +0900 Subject: [PATCH 02/29] feat(lane_change): ensure LC merging lane stop point is safe (#8369) * function to check for merging lane Signed-off-by: mohammad alqudah * function to compute distance from last fit width center line point to lane end Signed-off-by: mohammad alqudah * ensure lane width at LC stop point is larger than ego width Signed-off-by: mohammad alqudah * refactor function isMergingLane Signed-off-by: mohammad alqudah * improve implementation Signed-off-by: mohammad alqudah * apply logic only when current ego foot print is within lane Signed-off-by: mohammad alqudah * change implementation to use intersection points of buffered centerline and lane polygon Signed-off-by: mohammad alqudah * minor refactoring Signed-off-by: mohammad alqudah * overload function isEgoWithinOriginalLane to pass lane polygon directly Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../utils/calculation.hpp | 17 +++++++ .../src/scene.cpp | 16 +++++-- .../src/utils/calculation.cpp | 47 ++++++++++++++++++ .../utils/utils.hpp | 10 +++- .../src/utils/utils.cpp | 48 +++++++++++-------- 5 files changed, 114 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index f395902dcc476..779c62ad3aa23 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -63,6 +63,23 @@ double calc_dist_from_pose_to_terminal_end( */ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); +/** + * @brief Calculates the distance to last fit width position along the lane + * + * This function computes the distance from the current ego position to the last + * position along the lane where the ego foot prints stays within the lane + * boundaries. + * + * @param lanelets current ego lanelets + * @param src_pose source pose to calculate distance from + * @param bpp_param common parameters used in behavior path planner. + * @param margin additional margin for checking lane width + * @return distance to last fit width position along the lane + */ +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); + /** * @brief Calculates the maximum preparation longitudinal distance for lane change. * 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 3f55ef1c28e32..ad93db3eee0b0 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 @@ -429,6 +429,14 @@ void NormalLaneChange::insertStopPoint( const auto target_objects = filterObjects(); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + 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 & lc_start_point = status_.lane_change_path.info.lane_changing_start; if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { @@ -769,8 +777,9 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); if (!utils::isEgoWithinOriginalLane( - get_current_lanes(), getEgoPose(), planner_data_->parameters, + curr_lanes_poly, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; @@ -791,7 +800,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( - get_current_lanes(), estimated_pose, planner_data_->parameters, + curr_lanes_poly, estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; return is_ego_within_original_lane; @@ -849,13 +858,14 @@ bool NormalLaneChange::isAbleToStopSafely() const const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - get_current_lanes(), estimated_pose, planner_data_->parameters); + curr_lanes_poly, estimated_pose, planner_data_->parameters); } } return true; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 1d656a5ee698f..6b699d43bf03e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -16,6 +16,8 @@ #include #include +#include + namespace autoware::behavior_path_planner::utils::lane_change::calculation { double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) @@ -55,6 +57,51 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) return std::max(min_back_dist, param_back_dist); } +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; + universe_utils::MultiPolygon2d center_line_polygon; + namespace strategy = boost::geometry::strategy::buffer; + boost::geometry::buffer( + line_string, center_line_polygon, strategy::distance_symmetric(buffer_distance), + strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), + strategy::point_square()); + + if (center_line_polygon.empty()) return 0.0; + + std::vector intersection_points; + boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + + if (intersection_points.empty()) { + return utils::getDistanceToEndOfLane(src_pose, lanelets); + } + + Pose pose; + double distance = std::numeric_limits::max(); + for (const auto & point : intersection_points) { + pose.position.x = boost::geometry::get<0>(point); + pose.position.y = boost::geometry::get<1>(point); + distance = std::min(distance, utils::getSignedDistance(src_pose, pose, lanelets)); + } + + return std::max(distance - (bpp_param.base_link2front + margin), 0.0); +} + double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { const auto max_prepare_duration = common_data_ptr->lc_param_ptr->lane_change_prepare_duration; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 5c1eee92c2a5d..03e6c2d7f8167 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -55,6 +55,8 @@ using geometry_msgs::msg::Vector3; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; +static constexpr double eps = 0.01; + struct PolygonPoint { geometry_msgs::msg::Point point; @@ -241,6 +243,10 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); + // path management // TODO(Horibe) There is a similar function in route_handler. Check. @@ -249,8 +255,10 @@ std::shared_ptr generateCenterLinePath( PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, const bool left_side); double getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, const double base_link2rear, const Pose & vehicle_pose, const bool left_side); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 55a6c6ff39d30..942418412c2a5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -559,19 +559,24 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin) { - const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); - const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); - const auto base_link2front = common_param.base_link2front; - const auto base_link2rear = common_param.base_link2rear; - const auto vehicle_width = common_param.vehicle_width; + const auto combined_lane = lanelet::utils::combineLaneletsShape(current_lanes); + const auto lane_polygon = combined_lane.polygon2d().basicPolygon(); + return isEgoWithinOriginalLane(lane_polygon, current_pose, common_param, outer_margin); +} + +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin) +{ const auto vehicle_poly = autoware::universe_utils::toFootprint( - current_pose, base_link2front, base_link2rear, vehicle_width); + current_pose, common_param.base_link2front, common_param.base_link2rear, + common_param.vehicle_width); // Check if the ego vehicle is entirely within the lane with a given outer margin. for (const auto & p : vehicle_poly.outer()) { // When the point is in the polygon, the distance is 0. When it is out of the polygon, return a // positive value. - const auto dist = boost::geometry::distance(p, lanelet::utils::to2D(lane_poly).basicPolygon()); + const auto dist = boost::geometry::distance(p, lane_polygon); if (dist > std::max(outer_margin, 0.0)) { return false; // out of polygon } @@ -817,25 +822,29 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) return path.points.at(*insert_idx); } +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, bool left_side) +{ + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(position); + const auto & boundary_line_2d = left_side ? lanelet.leftBound2d() : lanelet.rightBound2d(); + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + return arc_coordinates.distance; +} + double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose, bool left_side) { lanelet::ConstLanelet closest_lanelet; - lanelet::ArcCoordinates arc_coordinates; + if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); - const auto & boundary_line_2d = left_side - ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - arc_coordinates = lanelet::geometry::toArcCoordinates( - boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "closest shoulder lanelet not found."); + return getSignedDistanceFromLaneBoundary(closest_lanelet, pose.position, left_side); } - return arc_coordinates.distance; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), "closest lanelet not found."); + + return 0.0; } std::optional getSignedDistanceFromBoundary( @@ -1212,7 +1221,6 @@ PathWithLaneId setDecelerationVelocity( const auto stop_point_length = autoware::motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; - constexpr double eps{0.01}; if (std::abs(target_velocity) < eps && stop_point_length > 0.0) { const auto stop_point = utils::insertStopPoint(stop_point_length, reference_path); } From 7cef47d24459030f949ebe10aff9dcef49c823b2 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 25 Sep 2024 16:22:45 +0900 Subject: [PATCH 03/29] refactor(lane_change): refactor getLaneChangePaths function (#8909) * refactor lane change utility funcions Signed-off-by: mohammad alqudah * LC utility function to get distance to next regulatory element Signed-off-by: mohammad alqudah * don't activate LC module when close to regulatory element Signed-off-by: mohammad alqudah * modify threshold distance calculation Signed-off-by: mohammad alqudah * move regulatory element check to canTransitFailureState() function Signed-off-by: mohammad alqudah * always run LC module if approaching terminal point Signed-off-by: mohammad alqudah * use max possible LC length as threshold Signed-off-by: mohammad alqudah * update LC readme Signed-off-by: mohammad alqudah * refactor implementation Signed-off-by: mohammad alqudah * update readme Signed-off-by: mohammad alqudah * refactor checking data validity Signed-off-by: mohammad alqudah * refactor sampling of prepare phase metrics and lane changing phase metrics Signed-off-by: mohammad alqudah * add route handler function to get pose from 2d arc length Signed-off-by: mohammad alqudah * refactor candidate path generation Signed-off-by: mohammad alqudah * refactor candidate path safety check Signed-off-by: mohammad alqudah * fix variable name Signed-off-by: mohammad alqudah * Update planning/autoware_route_handler/src/route_handler.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * correct parameter name Signed-off-by: mohammad alqudah * set prepare segment velocity after taking max path velocity value Signed-off-by: mohammad alqudah * update LC README Signed-off-by: mohammad alqudah * minor changes Signed-off-by: mohammad alqudah * check phase length difference with previos valid candidate path Signed-off-by: mohammad alqudah * change logger name Signed-off-by: mohammad alqudah * change functions names to snake case Signed-off-by: mohammad alqudah * use snake case for function names Signed-off-by: mohammad alqudah * add colors to flow chart in README Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../autoware/route_handler/route_handler.hpp | 3 + .../src/route_handler.cpp | 27 + .../README.md | 102 ++++ .../scene.hpp | 19 +- .../utils/calculation.hpp | 33 ++ .../utils/data_structs.hpp | 40 ++ .../utils/utils.hpp | 32 +- .../src/scene.cpp | 523 +++++++----------- .../src/utils/calculation.cpp | 126 +++++ .../src/utils/utils.cpp | 50 +- 10 files changed, 559 insertions(+), 396 deletions(-) diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 3b80314bae5a8..e62f33e085038 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -324,6 +324,9 @@ class RouteHandler lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; + Pose get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const; + private: // MUST lanelet::routing::RoutingGraphPtr routing_graph_ptr_; diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index d534ecc4c84a8..9c60d22972df4 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -46,6 +46,8 @@ namespace autoware::route_handler { namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; @@ -2225,4 +2227,29 @@ std::optional RouteHandler::findDrivableLanePath( if (route) return route->shortestPath(); return {}; } + +Pose RouteHandler::get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const +{ + double accumulated_distance2d = 0; + for (const auto & llt : lanelet_sequence) { + const auto & centerline = llt.centerline(); + for (auto it = centerline.begin(); std::next(it) != centerline.end(); ++it) { + const auto pt = *it; + const auto next_pt = *std::next(it); + const double distance2d = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt)); + if (accumulated_distance2d + distance2d > s) { + const double ratio = (s - accumulated_distance2d) / distance2d; + const auto interpolated_pt = pt.basicPoint() * (1 - ratio) + next_pt.basicPoint() * ratio; + const auto yaw = std::atan2(next_pt.y() - pt.y(), next_pt.x() - pt.x()); + Pose pose; + pose.position = createPoint(interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + pose.orientation = createQuaternionFromYaw(yaw); + return pose; + } + accumulated_distance2d += distance2d; + } + } + return Pose{}; +} } // namespace autoware::route_handler 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 5f1c3debabcf8..18a797976161c 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 @@ -24,6 +24,108 @@ The lane change candidate path is divided into two phases: preparation and lane- ![lane-change-phases](./images/lane_change-lane_change_phases.png) +The following chart illustrates the process of sampling candidate paths for lane change. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +if (Is current lanes, target lanes OR target neighbor lanes polygon empty?) then (yes) + #LightPink:Return prev approved path; + stop +else (no) +endif + +:Get target objects; + +:Calculate prepare phase metrics; + +:Start loop through prepare phase metrics; + +repeat + :Get prepare segment; + + if (Is LC start point outside target lanes range) then (yes) + if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop + else (no) + #LightPink:Return prev approved path; + stop + endif + else (no) + endif + + :Calculate shift phase metrics; + + :Start loop through shift phase metrics; + repeat + :Get candidate path; + note left: Details shown in the next chart + if (Is valid candidate path?) then (yes) + :Store candidate path; + if (Is candidate path safe?) then (yes) + #LightGreen:Return candidate path; + stop + else (no) + endif + else (no) + endif + repeat while (Finished looping shift phase metrics) is (FALSE) + repeat while (Is finished looping prepare phase metrics) is (FALSE) + +if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop +else (no) +endif + +#LightPink:Return prev approved path; +stop + +@enduml +``` + +While the following chart demonstrates the process of generating a valid candidate path. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +:Calculate resample interval; + +:Get reference path from target lanes; + +if (Is reference path empty?) then (yes) + #LightPink:Return; + stop +else (no) +endif + +:Get LC shift line; + +:Set lane change Info; +note left: set information from sampled prepare phase and shift phase metrics\n(duration, length, velocity, and acceleration) + +:Construct candidate path; + +if (Candidate path has enough length?) then (yes) + #LightGreen:Return valid candidate path; + stop +else (no) + #LightPink:Return; + stop +endif + +@enduml +``` + ### Preparation phase The preparation trajectory is the candidate path's first and the straight portion generated along the ego vehicle's current lane. The length of the preparation trajectory is computed as follows. 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 1bf8407d64ffb..61c0f5ad82ab5 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 @@ -153,18 +153,17 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; - bool hasEnoughLengthToCrosswalk( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; - bool hasEnoughLengthToIntersection( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + LaneChangePath get_candidate_path( + const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, + const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, + const Pose & lc_start_pose, const double target_lane_length, const double shift_length, + const double next_lc_buffer, const bool is_goal_in_route) const; - bool hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; - - bool getLaneChangePaths( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const; + bool check_candidate_path_safety( + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, + const double lane_change_buffer, const bool is_stuck) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 779c62ad3aa23..db8de51ba230c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -24,6 +24,7 @@ using autoware::route_handler::Direction; using autoware::route_handler::RouteHandler; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LCParamPtr; +using behavior_path_planner::lane_change::PhaseMetrics; /** * @brief Calculates the distance from the ego vehicle to the terminal point. @@ -132,6 +133,38 @@ double calc_maximum_lane_change_length( double calc_maximum_lane_change_length( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calc_phase_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration); + +double calc_lane_changing_acceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc); + +std::vector calc_prepare_phase_metrics( + const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, + const std::vector & lon_accel_values, const double current_velocity, + const double min_length_threshold = 0.0, + const double max_length_threshold = std::numeric_limits::max()); + +std::vector calc_shift_phase_metrics( + const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, + const double max_velocity, const double lon_accel, + const double max_length_threshold = std::numeric_limits::max()); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 4ba2e1aef893c..337c959f158f3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -193,6 +193,28 @@ struct PhaseInfo } }; +struct PhaseMetrics +{ + double duration{0.0}; + double length{0.0}; + double velocity{0.0}; + double sampled_lon_accel{0.0}; + double actual_lon_accel{0.0}; + double lat_accel{0.0}; + + PhaseMetrics( + const double _duration, const double _length, const double _velocity, + const double _sampled_lon_accel, const double _actual_lon_accel, const double _lat_accel) + : duration(_duration), + length(_length), + velocity(_velocity), + sampled_lon_accel(_sampled_lon_accel), + actual_lon_accel(_actual_lon_accel), + lat_accel(_lat_accel) + { + } +}; + struct Lanes { bool current_lane_in_goal_section{false}; @@ -216,6 +238,23 @@ struct Info double lateral_acceleration{0.0}; double terminal_lane_changing_velocity{0.0}; + + Info() = default; + Info( + const PhaseMetrics & _prep_metrics, const PhaseMetrics & _lc_metrics, + const Pose & _lc_start_pose, const Pose & _lc_end_pose, const ShiftLine & _shift_line) + { + longitudinal_acceleration = + PhaseInfo{_prep_metrics.actual_lon_accel, _lc_metrics.actual_lon_accel}; + duration = PhaseInfo{_prep_metrics.duration, _lc_metrics.duration}; + velocity = PhaseInfo{_prep_metrics.velocity, _prep_metrics.velocity}; + length = PhaseInfo{_prep_metrics.length, _lc_metrics.length}; + lane_changing_start = _lc_start_pose; + lane_changing_end = _lc_end_pose; + lateral_acceleration = _lc_metrics.lat_accel; + terminal_lane_changing_velocity = _lc_metrics.velocity; + shift_line = _shift_line; + } }; template @@ -317,6 +356,7 @@ using LaneChangeModuleType = lane_change::ModuleType; using LaneChangeParameters = lane_change::Parameters; using LaneChangeStates = lane_change::States; using LaneChangePhaseInfo = lane_change::PhaseInfo; +using LaneChangePhaseMetrics = lane_change::PhaseMetrics; using LaneChangeInfo = lane_change::Info; using FilteredByLanesObjects = lane_change::LanesObjects>; using FilteredByLanesExtendedObjects = lane_change::LanesObjects; 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 463b3baecf934..4da1fadb1bb87 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 @@ -69,10 +69,6 @@ double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); -double calcLaneChangingAcceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); @@ -104,17 +100,12 @@ bool pathFootprintExceedsTargetLaneBound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); -std::optional constructCandidatePath( +std::optional construct_candidate_path( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & target_lane_reference_path, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); -ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & reference_path, const double shift_length); - -ShiftLine getLaneChangingShiftLine( +ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); @@ -259,23 +250,6 @@ bool isWithinIntersection( */ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); -/** - * @brief Calculates the distance required during a lane change operation. - * - * Used for computing prepare or lane change length based on current and maximum velocity, - * acceleration, and duration, returning the lesser of accelerated distance or distance at max - * velocity. - * - * @param velocity The current velocity of the vehicle in meters per second (m/s). - * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). - * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). - * @param duration The duration of the lane change in seconds (s). - * @return The calculated minimum distance in meters (m). - */ -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double time); - LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); bool is_same_lane_with_prev_iteration( 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 ad93db3eee0b0..49ad0e74d9bc9 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 @@ -128,9 +128,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) } LaneChangePaths valid_paths{}; - const bool is_stuck = isVehicleStuck(current_lanes); - bool found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, is_stuck, &valid_paths); + bool found_safe_path = get_lane_change_paths(valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin lane_change_debug_.valid_paths = valid_paths; @@ -1350,76 +1348,31 @@ bool NormalLaneChange::hasEnoughLength( return true; } -bool NormalLaneChange::hasEnoughLengthToCrosswalk( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - - const double dist_to_crosswalk_from_lane_change_start_pose = - utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - - path.info.length.prepare; - // Check lane changing section includes crosswalk - if ( - dist_to_crosswalk_from_lane_change_start_pose > 0.0 && - dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { - return false; - } + lane_change_debug_.collision_check_objects.clear(); - return true; -} + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + const auto is_stuck = isVehicleStuck(current_lanes); -bool NormalLaneChange::hasEnoughLengthToIntersection( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const -{ - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + const auto is_empty = [&](const auto & data, const auto & s) { + if (!data.empty()) return false; + RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); + return true; + }; - const double dist_to_intersection_from_lane_change_start_pose = - utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; - // Check lane changing section includes intersection + const auto target_lane_neighbors_polygon_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if ( - dist_to_intersection_from_lane_change_start_pose > 0.0 && - dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { + is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || + is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { return false; } - return true; -} - -bool NormalLaneChange::hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const -{ - const auto current_pose = getEgoPose(); - const auto dist_to_next_traffic_light = - getDistanceToNextTrafficLight(current_pose, current_lanes); - const auto dist_to_next_traffic_light_from_lc_start_pose = - dist_to_next_traffic_light - path.info.length.prepare; - - return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 || - dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; -} - -bool NormalLaneChange::getLaneChangePaths( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const -{ - lane_change_debug_.collision_check_objects.clear(); - if (current_lanes.empty() || target_lanes.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return false; - } const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto backward_path_length = common_parameters.backward_path_length; - const auto forward_path_length = common_parameters.forward_path_length; - const auto minimum_lane_changing_velocity = - lane_change_parameters_->minimum_lane_changing_velocity; - const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; - // get velocity const auto current_velocity = getEgoVelocity(); @@ -1436,27 +1389,23 @@ bool NormalLaneChange::getLaneChangePaths( *lane_change_parameters_, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); - const auto dist_to_end_of_current_lanes = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto dist_to_terminal_end = calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto dist_to_target_start = + calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); + const auto dist_to_terminal_start = dist_to_terminal_end - lane_change_buffer; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto & target_neighbor_preferred_lane_poly_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if (target_neighbor_preferred_lane_poly_2d.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return false; - } - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - candidate_paths->reserve( - longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + candidate_paths.reserve( + longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * + prepare_durations.size()); RCLCPP_DEBUG( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", @@ -1467,270 +1416,230 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - for (const auto & prepare_duration : prepare_durations) { - for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { - // get path on original lanes - const auto prepare_velocity = std::clamp( - current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity, getCommonParam().max_vel); + const auto prepare_phase_metrics = calculation::calc_prepare_phase_metrics( + common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, + dist_to_target_start, dist_to_terminal_start); - // compute actual longitudinal acceleration - const double longitudinal_acc_on_prepare = - (prepare_duration < 1e-3) ? 0.0 - : ((prepare_velocity - current_velocity) / prepare_duration); + auto check_length_diff = + [&](const double prep_length, const double lc_length, const bool check_lc) { + if (candidate_paths.empty()) return true; - const auto prepare_length = utils::lane_change::calcPhaseLength( - current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); + const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length); + if (prep_diff > lane_change_parameters_->skip_process_lon_diff_th_prepare) return true; - const auto ego_dist_to_terminal_start = dist_to_end_of_current_lanes - lane_change_buffer; - if (prepare_length > ego_dist_to_terminal_start) { - RCLCPP_DEBUG( - logger_, - "Reject: Prepare length exceed distance to terminal start. prep_len: %.5f, ego dist to " - "terminal start: %.5f", - prepare_length, ego_dist_to_terminal_start); - continue; - } + if (!check_lc) return false; - if (!candidate_paths->empty()) { - const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; - if (std::abs(prev_prep_diff) < lane_change_parameters_->skip_process_lon_diff_th_prepare) { - RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); - continue; - } + const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); + if (lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing) return true; + + return false; + }; + + for (const auto & prep_metric : prepare_phase_metrics) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG( + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s, prep_metric.duration, + prep_metric.actual_lon_accel, prep_metric.length); + }; + + if (!check_length_diff(prep_metric.length, 0.0, false)) { + RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); + continue; + } + + auto prepare_segment = + getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); + + if (prepare_segment.points.empty()) { + debug_print("prepare segment is empty...? Unexpected."); + continue; + } + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { + debug_print( + "Reject: lane changing start point is not within the preferred lanes or its neighbors"); + continue; + } + + // lane changing start is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + debug_print("lane change start is behind target lanelet!"); + break; + } + + debug_print("Prepare path satisfy constraints"); + + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + dist_to_terminal_start > max_prepare_length + ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length + : dist_to_terminal_end - prep_metric.length; + auto target_lane_buffer = + lane_change_parameters_->lane_change_finish_judge_buffer + next_lane_change_buffer; + if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { + target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; } - auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_length; + }); + + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + prep_metric.sampled_lon_accel, max_lane_changing_length); - const auto debug_print = [&](const auto & s) { + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); + + for (const auto & lc_metric : lane_changing_metrics) { + const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | prep_len: %.5f", s, - prepare_duration, sampled_longitudinal_acc, longitudinal_acc_on_prepare, prepare_length); + logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), + lc_metric.duration, lc_metric.actual_lon_accel, lc_metric.lat_accel, lc_metric.length); }; - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); + if (!check_length_diff(prep_metric.length, lc_metric.length, true)) { + RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); continue; } - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); + LaneChangePath candidate_path; + try { + candidate_path = get_candidate_path( + prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, + target_lane_length, shift_length, next_lane_change_buffer, is_goal_in_route); + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); continue; } - // lane changing start getEgoPose() is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); + candidate_paths.push_back(candidate_path); - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - debug_print("lane change start getEgoPose() is behind target lanelet!"); - break; + bool is_safe = false; + try { + is_safe = + check_candidate_path_safety(candidate_path, target_objects, lane_change_buffer, is_stuck); + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); + return false; } - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - - const auto initial_lane_changing_velocity = prepare_velocity; - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - - // get lateral acceleration range - const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->lane_change_lat_acc_map.find(initial_lane_changing_velocity); - const auto lateral_acc_resolution = - std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - - std::vector sample_lat_acc; - constexpr double eps = 0.01; - for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) { - sample_lat_acc.push_back(a); + if (is_safe) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; } - RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); - - debug_print("Prepare path satisfy constraints"); - const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); - - for (const auto & lateral_acc : sample_lat_acc) { - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc); - const double longitudinal_acc_on_lane_changing = - utils::lane_change::calcLaneChangingAcceleration( - initial_lane_changing_velocity, max_path_velocity, lane_changing_time, - sampled_longitudinal_acc); - const auto lane_changing_length = utils::lane_change::calcPhaseLength( - initial_lane_changing_velocity, getCommonParam().max_vel, - longitudinal_acc_on_lane_changing, lane_changing_time); - - const auto debug_print_lat = [&](const auto & s) { - RCLCPP_DEBUG( - logger_, - " - %s | lc_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | lc_len: %.5f", s, - lane_changing_time, sampled_longitudinal_acc, longitudinal_acc_on_lane_changing, - lane_changing_length); - }; - if (!candidate_paths->empty()) { - const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; - const auto lc_length_diff = - candidate_paths->back().info.length.lane_changing - lane_changing_length; - - // We only check lc_length_diff if and only if the current prepare_length is equal to the - // previous prepare_length. - if ( - std::abs(prev_prep_diff) < eps && - std::abs(lc_length_diff) < - lane_change_parameters_->skip_process_lon_diff_th_lane_changing) { - RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); - continue; - } - } - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } - if ( - ego_dist_to_terminal_start > max_prepare_length && - lane_changing_length + prepare_length > dist_to_next_regulatory_element) { - debug_print_lat( - "Reject: length of lane changing path is longer than length to regulatory element!!"); - continue; - } + debug_print_lat("Reject: sampled path is not safe."); + } + } - // if multiple lane change is necessary, does the remaining distance is sufficient - const auto remaining_dist_in_target = std::invoke([&]() { - const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; - const auto num_to_preferred_lane_from_target_lane = - std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const auto backward_len_buffer = - lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto backward_buffer_to_target_lane = - num_to_preferred_lane_from_target_lane == 0 ? 0.0 : backward_len_buffer; - return lane_changing_length + finish_judge_buffer + backward_buffer_to_target_lane + - next_lane_change_buffer; - }); - - if (remaining_dist_in_target > dist_lc_start_to_end_of_lanes) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } + RCLCPP_DEBUG(logger_, "No safety path found."); + return false; +} - const auto terminal_lane_changing_velocity = std::min( - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, - getCommonParam().max_vel); - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); +LaneChangePath NormalLaneChange::get_candidate_path( + const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, + const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, + const Pose & lc_start_pose, const double target_lane_length, const double shift_length, + const double next_lc_buffer, const bool is_goal_in_route) const +{ + const auto & route_handler = *getRouteHandler(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto & forward_path_length = planner_data_->parameters.forward_path_length; - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, - initial_lane_changing_velocity, next_lane_change_buffer); + const auto resample_interval = + utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lc_start_pose, target_lane_length, lc_metrics.length, + forward_path_length, resample_interval, is_goal_in_route, next_lc_buffer); - if (target_segment.points.empty()) { - debug_print_lat("Reject: target segment is empty!! something wrong..."); - continue; - } + if (target_lane_reference_path.points.empty()) { + throw std::logic_error("target_lane_reference_path is empty!"); + } - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, initial_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose, target_lane_length, - lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); - - if (target_lane_reference_path.points.empty()) { - debug_print_lat("Reject: target_lane_reference_path is empty!!"); - continue; - } + const auto lc_end_pose = std::invoke([&]() { + const auto dist_to_lc_start = + lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; + const auto dist_to_lc_end = dist_to_lc_start + lc_metrics.length; + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); + }); - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); + const auto shift_line = utils::lane_change::get_lane_changing_shift_line( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); - const auto candidate_path = utils::lane_change::constructCandidatePath( - common_data_ptr_, lane_change_info, prepare_segment, target_segment, - target_lane_reference_path, sorted_lane_ids); + LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - if (!candidate_path) { - debug_print_lat("Reject: failed to generate candidate path!!"); - continue; - } + const auto candidate_path = utils::lane_change::construct_candidate_path( + common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print_lat("Reject: invalid candidate path!!"); - continue; - } + if (!candidate_path) { + throw std::logic_error("failed to generate candidate path!"); + } - candidate_paths->push_back(*candidate_path); + if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + throw std::logic_error("invalid candidate path length!"); + } - if ( - !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, *candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { - debug_print_lat( - "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " - "lane change."); - return false; - } + return *candidate_path; +} - if ( - lane_change_parameters_->enable_target_lane_bound_check && - utils::lane_change::pathFootprintExceedsTargetLaneBound( - common_data_ptr_, candidate_path.value().shifted_path.path, - common_parameters.vehicle_info)) { - debug_print_lat("Reject: Path footprint exceeds target lane boundary. Skip lane change."); - return false; - } +bool NormalLaneChange::check_candidate_path_safety( + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, + const double lane_change_buffer, const bool is_stuck) const +{ + if ( + !is_stuck && !utils::lane_change::passed_parked_objects( + common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, + lane_change_buffer, lane_change_debug_.collision_check_objects)) { + throw std::logic_error( + "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); + } - const auto is_safe = std::invoke([&]() { - constexpr size_t decel_sampling_num = 1; - const auto safety_check_with_normal_rss = isLaneChangePathSafe( - *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, - decel_sampling_num, lane_change_debug_.collision_check_objects); - - if (!safety_check_with_normal_rss.is_safe && is_stuck) { - const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, - decel_sampling_num, lane_change_debug_.collision_check_objects); - return safety_check_with_stuck_rss.is_safe; - } - - return safety_check_with_normal_rss.is_safe; - }); - - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } + constexpr size_t decel_sampling_num = 1; + const auto safety_check_with_normal_rss = isLaneChangePathSafe( + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num, + lane_change_debug_.collision_check_objects); - debug_print_lat("Reject: sampled path is not safe."); - } - } + if (!safety_check_with_normal_rss.is_safe && is_stuck) { + const auto safety_check_with_stuck_rss = isLaneChangePathSafe( + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, + decel_sampling_num, lane_change_debug_.collision_check_objects); + return safety_check_with_stuck_rss.is_safe; } - RCLCPP_DEBUG(logger_, "No safety path found."); - return false; + return safety_check_with_normal_rss.is_safe; } std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { - if (current_lanes.empty() || target_lanes.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + const auto is_empty = [&](const auto & data, const auto & s) { + if (!data.empty()) return false; + RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); + return true; + }; + + const auto target_lane_neighbors_polygon_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; + if ( + is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || + is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { return {}; } + const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; @@ -1752,13 +1661,6 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto target_neighbor_preferred_lane_poly_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if (target_neighbor_preferred_lane_poly_2d.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return {}; - } - // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); @@ -1805,15 +1707,6 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( return {}; } - const lanelet::BasicPoint2d lc_start_point( - lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); - - const auto & target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); - - const auto is_valid_start_point = - boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || - boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); - LaneChangeInfo lane_change_info; lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; @@ -1825,7 +1718,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.lateral_acceleration = max_lateral_acc; lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; - if (!is_valid_start_point) { + if (!is_valid_start_point(common_data_ptr_, lane_changing_start_pose.value())) { RCLCPP_DEBUG( logger_, "Reject: lane changing points are not inside of the target preferred lanes or its " @@ -1845,7 +1738,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( return {}; } - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_change_info.shift_line = utils::lane_change::get_lane_changing_shift_line( lane_changing_start_pose.value(), target_segment.points.front().point.pose, target_lane_reference_path, shift_length); @@ -1858,9 +1751,9 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points.pop_back(); reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; - const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( - common_data_ptr_, lane_change_info, reference_segment, target_segment, - target_lane_reference_path, sorted_lane_ids); + const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( + common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, + sorted_lane_ids); return terminal_lane_change_path; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 6b699d43bf03e..4473ab95dc18d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -20,6 +20,14 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation { + +rclcpp::Logger get_logger() +{ + constexpr const char * name{"lane_change.utils"}; + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; +} + double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) { const auto & lanes_ptr = common_data_ptr->lanes_ptr; @@ -231,4 +239,122 @@ double calc_maximum_lane_change_length( return calc_maximum_lane_change_length( vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc); } + +double calc_phase_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} + +double calc_lane_changing_acceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc) +{ + if (prepare_longitudinal_acc <= 0.0) { + return 0.0; + } + + return std::clamp( + (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, + prepare_longitudinal_acc); +} + +std::vector calc_prepare_phase_metrics( + const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, + const std::vector & lon_accel_values, const double current_velocity, + const double min_length_threshold, const double max_length_threshold) +{ + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; + + std::vector metrics; + + auto is_skip = [&](const double prepare_length) { + if (prepare_length > max_length_threshold || prepare_length < min_length_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Skip: prepare length out of expected range. length: %.5f, threshold min: %.5f, max: %.5f", + prepare_length, min_length_threshold, max_length_threshold); + return true; + } + return false; + }; + + metrics.reserve(prepare_durations.size() * lon_accel_values.size()); + for (const auto & prepare_duration : prepare_durations) { + for (const auto & lon_accel : lon_accel_values) { + const auto prepare_velocity = + std::clamp(current_velocity + lon_accel * prepare_duration, min_lc_vel, max_vel); + + // compute actual longitudinal acceleration + const double prepare_accel = (prepare_duration < 1e-3) + ? 0.0 + : ((prepare_velocity - current_velocity) / prepare_duration); + + const auto prepare_length = + calc_phase_length(current_velocity, max_vel, prepare_accel, prepare_duration); + + if (is_skip(prepare_length)) continue; + + metrics.emplace_back( + prepare_duration, prepare_length, prepare_velocity, lon_accel, prepare_accel, 0.0); + } + } + + return metrics; +} + +std::vector calc_shift_phase_metrics( + const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, + const double max_path_velocity, const double lon_accel, const double max_length_threshold) +{ + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; + + // get lateral acceleration range + const auto [min_lateral_acc, max_lateral_acc] = + common_data_ptr->lc_param_ptr->lane_change_lat_acc_map.find(initial_velocity); + const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / + common_data_ptr->lc_param_ptr->lateral_acc_sampling_num; + + std::vector metrics; + + auto is_skip = [&](const double lane_changing_length) { + if (lane_changing_length > max_length_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Skip: lane changing length exceeds maximum threshold. length: %.5f, threshold: %.5f", + lane_changing_length, max_length_threshold); + return true; + } + return false; + }; + + for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; + lat_acc += lateral_acc_resolution) { + const auto lane_changing_duration = PathShifter::calcShiftTimeFromJerk( + shift_length, common_data_ptr->lc_param_ptr->lane_changing_lateral_jerk, lat_acc); + + const double lane_changing_accel = calc_lane_changing_acceleration( + initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); + + const auto lane_changing_length = calculation::calc_phase_length( + initial_velocity, max_vel, lane_changing_accel, lane_changing_duration); + + if (is_skip(lane_changing_length)) continue; + + const auto lane_changing_velocity = std::clamp( + initial_velocity + lane_changing_accel * lane_changing_duration, min_lc_vel, max_vel); + + metrics.emplace_back( + lane_changing_duration, lane_changing_length, lane_changing_velocity, lon_accel, + lane_changing_accel, lat_acc); + } + + return metrics; +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation 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 747bd0796dd27..5e1f17a4612fa 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 @@ -74,7 +74,8 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { constexpr const char * name{"lane_change.utils"}; - return rclcpp::get_logger(name); + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; } double calcLaneChangeResampleInterval( @@ -105,19 +106,6 @@ double calcMaximumAcceleration( return std::clamp(acc, 0.0, max_longitudinal_acc); } -double calcLaneChangingAcceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc) -{ - if (prepare_longitudinal_acc <= 0.0) { - return 0.0; - } - - return std::clamp( - (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, - prepare_longitudinal_acc); -} - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { @@ -144,7 +132,7 @@ std::vector getAccelerationValues( } if (max_acc - min_acc < std::numeric_limits::epsilon()) { - return {0.0}; + return {min_acc}; } constexpr double epsilon = 0.001; @@ -271,10 +259,9 @@ bool pathFootprintExceedsTargetLaneBound( return false; } -std::optional constructCandidatePath( +std::optional construct_candidate_path( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & target_lane_reference_path, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids) { const auto & shift_line = lane_change_info.shift_line; @@ -326,8 +313,8 @@ std::optional constructCandidatePath( continue; } const auto nearest_idx = - autoware::motion_utils::findNearestIndex(target_segment.points, point.point.pose); - point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; + autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); + point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; } // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster @@ -397,18 +384,7 @@ PathWithLaneId getReferencePathFromTargetLane( lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); } -ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & reference_path, const double shift_length) -{ - const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; - - return getLaneChangingShiftLine( - lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); -} - -ShiftLine getLaneChangingShiftLine( +ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length) { @@ -1113,16 +1089,6 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); } -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double duration) -{ - const auto length_with_acceleration = - velocity * duration + 0.5 * acceleration * std::pow(duration, 2); - const auto length_with_max_velocity = maximum_velocity * duration; - return std::min(length_with_acceleration, length_with_max_velocity); -} - LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) { const auto & lanes = common_data_ptr->lanes_ptr; From b62a71f28870c4b85745803f3164329696aa862f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 27 Sep 2024 19:05:55 +0900 Subject: [PATCH 04/29] refactor(lane_change): add TransientData to store commonly used lane change-related variables. (#8954) * add transient data Signed-off-by: Zulfaqar Azmi * reverted max lc dist in calcCurrentMinMax Signed-off-by: Zulfaqar Azmi * rename Signed-off-by: Zulfaqar Azmi * minor refactoring Signed-off-by: Zulfaqar Azmi * update doxygen comments Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 15 +- .../src/scene.hpp | 2 +- .../scene.hpp | 2 + .../utils/base_class.hpp | 2 + .../utils/calculation.hpp | 63 ++--- .../utils/data_structs.hpp | 46 +++- .../utils/utils.hpp | 3 + .../src/interface.cpp | 1 + .../src/scene.cpp | 238 +++++++++--------- .../src/utils/calculation.cpp | 180 +++++++++---- .../src/utils/utils.cpp | 24 +- 11 files changed, 357 insertions(+), 219 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 86810789c20ae..e854a3919c925 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -83,7 +83,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto & nearest_object = data.target_objects.front(); const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); - const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); + const auto minimum_lane_change_length = calc_minimum_dist_buffer(); lane_change_debug_.execution_area = createExecutionArea( getCommonParam().vehicle_info, getEgoPose(), @@ -274,16 +274,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ return avoidance_helper_->getMinAvoidanceDistance(shift_length); } -double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const +double AvoidanceByLaneChange::calc_minimum_dist_buffer() const { - const auto current_lanes = get_current_lanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "no empty lanes"); - return std::numeric_limits::infinity(); - } - - return utils::lane_change::calculation::calc_minimum_lane_change_length( - getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); + const auto [_, dist_buffer] = utils::lane_change::calculation::calc_lc_length_and_dist_buffer( + common_data_ptr_, get_current_lanes()); + return dist_buffer.min; } double AvoidanceByLaneChange::calcLateralOffset() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp index d6bf6bc29df97..42ae41fa380cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp @@ -63,7 +63,7 @@ class AvoidanceByLaneChange : public NormalLaneChange void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; double calcMinAvoidanceLength(const ObjectData & nearest_object) const; - double calcMinimumLaneChangeLength() const; + double calc_minimum_dist_buffer() const; double calcLateralOffset() const; }; } // namespace autoware::behavior_path_planner 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 61c0f5ad82ab5..f976e1fa59f4f 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 @@ -54,6 +54,8 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; + void update_transient_data() final; + void update_filtered_objects() final; void updateLaneChangeStatus() override; 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 b237368692312..72a40caca1d6a 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 @@ -67,6 +67,8 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; + virtual void update_transient_data() = 0; + virtual void update_filtered_objects() = 0; virtual void updateLaneChangeStatus() = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index db8de51ba230c..ed6c48d4a332e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -24,25 +24,9 @@ using autoware::route_handler::Direction; using autoware::route_handler::RouteHandler; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LCParamPtr; +using behavior_path_planner::lane_change::MinMaxValue; using behavior_path_planner::lane_change::PhaseMetrics; -/** - * @brief Calculates the distance from the ego vehicle to the terminal point. - * - * This function computes the distance from the current position of the ego vehicle - * to either the goal pose or the end of the current lane, depending on whether - * the vehicle's current lane is within the goal section. - * - * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - Non null `lanes_ptr` that points to the current lanes data. - * - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle. - * - Non null `route_handler_ptr` that contains the goal pose of the route. - * - * @return The distance to the terminal point (either the goal pose or the end of the current lane) - * in meters. - */ -double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr); - double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, const Pose & src_pose); @@ -119,13 +103,6 @@ double calc_ego_dist_to_lanes_start( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -double calc_minimum_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); - -double calc_minimum_lane_change_length( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction); - double calc_maximum_lane_change_length( const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc); @@ -134,6 +111,10 @@ double calc_maximum_lane_change_length( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc); +double calc_lane_changing_acceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc); + /** * @brief Calculates the distance required during a lane change operation. * @@ -151,10 +132,6 @@ double calc_phase_length( const double velocity, const double maximum_velocity, const double acceleration, const double duration); -double calc_lane_changing_acceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); - std::vector calc_prepare_phase_metrics( const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, const std::vector & lon_accel_values, const double current_velocity, @@ -165,6 +142,36 @@ std::vector calc_shift_phase_metrics( const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, const double max_velocity, const double lon_accel, const double max_length_threshold = std::numeric_limits::max()); + +/** + * @brief Calculates the minimum and maximum lane changing lengths, along with the corresponding + * distance buffers. + * + * This function computes the minimum and maximum lane change lengths based on shift intervals and + * vehicle parameters. It only accounts for the lane changing phase itself, excluding the prepare + * distance, which should be handled separately during path generation. + * + * Additionally, the function calculates the distance buffer to ensure that the ego vehicle has + * enough space to complete the lane change, including cases where multiple lane changes may be + * necessary. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which includes: + * - `lc_param_ptr`: Parameters related to lane changing, such as velocity, lateral acceleration, + * and jerk. + * - `route_handler_ptr`: Pointer to the route handler for managing routes. + * - `direction`: Lane change direction (e.g., left or right). + * - `transient_data.acc.max`: Maximum acceleration of the vehicle. + * - Other relevant data for the ego vehicle's dynamics and state. + * @param lanes The set of lanelets representing the lanes for the lane change. + * + * @return A pair of `MinMaxValue` structures where: + * - The first element contains the minimum and maximum lane changing lengths in meters. + * - The second element contains the minimum and maximum distance buffers in meters. + * If the lanes or necessary data are unavailable, returns `std::numeric_limits::max()` for + * both values. + */ +std::pair calc_lc_length_and_dist_buffer( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 337c959f158f3..46a1c0b05656e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -218,6 +219,7 @@ struct PhaseMetrics struct Lanes { bool current_lane_in_goal_section{false}; + bool target_lane_in_goal_section{false}; lanelet::ConstLanelets current; lanelet::ConstLanelets target_neighbor; lanelet::ConstLanelets target; @@ -313,6 +315,32 @@ struct LanesPolygon std::vector preceding_target; }; +struct MinMaxValue +{ + double min{std::numeric_limits::infinity()}; + double max{std::numeric_limits::infinity()}; +}; + +struct TransientData +{ + MinMaxValue acc; // acceleration profile for accelerating lane change path + MinMaxValue lane_changing_length; // lane changing length for a single lane change + MinMaxValue + current_dist_buffer; // distance buffer computed backward from current lanes' terminal end + MinMaxValue + next_dist_buffer; // distance buffer computed backward from target lanes' terminal end + double dist_to_terminal_end{ + std::numeric_limits::min()}; // distance from ego base link to the current lanes' + // terminal end + double dist_to_terminal_start{ + std::numeric_limits::min()}; // distance from ego base link to the current lanes' + // terminal start + double max_prepare_length{ + std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + + bool is_ego_near_current_terminal_start{false}; +}; + using RouteHandlerPtr = std::shared_ptr; using BppParamPtr = std::shared_ptr; using LCParamPtr = std::shared_ptr; @@ -327,12 +355,14 @@ struct CommonData LCParamPtr lc_param_ptr; LanesPtr lanes_ptr; LanesPolygonPtr lanes_polygon_ptr; + TransientData transient_data; + PathWithLaneId current_lanes_path; ModuleType lc_type; Direction direction; - [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } + [[nodiscard]] const Pose & get_ego_pose() const { return self_odometry_ptr->pose.pose; } - [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; } + [[nodiscard]] const Twist & get_ego_twist() const { return self_odometry_ptr->twist.twist; } [[nodiscard]] double get_ego_speed(bool use_norm = false) const { @@ -344,6 +374,18 @@ struct CommonData const auto y = get_ego_twist().linear.y; return std::hypot(x, y); } + + [[nodiscard]] bool is_data_available() const + { + return route_handler_ptr && self_odometry_ptr && bpp_param_ptr && lc_param_ptr && lanes_ptr && + lanes_polygon_ptr; + } + + [[nodiscard]] bool is_lanes_available() const + { + return lanes_ptr && !lanes_ptr->current.empty() && !lanes_ptr->target.empty() && + !lanes_ptr->target_neighbor.empty(); + } }; using CommonDataPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change 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 4da1fadb1bb87..b6a46af49ac2e 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 @@ -126,6 +126,9 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path); +bool hasEnoughLengthToLaneChangeAfterAbort( + const CommonDataPtr & common_data_ptr, const double abort_return_dist); + CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); std::optional getLaneChangeTargetLane( 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 4ee5156f0ab03..af816bf621582 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 @@ -76,6 +76,7 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); + module_type_->update_transient_data(); module_type_->update_filtered_objects(); module_type_->updateSpecialData(); 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 49ad0e74d9bc9..4e0e0443c5a83 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 @@ -86,11 +86,17 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target = target_lanes; const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + common_data_ptr_->current_lanes_path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); common_data_ptr_->lanes_ptr->current_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(current_lanes.back()); + common_data_ptr_->lanes_ptr->target_lane_in_goal_section = + route_handler_ptr->isInGoalRouteSection(target_lanes.back()); + common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), common_data_ptr_->lc_param_ptr->backward_lane_length); @@ -98,6 +104,53 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } +void NormalLaneChange::update_transient_data() +{ + if ( + !common_data_ptr_ || !common_data_ptr_->is_data_available() || + !common_data_ptr_->is_lanes_available()) { + return; + } + + auto & transient_data = common_data_ptr_->transient_data; + std::tie(transient_data.acc.min, transient_data.acc.max) = calcCurrentMinMaxAcceleration(); + + std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = + calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); + + transient_data.next_dist_buffer.min = + transient_data.current_dist_buffer.min - transient_data.lane_changing_length.min - + common_data_ptr_->lc_param_ptr->lane_change_finish_judge_buffer; + + transient_data.dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + transient_data.dist_to_terminal_start = + transient_data.dist_to_terminal_end - transient_data.current_dist_buffer.min; + + transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + + transient_data.is_ego_near_current_terminal_start = + transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + + RCLCPP_DEBUG( + logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); + RCLCPP_DEBUG( + logger_, "lane_changing_length - min: %.4f, max: %.4f", transient_data.lane_changing_length.min, + transient_data.lane_changing_length.max); + RCLCPP_DEBUG( + logger_, "current_dist_buffer - min: %.4f, max: %.4f", transient_data.current_dist_buffer.min, + transient_data.current_dist_buffer.max); + RCLCPP_DEBUG( + logger_, "next_dist_buffer - min: %.4f, max: %.4f", transient_data.next_dist_buffer.min, + transient_data.next_dist_buffer.max); + RCLCPP_DEBUG(logger_, "dist_to_terminal_start: %.4f", transient_data.dist_to_terminal_start); + RCLCPP_DEBUG(logger_, "dist_to_terminal_end: %.4f", transient_data.dist_to_terminal_end); + RCLCPP_DEBUG(logger_, "max_prepare_length: %.4f", transient_data.max_prepare_length); + RCLCPP_DEBUG( + logger_, "is_ego_near_current_terminal_start: %s", + (transient_data.is_ego_near_current_terminal_start ? "true" : "false")); +} + void NormalLaneChange::update_filtered_objects() { filtered_objects_ = filterObjects(); @@ -151,24 +204,20 @@ bool NormalLaneChange::isLaneChangeRequired() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto current_lanes = - utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); - - if (current_lanes.empty()) { + if ( + !common_data_ptr_ || !common_data_ptr_->is_data_available() || + !common_data_ptr_->is_lanes_available()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - - if (target_lanes.empty()) { - return false; - } + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto ego_dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto maximum_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - if (ego_dist_to_target_start > maximum_prepare_length) { + if (ego_dist_to_target_start > max_prepare_length) { return false; } @@ -182,21 +231,10 @@ bool NormalLaneChange::isLaneChangeRequired() bool NormalLaneChange::is_near_regulatory_element() const { - const auto & current_lanes = get_current_lanes(); - - if (current_lanes.empty()) return false; - - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back()); + if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - if (shift_intervals.empty()) return false; - - const auto & lc_params = *common_data_ptr_->lc_param_ptr; const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto min_lc_length = - calculation::calc_minimum_lane_change_length(lc_params, shift_intervals); - const auto dist_to_terminal_start = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_) - min_lc_length; + const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; if (dist_to_terminal_start <= max_prepare_length) return false; @@ -249,14 +287,9 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back()); - const double next_lane_change_buffer = - calculation::calc_minimum_lane_change_length(lane_change_param, shift_intervals); - - const double buffer = next_lane_change_buffer + - lane_change_param.min_length_for_turn_signal_activation + - common_param.base_link2front; + const auto buffer = common_data_ptr_->transient_data.current_dist_buffer.min + + lane_change_param.min_length_for_turn_signal_activation + + common_param.base_link2front; const double path_length = autoware::motion_utils::calcArcLength(path.points); const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path.points, 0, std::max(path_length - buffer, 0.0)); @@ -405,9 +438,8 @@ void NormalLaneChange::insertStopPoint( return; } - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const auto lane_change_buffer = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); + const auto [_, lanes_dist_buffer] = + calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, lanelets); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -425,7 +457,7 @@ void NormalLaneChange::insertStopPoint( 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 - lane_change_buffer - stop_point_buffer; + double stopping_distance = distance_to_terminal - lanes_dist_buffer.min - stop_point_buffer; const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { @@ -486,11 +518,9 @@ void NormalLaneChange::insertStopPoint( 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 = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - + distance_to_ego_lane_obj - lanes_dist_buffer.min - lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - getCommonParam().base_link2front; @@ -704,9 +734,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & route_handler = getRouteHandler(); const auto & current_pose = getEgoPose(); - const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + // TODO(Azu) fully change to transient data const auto distance_to_lane_change_end = std::invoke([&]() { auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -716,7 +745,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); } - return std::max(0.0, distance_to_end) - lane_change_buffer; + return std::max(0.0, distance_to_end) - + common_data_ptr_->transient_data.current_dist_buffer.min; }); lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; @@ -811,27 +841,18 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const bool NormalLaneChange::is_near_terminal() const { - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - - if (current_lanes.empty()) { + if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) { return true; } - const auto & current_lanes_terminal = current_lanes.back(); + // TODO(Azu) fully change to transient data const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto direction = common_data_ptr_->direction; - const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; - const auto min_lane_changing_distance = calculation::calc_minimum_lane_change_length( - route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction); - const auto backward_buffer = calculation::calc_stopping_distance(lc_param_ptr); - const auto min_lc_dist_with_buffer = - backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer; - const auto dist_from_ego_to_terminal_end = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto min_lc_dist_with_buffer = backward_buffer + current_min_dist_buffer; - return dist_from_ego_to_terminal_end < min_lc_dist_with_buffer; + return common_data_ptr_->transient_data.dist_to_terminal_end < min_lc_dist_with_buffer; } bool NormalLaneChange::isEgoOnPreparePhase() const @@ -954,10 +975,11 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // calculate maximum lane change length - const double max_lane_change_length = + // TODO(Azu) Double check why it's failing with transient data + const auto current_max_dist_buffer = calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (current_max_dist_buffer > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { RCLCPP_DEBUG( logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); @@ -979,12 +1001,12 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( // sample max acc if (route_handler.isInGoalRouteSection(target_lanes.back())) { const auto goal_pose = route_handler.getGoalPose(); - if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + if (current_max_dist_buffer < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { RCLCPP_DEBUG( logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } - } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + } else if (current_max_dist_buffer < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { RCLCPP_DEBUG( logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; @@ -1312,7 +1334,7 @@ PathWithLaneId NormalLaneChange::getTargetSegment( bool NormalLaneChange::hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction) const + const lanelet::ConstLanelets & target_lanes, [[maybe_unused]] const Direction direction) const { if (target_lanes.empty()) { return false; @@ -1322,8 +1344,7 @@ bool NormalLaneChange::hasEnoughLength( const auto & route_handler = getRouteHandler(); const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); const auto minimum_lane_change_length_to_preferred_lane = - calculation::calc_minimum_lane_change_length( - route_handler, target_lanes.back(), *lane_change_parameters_, direction); + common_data_ptr_->transient_data.next_dist_buffer.min; const double lane_change_length = path.info.length.sum(); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { @@ -1380,19 +1401,15 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + const auto is_goal_in_route = common_data_ptr_->lanes_ptr->target_lane_in_goal_section; - const double lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto dist_to_terminal_end = calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); const auto dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto dist_to_terminal_start = dist_to_terminal_end - lane_change_buffer; + const auto dist_to_terminal_end = common_data_ptr_->transient_data.dist_to_terminal_end; + const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); @@ -1430,9 +1447,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) if (!check_lc) return false; const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); - if (lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing) return true; - - return false; + return lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing; }; for (const auto & prep_metric : prepare_phase_metrics) { @@ -1488,7 +1503,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length : dist_to_terminal_end - prep_metric.length; auto target_lane_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer + next_lane_change_buffer; + lane_change_parameters_->lane_change_finish_judge_buffer + next_min_dist_buffer; if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; } @@ -1519,7 +1534,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) try { candidate_path = get_candidate_path( prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - target_lane_length, shift_length, next_lane_change_buffer, is_goal_in_route); + target_lane_length, shift_length, next_min_dist_buffer, is_goal_in_route); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1529,8 +1544,8 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) bool is_safe = false; try { - is_safe = - check_candidate_path_safety(candidate_path, target_objects, lane_change_buffer, is_stuck); + is_safe = check_candidate_path_safety( + candidate_path, target_objects, current_min_dist_buffer, is_stuck); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); return false; @@ -1649,12 +1664,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); @@ -1673,7 +1684,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto lane_changing_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( prev_module_output_.path.points, current_lane_terminal_point, - -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + -(current_min_dist_buffer + next_min_dist_buffer + distance_to_terminal_from_goal)); if (!lane_changing_start_pose) { RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); @@ -1699,8 +1710,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, - minimum_lane_changing_velocity, next_lane_change_buffer); + target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, + minimum_lane_changing_velocity, next_min_dist_buffer); if (target_segment.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); @@ -1712,7 +1723,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; lane_change_info.velocity = LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.length = LaneChangePhaseInfo{0.0, current_min_dist_buffer}; lane_change_info.lane_changing_start = lane_changing_start_pose.value(); lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = max_lateral_acc; @@ -1727,11 +1738,11 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_change_buffer, minimum_lane_changing_velocity); + current_min_dist_buffer, minimum_lane_changing_velocity); const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); + current_min_dist_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_min_dist_buffer); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); @@ -1772,12 +1783,11 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto min_lc_length = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, min_lc_length, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, current_min_dist_buffer, + debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -1893,16 +1903,14 @@ bool NormalLaneChange::calcAbortPath() const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; - const auto direction = getDirection(); - const auto minimum_lane_change_length = calculation::calc_minimum_lane_change_length( - route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto & lane_changing_path = selected_path.path; const auto & reference_lanelets = get_current_lanes(); const auto lane_changing_end_pose_idx = std::invoke([&]() { constexpr double s_start = 0.0; const double s_end = std::max( - lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, 0.0); + lanelet::utils::getLaneletLength2d(reference_lanelets) - current_min_dist_buffer, 0.0); const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( @@ -1942,21 +1950,8 @@ bool NormalLaneChange::calcAbortPath() return false; } - const auto dist_to_terminal_start = std::invoke([&]() { - const auto dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, get_current_lanes(), common_data_ptr_->get_ego_pose()); - const auto min_lc_length = calculation::calc_minimum_lane_change_length( - route_handler, get_current_lanes().back(), *common_data_ptr_->lc_param_ptr, - common_data_ptr_->direction); - return dist_to_terminal_end - min_lc_length; - }); - - const auto enough_abort_dist = - abort_start_dist + abort_return_dist + - calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <= - dist_to_terminal_start; - - if (!enough_abort_dist) { + if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( + common_data_ptr_, abort_return_dist)) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -2238,10 +2233,9 @@ bool NormalLaneChange::isVehicleStuck( route_handler->isInGoalRouteSection(current_lanes.back()) ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; + const double terminal_judge_buffer = current_min_dist_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { return true; } @@ -2275,8 +2269,7 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan } const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto max_lane_change_length = - calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); + const auto current_max_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.max; const auto rss_dist = calcRssDistance( 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); @@ -2287,9 +2280,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan // stopped at a traffic light. Essentially, the calculation should be based on the information of // the stop reason, but this is outside the scope of one module. I keep it as a TODO. constexpr double DETECTION_DISTANCE_MARGIN = 10.0; - const auto detection_distance = max_lane_change_length + rss_dist + + const auto detection_distance = current_max_dist_buffer + rss_dist + getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + RCLCPP_DEBUG( + logger_, "current_max_dist_buffer: %f, max_acc: %f", current_max_dist_buffer, max_acc); auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 4473ab95dc18d..b94434f03e80c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -28,15 +28,6 @@ rclcpp::Logger get_logger() return logger; } -double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) -{ - const auto & lanes_ptr = common_data_ptr->lanes_ptr; - const auto & current_lanes = lanes_ptr->current; - const auto & current_pose = common_data_ptr->get_ego_pose(); - - return calc_dist_from_pose_to_terminal_end(common_data_ptr, current_lanes, current_pose); -} - double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, const Pose & src_pose) @@ -45,10 +36,10 @@ double calc_dist_from_pose_to_terminal_end( return 0.0; } - const auto & lanes_ptr = common_data_ptr->lanes_ptr; - const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); - - if (lanes_ptr->current_lane_in_goal_section) { + const auto in_goal_route_section = + common_data_ptr->route_handler_ptr->isInGoalRouteSection(lanes.back()); + if (in_goal_route_section) { + const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); return utils::getSignedDistance(src_pose, goal_pose, lanes); } return utils::getDistanceToEndOfLane(src_pose, lanes); @@ -137,8 +128,7 @@ double calc_ego_dist_to_lanes_start( return std::numeric_limits::max(); } - const auto path = - route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr->current_lanes_path; if (path.points.empty()) { return std::numeric_limits::max(); @@ -150,44 +140,6 @@ double calc_ego_dist_to_lanes_start( return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); } -double calc_minimum_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals) -{ - if (shift_intervals.empty()) { - return 0.0; - } - - const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; - const auto min_max_lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(min_vel); - // const auto min_lat_acc = std::get<0>(min_max_lat_acc); - const auto max_lat_acc = std::get<1>(min_max_lat_acc); - const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - - const auto calc_sum = [&](double sum, double shift_interval) { - const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); - return sum + (min_vel * t + finish_judge_buffer); - }; - - const auto total_length = - std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); - - const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); -} - -double calc_minimum_lane_change_length( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction) -{ - if (!route_handler) { - return std::numeric_limits::max(); - } - - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lane, direction); - return calc_minimum_lane_change_length(lane_change_parameters, shift_intervals); -} - double calc_maximum_lane_change_length( const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc) @@ -240,6 +192,128 @@ double calc_maximum_lane_change_length( vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc); } +std::vector calc_all_min_lc_lengths( + const LCParamPtr & lc_param_ptr, const std::vector & shift_intervals) +{ + if (shift_intervals.empty()) { + return {}; + } + + const auto min_vel = lc_param_ptr->minimum_lane_changing_velocity; + const auto min_max_lat_acc = lc_param_ptr->lane_change_lat_acc_map.find(min_vel); + const auto max_lat_acc = std::get<1>(min_max_lat_acc); + const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; + + std::vector min_lc_lengths{}; + min_lc_lengths.reserve(shift_intervals.size()); + + const auto min_lc_length = [&](const auto shift_interval) { + const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + return min_vel * t; + }; + + std::transform( + shift_intervals.cbegin(), shift_intervals.cend(), std::back_inserter(min_lc_lengths), + min_lc_length); + + return min_lc_lengths; +} + +std::vector calc_all_max_lc_lengths( + const CommonDataPtr & common_data_ptr, const std::vector & shift_intervals) +{ + if (shift_intervals.empty()) { + return {}; + } + + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; + const auto t_prepare = lc_param_ptr->lane_change_prepare_duration; + const auto max_acc = common_data_ptr->transient_data.acc.max; + + const auto limit_vel = [&](const auto vel) { + const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; + return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); + }; + + auto vel = limit_vel(common_data_ptr->get_ego_speed()); + + std::vector max_lc_lengths{}; + + const auto max_lc_length = [&](const auto shift_interval) { + // prepare section + vel = limit_vel(vel + max_acc * t_prepare); + const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + + // lane changing section + const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); + const auto t_lane_changing = + PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + const auto lane_changing_length = + vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; + + vel = limit_vel(vel + max_acc * t_lane_changing); + return prepare_length + lane_changing_length; + }; + + std::transform( + shift_intervals.cbegin(), shift_intervals.cend(), std::back_inserter(max_lc_lengths), + max_lc_length); + return max_lc_lengths; +} + +double calc_distance_buffer( + const LCParamPtr & lc_param_ptr, const std::vector & min_lc_lengths) +{ + if (min_lc_lengths.empty()) { + return std::numeric_limits::max(); + } + + const auto finish_judge_buffer = lc_param_ptr->lane_change_finish_judge_buffer; + const auto backward_buffer = calc_stopping_distance(lc_param_ptr); + const auto lengths_sum = std::accumulate(min_lc_lengths.begin(), min_lc_lengths.end(), 0.0); + const auto num_of_lane_changes = static_cast(min_lc_lengths.size()); + return lengths_sum + (num_of_lane_changes * finish_judge_buffer) + + ((num_of_lane_changes - 1.0) * backward_buffer); +} + +std::vector calc_shift_intervals( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes) +{ + if (!common_data_ptr || !common_data_ptr->is_data_available() || lanes.empty()) { + return {}; + } + + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto direction = common_data_ptr->direction; + + return route_handler_ptr->getLateralIntervalsToPreferredLane(lanes.back(), direction); +} + +std::pair calc_lc_length_and_dist_buffer( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes) +{ + if (!common_data_ptr || !common_data_ptr->is_data_available() || lanes.empty()) { + return {}; + } + const auto shift_intervals = calculation::calc_shift_intervals(common_data_ptr, lanes); + const auto all_min_lc_lengths = + calculation::calc_all_min_lc_lengths(common_data_ptr->lc_param_ptr, shift_intervals); + const auto min_lc_length = + !all_min_lc_lengths.empty() ? all_min_lc_lengths.front() : std::numeric_limits::max(); + const auto min_dist_buffer = + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_min_lc_lengths); + + const auto all_max_lc_lengths = + calculation::calc_all_max_lc_lengths(common_data_ptr, shift_intervals); + const auto max_lc_length = + !all_max_lc_lengths.empty() ? all_max_lc_lengths.front() : std::numeric_limits::max(); + const auto max_dist_buffer = + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_max_lc_lengths); + + return {{min_lc_length, max_lc_length}, {min_dist_buffer, max_dist_buffer}}; +} + double calc_phase_length( const double velocity, const double maximum_velocity, const double acceleration, const double duration) 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 5e1f17a4612fa..798a6e1b32e2e 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 @@ -577,6 +577,26 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } +bool hasEnoughLengthToLaneChangeAfterAbort( + const CommonDataPtr & common_data_ptr, const double abort_return_dist) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + if (current_lanes.empty()) { + return false; + } + + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto abort_plus_lane_change_length = + abort_return_dist + common_data_ptr->transient_data.current_dist_buffer.min; + if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + return false; + } + + const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); + return abort_plus_lane_change_length <= + utils::getSignedDistance(current_pose, goal_pose, current_lanes); +} + std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) @@ -858,9 +878,7 @@ bool passed_parked_objects( lane_change_parameters.object_check_min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = lane_change_parameters.object_shiftable_ratio_threshold; - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - const auto current_lane_path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & current_lane_path = common_data_ptr->current_lanes_path; if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { return true; From 5c1fa6c2a1aef2ab7ce91d1cc57801bae10ecfe6 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 27 Sep 2024 20:40:37 +0900 Subject: [PATCH 05/29] fix(lane_change): fix abort distance enough check (#8979) * RT1-7991 fix abort distance enough check Signed-off-by: Zulfaqar Azmi * RT-7991 remove unused function Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 3 --- .../src/scene.cpp | 8 ++++++-- .../src/utils/utils.cpp | 20 ------------------- 3 files changed, 6 insertions(+), 25 deletions(-) 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 b6a46af49ac2e..4da1fadb1bb87 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 @@ -126,9 +126,6 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path); -bool hasEnoughLengthToLaneChangeAfterAbort( - const CommonDataPtr & common_data_ptr, const double abort_return_dist); - CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); std::optional getLaneChangeTargetLane( 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 4e0e0443c5a83..58734b725049c 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 @@ -1950,8 +1950,12 @@ bool NormalLaneChange::calcAbortPath() return false; } - if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - common_data_ptr_, abort_return_dist)) { + const auto enough_abort_dist = + abort_start_dist + abort_return_dist + + calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <= + common_data_ptr_->transient_data.dist_to_terminal_start; + + if (!enough_abort_dist) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } 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 798a6e1b32e2e..ffabe3b73a262 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 @@ -577,26 +577,6 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -bool hasEnoughLengthToLaneChangeAfterAbort( - const CommonDataPtr & common_data_ptr, const double abort_return_dist) -{ - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - if (current_lanes.empty()) { - return false; - } - - const auto & current_pose = common_data_ptr->get_ego_pose(); - const auto abort_plus_lane_change_length = - abort_return_dist + common_data_ptr->transient_data.current_dist_buffer.min; - if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } - - const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); - return abort_plus_lane_change_length <= - utils::getSignedDistance(current_pose, goal_pose, current_lanes); -} - std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) From 4765981b599ca2f5c9d6d7181425717b5b0604c6 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 4 Oct 2024 08:46:48 +0900 Subject: [PATCH 06/29] refactor(lane_change): refactor code using transient data (#8997) * add target lane length and ego arc length along current and target lanes to transient data Signed-off-by: mohammad alqudah * refactor code using transient data Signed-off-by: mohammad alqudah * refactor get_lane_change_paths function Signed-off-by: mohammad alqudah * minor refactoring Signed-off-by: mohammad alqudah * refactor util functions Signed-off-by: mohammad alqudah * refactor getPrepareSegment function Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../scene.hpp | 15 +- .../utils/base_class.hpp | 5 +- .../utils/data_structs.hpp | 5 + .../utils/utils.hpp | 16 +- .../src/scene.cpp | 291 +++++++++--------- .../src/utils/utils.cpp | 33 +- 6 files changed, 184 insertions(+), 181 deletions(-) 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 f976e1fa59f4f..3ec74461e014b 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 @@ -142,9 +142,8 @@ class NormalLaneChange : public LaneChangeBase FilteredByLanesObjects filterObjectsByLanelets( const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const override; + bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const override; PathWithLaneId getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, @@ -155,17 +154,21 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + std::vector get_prepare_metrics() const; + std::vector get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, + const double shift_length, const double dist_to_reg_element) const; + bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; LaneChangePath get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const; + const Pose & lc_start_pose, const double shift_length) const; bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const; + const bool is_stuck) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, 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 72a40caca1d6a..580c5709cb5c1 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 @@ -238,9 +238,8 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const = 0; + virtual bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 46a1c0b05656e..5c8b70c59cdc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -338,6 +338,11 @@ struct TransientData double max_prepare_length{ std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + double target_lane_length{std::numeric_limits::min()}; + + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes + lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes + bool is_ego_near_current_terminal_start{false}; }; 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 4da1fadb1bb87..b86bfaf425af5 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 @@ -79,9 +79,7 @@ std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, @@ -109,12 +107,9 @@ ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, - const double next_lane_change_buffer); +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval); std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, @@ -149,8 +144,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug); + const std::vector & objects, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, 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 58734b725049c..db375651c4e3a 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 @@ -129,6 +129,15 @@ void NormalLaneChange::update_transient_data() transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + transient_data.target_lane_length = + lanelet::utils::getLaneletLength2d(common_data_ptr_->lanes_ptr->target); + + transient_data.current_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + + transient_data.target_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; @@ -166,8 +175,7 @@ void NormalLaneChange::updateLaneChangeStatus() status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); - status_.start_distance = arclength_start.length; + status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -233,10 +241,7 @@ bool NormalLaneChange::is_near_regulatory_element() const { if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; - - if (dist_to_terminal_start <= max_prepare_length) return false; + if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) return false; const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; @@ -244,8 +249,9 @@ bool NormalLaneChange::is_near_regulatory_element() const RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); } - return max_prepare_length > utils::lane_change::get_distance_to_next_regulatory_element( - common_data_ptr_, only_tl, only_tl); + return common_data_ptr_->transient_data.max_prepare_length > + utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -593,12 +599,13 @@ std::optional NormalLaneChange::extendPath() } auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); + const auto & transient_data = common_data_ptr_->transient_data; const auto forward_path_length = getCommonParam().forward_path_length; - if ((target_lane_length - dist_in_target.length) > forward_path_length) { + if ( + (transient_data.target_lane_length - transient_data.target_lanes_ego_arc.length) > + forward_path_length) { return std::nullopt; } @@ -646,6 +653,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getCenterLinePath( target_lanes, dist_to_end_of_path, dist_to_target_pose); } + void NormalLaneChange::resetParameters() { is_abort_path_approved_ = false; @@ -662,11 +670,10 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & pose = getEgoPose(); const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; - const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + const auto current_shift_length = common_data_ptr_->transient_data.current_lanes_ego_arc.distance; constexpr bool is_driving_forward = true; // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's // current lane, lane change is different, so we set this flag to false. @@ -789,7 +796,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + const auto & arc_length = common_data_ptr_->transient_data.target_lanes_ego_arc; const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -1037,21 +1044,41 @@ std::vector NormalLaneChange::calcPrepareDuration( return prepare_durations; } -PathWithLaneId NormalLaneChange::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const +bool NormalLaneChange::get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const { - if (current_lanes.empty()) { - return PathWithLaneId(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + return false; } - auto prepare_segment = prev_module_output_.path; + prepare_segment = prev_module_output_.path; const size_t current_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - return prepare_segment; + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + return true; } lane_change::TargetObjects NormalLaneChange::getTargetObjects( @@ -1369,73 +1396,90 @@ bool NormalLaneChange::hasEnoughLength( return true; } -bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const +std::vector NormalLaneChange::get_prepare_metrics() const { - lane_change_debug_.collision_check_objects.clear(); - - const auto & current_lanes = get_current_lanes(); - const auto & target_lanes = get_target_lanes(); - const auto is_stuck = isVehicleStuck(current_lanes); - - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; - - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { - return false; - } - - const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - - // get velocity + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto current_velocity = getEgoVelocity(); // get sampling acceleration values const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto is_goal_in_route = common_data_ptr_->lanes_ptr->target_lane_in_goal_section; + const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); const auto dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto dist_to_terminal_end = common_data_ptr_->transient_data.dist_to_terminal_end; - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; + return calculation::calc_prepare_phase_metrics( + common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, + dist_to_target_start, common_data_ptr_->transient_data.dist_to_terminal_start); +} + +std::vector NormalLaneChange::get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, + const double shift_length, const double dist_to_reg_element) const +{ + const auto & route_handler = getRouteHandler(); + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prep_segment.points.back().point.pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + transient_data.is_ego_near_current_terminal_start + ? transient_data.dist_to_terminal_end - prep_metric.length + : std::min(transient_data.dist_to_terminal_end, dist_to_reg_element) - prep_metric.length; + auto target_lane_buffer = lane_change_parameters_->lane_change_finish_judge_buffer + + transient_data.next_dist_buffer.min; + if (std::abs(route_handler->getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { + target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; + } + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_length; + }); - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; + return calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + prep_metric.sampled_lon_accel, max_lane_changing_length); +} - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); +bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const +{ + lane_change_debug_.collision_check_objects.clear(); + + if (!common_data_ptr_->is_lanes_available()) { + RCLCPP_WARN(logger_, "lanes are not available. Not expected."); + return false; + } + if (common_data_ptr_->lanes_polygon_ptr->target_neighbor.empty()) { + RCLCPP_WARN(logger_, "target_lane_neighbors_polygon_2d is empty. Not expected."); + return false; + } + + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + + const auto is_stuck = isVehicleStuck(current_lanes); + const auto current_velocity = getEgoVelocity(); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_phase_metrics = get_prepare_metrics(); candidate_paths.reserve( - longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * - prepare_durations.size()); - - RCLCPP_DEBUG( - logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", - prepare_durations.size(), longitudinal_acc_sampling_values.size()); + prepare_phase_metrics.size() * lane_change_parameters_->lateral_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; const auto dist_to_next_regulatory_element = utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - - const auto prepare_phase_metrics = calculation::calc_prepare_phase_metrics( - common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, - dist_to_target_start, dist_to_terminal_start); auto check_length_diff = [&](const double prep_length, const double lc_length, const bool check_lc) { @@ -1451,10 +1495,10 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) }; for (const auto & prep_metric : prepare_phase_metrics) { - const auto debug_print = [&](const auto & s) { + const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s, prep_metric.duration, - prep_metric.actual_lon_accel, prep_metric.length); + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), + prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length); }; if (!check_length_diff(prep_metric.length, 0.0, false)) { @@ -1462,59 +1506,26 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) continue; } - auto prepare_segment = - getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); - - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); - continue; - } - - // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is - // larger than distance to target lane start - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); - continue; - } - - // lane changing start is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - debug_print("lane change start is behind target lanelet!"); + PathWithLaneId prepare_segment; + try { + if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + debug_print("Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + debug_print(e.what()); break; } debug_print("Prepare path satisfy constraints"); + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); - - const auto max_lane_changing_length = std::invoke([&]() { - double max_length = - dist_to_terminal_start > max_prepare_length - ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length - : dist_to_terminal_end - prep_metric.length; - auto target_lane_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer + next_min_dist_buffer; - if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { - target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; - } - max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); - return max_length; - }); - - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( - common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, - prep_metric.sampled_lon_accel, max_lane_changing_length); + const auto lane_changing_metrics = get_lane_changing_metrics( + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); @@ -1534,7 +1545,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) try { candidate_path = get_candidate_path( prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - target_lane_length, shift_length, next_min_dist_buffer, is_goal_in_route); + shift_length); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1542,20 +1553,16 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) candidate_paths.push_back(candidate_path); - bool is_safe = false; try { - is_safe = check_candidate_path_safety( - candidate_path, target_objects, current_min_dist_buffer, is_stuck); + if (check_candidate_path_safety(candidate_path, target_objects, is_stuck)) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; + } } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); return false; } - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } - debug_print_lat("Reject: sampled path is not safe."); } } @@ -1567,19 +1574,16 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) LaneChangePath NormalLaneChange::get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const + const Pose & lc_start_pose, const double shift_length) const { const auto & route_handler = *getRouteHandler(); const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto & forward_path_length = planner_data_->parameters.forward_path_length; const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lc_start_pose, target_lane_length, lc_metrics.length, - forward_path_length, resample_interval, is_goal_in_route, next_lc_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); if (target_lane_reference_path.points.empty()) { throw std::logic_error("target_lane_reference_path is empty!"); @@ -1613,12 +1617,12 @@ LaneChangePath NormalLaneChange::get_candidate_path( bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const + const bool is_stuck) const { if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { + lane_change_debug_.collision_check_objects)) { throw std::logic_error( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); } @@ -1656,9 +1660,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto forward_path_length = common_parameters.forward_path_length; const auto minimum_lane_changing_velocity = lane_change_parameters_->minimum_lane_changing_velocity; @@ -1667,10 +1669,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = @@ -1709,6 +1708,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; const auto target_segment = getTargetSegment( target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, minimum_lane_changing_velocity, next_min_dist_buffer); @@ -1739,10 +1739,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - current_min_dist_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_min_dist_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); @@ -1783,11 +1781,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, current_min_dist_buffer, - debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -2213,8 +2208,7 @@ bool NormalLaneChange::isVehicleStuck( } // Check if any stationary object exist in obstacle_check_distance - using lanelet::utils::getArcCoordinates; - const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; + const auto base_distance = common_data_ptr_->transient_data.current_lanes_ego_arc.length; for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point @@ -2224,7 +2218,8 @@ bool NormalLaneChange::isVehicleStuck( continue; } - const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; + const auto ego_to_obj_dist = + lanelet::utils::getArcCoordinates(current_lanes, p).length - base_distance; if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); return true; // Stationary object is in front of ego. 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 ffabe3b73a262..b3c0dd95a3d96 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 @@ -351,12 +351,17 @@ std::optional construct_candidate_path( return std::optional{candidate_path}; } -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer) +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) { + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -366,10 +371,10 @@ PathWithLaneId getReferencePathFromTargetLane( if (is_goal_in_route) { const double s_goal = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - - next_lane_change_buffer; + next_lc_buffer; return std::min(dist_from_lc_start, s_goal); } - return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); }); constexpr double epsilon = 1e-4; @@ -577,10 +582,13 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr) { + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto rough_shift_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; @@ -849,8 +857,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug) + const std::vector & objects, CollisionCheckDebugMap & object_debug) { const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; @@ -900,7 +907,7 @@ bool passed_parked_objects( }); // 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) { + if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { return true; } From 88352a18ae4b5bd7ff9fc76b1249af64ea522d6f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 4 Oct 2024 15:53:09 +0900 Subject: [PATCH 07/29] refactor(lane_change): replace any code that can use transient data (#8999) * RT1-8004 replace hasEnoughLength Signed-off-by: Zulfaqar Azmi * RT1-8004 Removed isNearEndOfCurrentLanes Signed-off-by: Zulfaqar Azmi * RT1-8004 refactor sample longitudinal acc values Signed-off-by: Zulfaqar Azmi * remove calc maximum lane change length Signed-off-by: Zulfaqar Azmi * Revert "remove calc maximum lane change length" This reverts commit e9cc386e1c21321c59f518d2acbe78a3c668471f. * Revert "RT1-8004 refactor sample longitudinal acc values" This reverts commit 775bcdb8fa1817511741776861f9edb7e22fd744. * replace generateCenterLinePath Signed-off-by: Zulfaqar Azmi * RT1-8004 simplify stuck detection Signed-off-by: Zulfaqar Azmi * swap call to update filtered_objects and update transient data Signed-off-by: Zulfaqar Azmi * RT1-8004 fix conflict Signed-off-by: Zulfaqar Azmi * RT1-8004 Rename isVehicleStuck to is_ego_stuck() Signed-off-by: Zulfaqar Azmi * RT1-8004 change calcPrepareDuration to snake case Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../README.md | 2 +- .../scene.hpp | 23 +- .../utils/base_class.hpp | 4 - .../utils/data_structs.hpp | 1 + .../src/interface.cpp | 2 +- .../src/scene.cpp | 236 +++++------------- 6 files changed, 74 insertions(+), 194 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 18a797976161c..02280f7ffa697 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 (isVehicleStuck(current_lanes)) then (yes) +if ego is stuck in the current lanes then (yes) :Return **sampled acceleration values**; stop else (no) 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 3ec74461e014b..0b4d6deea7a64 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 @@ -89,10 +89,6 @@ class NormalLaneChange : public LaneChangeBase bool isRequiredStop(const bool is_trailing_object) override; - bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const override; - bool hasFinishedLaneChange() const override; bool isAbleToReturnCurrentLane() const override; @@ -127,9 +123,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - std::vector calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + std::vector calc_prepare_durations() const; lane_change::TargetObjects getTargetObjects( const FilteredByLanesExtendedObjects & predicted_objects, @@ -150,10 +144,6 @@ class NormalLaneChange : public LaneChangeBase const double target_lane_length, const double lane_changing_length, const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - bool hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; - std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, @@ -167,8 +157,7 @@ class NormalLaneChange : public LaneChangeBase const Pose & lc_start_pose, const double shift_length) const; bool check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const bool is_stuck) const; + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, @@ -193,15 +182,9 @@ class NormalLaneChange : public LaneChangeBase const RSSparams & selected_rss_param, const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const; - //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. - //! @param obstacle_check_distance Distance to check ahead for any objects that might be - //! obstructing ego path. It makes sense to use values like the maximum lane change distance. - bool isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; - double get_max_velocity_for_safety_check() const; - bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool is_ego_stuck() const; /** * @brief Checks if the given pose is a valid starting point for a lane change. 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 580c5709cb5c1..4816a2f6c4eac 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 @@ -112,10 +112,6 @@ class LaneChangeBase virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( PathSafetyStatus approve_path_safety_status) = 0; - virtual bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const = 0; - virtual bool isStoppedAtRedTrafficLight() const = 0; virtual bool calcAbortPath() = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 5c8b70c59cdc7..6b4b7c4f2dc45 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -344,6 +344,7 @@ struct TransientData lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes bool is_ego_near_current_terminal_start{false}; + bool is_ego_stuck{false}; }; using RouteHandlerPtr = std::shared_ptr; 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 af816bf621582..23ae850189e2a 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 @@ -76,8 +76,8 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); - module_type_->update_transient_data(); module_type_->update_filtered_objects(); + module_type_->update_transient_data(); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { 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 db375651c4e3a..d49d05f2a6f18 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 @@ -141,6 +141,9 @@ void NormalLaneChange::update_transient_data() transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + updateStopTime(); + transient_data.is_ego_stuck = is_ego_stuck(); + RCLCPP_DEBUG( logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); RCLCPP_DEBUG( @@ -168,7 +171,6 @@ void NormalLaneChange::update_filtered_objects() void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -731,35 +733,6 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( lane_change_lane.value(), getEgoPose(), backward_length, forward_length); } -bool NormalLaneChange::isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const -{ - if (current_lanes.empty()) { - return false; - } - - const auto & route_handler = getRouteHandler(); - const auto & current_pose = getEgoPose(); - - // TODO(Azu) fully change to transient data - const auto distance_to_lane_change_end = std::invoke([&]() { - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - - if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { - distance_to_end = std::min( - distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); - } - - return std::max(0.0, distance_to_end) - - common_data_ptr_->transient_data.current_dist_buffer.min; - }); - - lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; - return distance_to_lane_change_end < threshold; -} - bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -963,6 +936,7 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { + // TODO(Azu): sampler should work even when we're not approaching terminal if (prev_module_output_.path.points.empty()) { return {}; } @@ -986,7 +960,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( const auto current_max_dist_buffer = calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - if (current_max_dist_buffer > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { RCLCPP_DEBUG( logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); @@ -995,7 +969,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuck(current_lanes)) { + if (common_data_ptr_->transient_data.is_ego_stuck) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, @@ -1023,22 +997,23 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -std::vector NormalLaneChange::calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +std::vector NormalLaneChange::calc_prepare_durations() const { - const auto base_link2front = planner_data_->parameters.base_link2front; - const auto threshold = - lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front + + lc_param_ptr->min_length_for_turn_signal_activation; + const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; + + // TODO(Azu) this check seems to cause scenario failures. + if (common_data_ptr_->transient_data.dist_to_terminal_start >= threshold) { + return {max_prepare_duration}; + } std::vector prepare_durations; constexpr double step = 0.5; - for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; - duration -= step) { + for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { prepare_durations.push_back(duration); - if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { - break; - } } return prepare_durations; @@ -1083,12 +1058,11 @@ bool NormalLaneChange::get_prepare_segment( lane_change::TargetObjects NormalLaneChange::getTargetObjects( const FilteredByLanesExtendedObjects & filtered_objects, - const lanelet::ConstLanelets & current_lanes) const + [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; - const auto is_stuck = isVehicleStuck(current_lanes); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; - if (chk_obj_in_curr_lanes || is_stuck) { + if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { leading_objects.insert( leading_objects.end(), filtered_objects.current_lane.begin(), filtered_objects.current_lane.end()); @@ -1133,8 +1107,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto path = - route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr_->current_lanes_path; auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); @@ -1359,43 +1332,6 @@ PathWithLaneId NormalLaneChange::getTargetSegment( return target_segment; } -bool NormalLaneChange::hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, [[maybe_unused]] const Direction direction) const -{ - if (target_lanes.empty()) { - return false; - } - - const auto current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); - const auto minimum_lane_change_length_to_preferred_lane = - common_data_ptr_->transient_data.next_dist_buffer.min; - - const double lane_change_length = path.info.length.sum(); - if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } - - const auto goal_pose = route_handler->getGoalPose(); - if ( - route_handler->isInGoalRouteSection(current_lanes.back()) && - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { - return false; - } - - // return if there are no target lanes - if ( - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - return false; - } - - return true; -} - std::vector NormalLaneChange::get_prepare_metrics() const { const auto & current_lanes = common_data_ptr_->lanes_ptr->current; @@ -1406,7 +1342,7 @@ std::vector NormalLaneChange::get_prepare_metrics() cons const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_durations = calc_prepare_durations(); RCLCPP_DEBUG( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", @@ -1467,7 +1403,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto & current_lanes = get_current_lanes(); const auto & target_lanes = get_target_lanes(); - const auto is_stuck = isVehicleStuck(current_lanes); const auto current_velocity = getEgoVelocity(); const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); @@ -1554,7 +1489,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) candidate_paths.push_back(candidate_path); try { - if (check_candidate_path_safety(candidate_path, target_objects, is_stuck)) { + if (check_candidate_path_safety(candidate_path, target_objects)) { debug_print_lat("ACCEPT!!!: it is valid and safe!"); return true; } @@ -1577,7 +1512,6 @@ LaneChangePath NormalLaneChange::get_candidate_path( const Pose & lc_start_pose, const double shift_length) const { const auto & route_handler = *getRouteHandler(); - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto resample_interval = @@ -1608,7 +1542,10 @@ LaneChangePath NormalLaneChange::get_candidate_path( throw std::logic_error("failed to generate candidate path!"); } - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + if ( + candidate_path.value().info.length.sum() + + common_data_ptr_->transient_data.next_dist_buffer.min > + common_data_ptr_->transient_data.dist_to_terminal_end) { throw std::logic_error("invalid candidate path length!"); } @@ -1616,9 +1553,9 @@ LaneChangePath NormalLaneChange::get_candidate_path( } bool NormalLaneChange::check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const bool is_stuck) const + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { + const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck; if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, @@ -1874,10 +1811,9 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && - isAbleToStopSafely() && is_trailing_object) { + common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && + is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -2190,63 +2126,6 @@ bool NormalLaneChange::is_collided( return !is_collided; } -// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes -bool NormalLaneChange::isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // Ego is still moving, not in stuck - if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { - RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); - return false; - } - - // Ego is just stopped, not sure it is in stuck yet. - if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); - return false; - } - - // Check if any stationary object exist in obstacle_check_distance - const auto base_distance = common_data_ptr_->transient_data.current_lanes_ego_arc.length; - - for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point - - // Note: it needs chattering prevention. - if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary - continue; - } - - const auto ego_to_obj_dist = - lanelet::utils::getArcCoordinates(current_lanes, p).length - base_distance; - if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { - RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); - return true; // Stationary object is in front of ego. - } - } - - // Check if Ego is in terminal of current lanes - const auto & route_handler = getRouteHandler(); - const double distance_to_terminal = - route_handler->isInGoalRouteSection(current_lanes.back()) - ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) - : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = current_min_dist_buffer + stop_point_buffer + 1.0; - if (distance_to_terminal < terminal_judge_buffer) { - return true; - } - - // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current - RCLCPP_DEBUG( - logger_, - "No stationary objects found in obstacle_check_distance and Ego is not in " - "terminal of current lanes"); - return false; -} - double NormalLaneChange::get_max_velocity_for_safety_check() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2259,35 +2138,56 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const return getCommonParam().max_vel; } -bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::is_ego_stuck() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (current_lanes.empty()) { - lane_change_debug_.is_stuck = false; - return false; // can not check + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + + if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); + return false; } - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto current_max_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.max; - const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lc_param_ptr->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + const auto & current_lanes_path = common_data_ptr_->current_lanes_path; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto rss_dist = + calcRssDistance(0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be // determined, even though you are stuck by an obstacle. If it is too long, // the ego will be judged to be stuck by a distant vehicle, even though the ego is only // stopped at a traffic light. Essentially, the calculation should be based on the information of // the stop reason, but this is outside the scope of one module. I keep it as a TODO. - constexpr double DETECTION_DISTANCE_MARGIN = 10.0; - const auto detection_distance = current_max_dist_buffer + rss_dist + - getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_DEBUG( - logger_, "current_max_dist_buffer: %f, max_acc: %f", current_max_dist_buffer, max_acc); + constexpr auto detection_distance_margin = 10.0; + const auto obstacle_check_distance = common_data_ptr_->transient_data.lane_changing_length.max + + rss_dist + common_data_ptr_->bpp_param_ptr->base_link2front + + detection_distance_margin; + const auto has_object_blocking = std::any_of( + filtered_objects_.current_lane.begin(), filtered_objects_.current_lane.end(), + [&](const auto & object) { + // Note: it needs chattering prevention. + if ( + std::abs(object.initial_twist.linear.x) > + lc_param_ptr->stopped_object_velocity_threshold) { // check if stationary + return false; + } - auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + const auto ego_to_obj_dist = + calcSignedArcLength( + current_lanes_path.points, ego_pose.position, object.initial_pose.position) - + obstacle_check_distance; + return ego_to_obj_dist < 0.0; + }); - lane_change_debug_.is_stuck = is_vehicle_stuck; - return is_vehicle_stuck; + lane_change_debug_.is_stuck = has_object_blocking; + return has_object_blocking; } bool NormalLaneChange::is_valid_start_point( From 284525d21bceb457323ea264a2bc51b9a537c16a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 7 Oct 2024 18:07:45 +0900 Subject: [PATCH 08/29] refactor(lane_change): refactor get_lane_change_lanes function (#9044) * refactor(lane_change): refactor get_lane_change_lanes function Signed-off-by: Zulfaqar Azmi * Add doxygen comment for to_geom_msg_pose Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 3 +- .../utils/base_class.hpp | 3 -- .../utils/utils.hpp | 9 ++-- .../src/scene.cpp | 43 +++++-------------- .../src/utils/utils.cpp | 28 ++++++------ .../utils/utils.hpp | 27 ++++++++++++ .../src/utils/utils.cpp | 21 +-------- 7 files changed, 63 insertions(+), 71 deletions(-) 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 0b4d6deea7a64..1397a9ca5765a 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 @@ -112,8 +112,7 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() const final; protected: - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const override; + lanelet::ConstLanelets get_lane_change_lanes(const lanelet::ConstLanelets & current_lanes) const; int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; 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 4816a2f6c4eac..c124353e2873a 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 @@ -241,9 +241,6 @@ class LaneChangeBase virtual bool isAbleToStopSafely() const = 0; - virtual lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; - virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0; TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const 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 b86bfaf425af5..41cef16733e54 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 @@ -51,6 +51,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -58,6 +59,8 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; +bool is_mandatory_lane_change(const ModuleType lc_type); + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); @@ -123,9 +126,9 @@ double getLateralShift(const LaneChangePath & path); CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction); + +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes); std::vector convert_to_predicted_path( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, 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 d49d05f2a6f18..af9a401fa1e80 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 @@ -69,7 +69,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) return; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + const auto target_lanes = get_lane_change_lanes(current_lanes); if (target_lanes.empty()) { return; } @@ -345,7 +345,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const } const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); if (!terminal_path) { RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); return prev_module_output_; @@ -636,15 +636,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getGoalPose(); } - Pose back_pose; - const auto back_point = - lanelet::utils::conversion::toGeomMsgPt(next_lane.centerline2d().back()); - const double front_yaw = lanelet::utils::getLaneletAngle(next_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - return back_pose; + return utils::to_geom_msg_pose(next_lane.centerline2d().back(), next_lane); }); const auto dist_to_target_pose = @@ -689,8 +681,8 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const +lanelet::ConstLanelets NormalLaneChange::get_lane_change_lanes( + const lanelet::ConstLanelets & current_lanes) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { @@ -699,34 +691,21 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( // Get lane change lanes const auto & route_handler = getRouteHandler(); - const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( - *getRouteHandler(), current_lanes, type_, direction); + const auto lane_change_lane = + utils::lane_change::get_lane_change_target_lane(common_data_ptr_, current_lanes); if (!lane_change_lane) { return {}; } - const auto front_pose = std::invoke([&lane_change_lane]() { - const auto & p = lane_change_lane->centerline().front(); - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); - const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); - geometry_msgs::msg::Pose front_pose; - front_pose.position = front_point; - tf2::Quaternion quat; - quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(quat); - return front_pose; - }); - const auto forward_length = std::invoke([&]() { + const auto front_pose = + utils::to_geom_msg_pose(lane_change_lane->centerline().front(), *lane_change_lane); const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); const auto forward_path_length = planner_data_->parameters.forward_path_length; - if (signed_distance <= 0.0) { - return forward_path_length; - } - - return signed_distance + forward_path_length; + return forward_path_length + std::max(signed_distance, 0.0); }); + const auto backward_length = lane_change_parameters_->backward_lane_length; return route_handler->getLaneletSequence( 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 b3c0dd95a3d96..cdf21e2f3d8cb 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 @@ -78,6 +78,12 @@ rclcpp::Logger get_logger() return logger; } +bool is_mandatory_lane_change(const ModuleType lc_type) +{ + return lc_type == LaneChangeModuleType::NORMAL || + lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE; +} + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity) { @@ -189,14 +195,13 @@ lanelet::ConstLanelets getTargetNeighborLanes( lanelet::ConstLanelets neighbor_lanes; for (const auto & current_lane : current_lanes) { + const auto mandatory_lane_change = is_mandatory_lane_change(type); if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { + if (mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } else { - if (type != LaneChangeModuleType::NORMAL) { + if (!mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } @@ -662,17 +667,16 @@ CandidateOutput assignToCandidate( return candidate_output; } -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction) +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { - return route_handler.getLaneChangeTarget(current_lanes, direction); + const auto direction = common_data_ptr->direction; + const auto route_handler_ptr = common_data_ptr->route_handler_ptr; + if (is_mandatory_lane_change(common_data_ptr->lc_type)) { + return route_handler_ptr->getLaneChangeTarget(current_lanes, direction); } - return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); + return route_handler_ptr->getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } std::vector convert_to_predicted_path( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 03e6c2d7f8167..04b33e291d945 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -106,6 +106,33 @@ FrenetPoint convertToFrenetPoint( std::vector getIds(const lanelet::ConstLanelets & lanelets); +/** + * @brief Converts a Lanelet point to a ROS Pose message. + * + * This function converts a point from a Lanelet map to a ROS geometry_msgs::msg::Pose. + * It sets the position from the point and calculates the orientation (yaw) based on the target + * lane. + * + * @tparam LaneletPointType The type of the input point. + * + * @param[in] src_point The point to convert. + * @param[in] target_lane The lanelet used to determine the orientation. + * + * @return A Pose message with the position and orientation of the point. + */ +template +Pose to_geom_msg_pose(const LaneletPointType & src_point, const lanelet::ConstLanelet & target_lane) +{ + const auto point = lanelet::utils::conversion::toGeomMsgPt(src_point); + const auto yaw = lanelet::utils::getLaneletAngle(target_lane, point); + geometry_msgs::msg::Pose pose; + pose.position = point; + tf2::Quaternion quat; + quat.setRPY(0, 0, yaw); + pose.orientation = tf2::toMsg(quat); + return pose; +} + // distance (arclength) calculation double l2Norm(const Vector3 vector); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 942418412c2a5..929d3ef5894ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -982,25 +982,8 @@ double getArcLengthToTargetLanelet( const auto target_center_line = target_lane.centerline().basicLineString(); - Pose front_pose, back_pose; - - { - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.front()); - const double front_yaw = lanelet::utils::getLaneletAngle(target_lane, front_point); - front_pose.position = front_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(tf_quat); - } - - { - const auto back_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.back()); - const double back_yaw = lanelet::utils::getLaneletAngle(target_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, back_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - } + const auto front_pose = to_geom_msg_pose(target_center_line.front(), target_lane); + const auto back_pose = to_geom_msg_pose(target_center_line.back(), target_lane); const auto arc_front = lanelet::utils::getArcCoordinates(lanelet_sequence, front_pose); const auto arc_back = lanelet::utils::getArcCoordinates(lanelet_sequence, back_pose); From f76dac06f59939ee35d1d5f59d629bacbc1e1d54 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 09/29] 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 1397a9ca5765a..640da2a2fc375 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; @@ -210,7 +212,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 41cef16733e54..8379b32eca89b 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 @@ -273,6 +273,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 namespace autoware::behavior_path_planner::utils::lane_change::debug 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 23ae850189e2a..817c42cee5440 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 @@ -149,7 +149,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 af9a401fa1e80..90f23fd4fb0b8 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); @@ -2181,9 +2159,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 cdf21e2f3d8cb..188f102bc27a6 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 @@ -1285,6 +1285,71 @@ 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 namespace autoware::behavior_path_planner::utils::lane_change::debug From 33a22137ca0e75fc5fc628de7d3a884e2d160f38 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 17 Oct 2024 11:07:39 +0900 Subject: [PATCH 10/29] refactor(lane_change): reducing clang-tidy warnings (#9085) * refactor(lane_change): reducing clang-tidy warnings Signed-off-by: Zulfaqar Azmi * change function name to snake case Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 4 +- .../utils/calculation.hpp | 2 +- .../utils/utils.hpp | 7 +-- .../src/scene.cpp | 22 ++++---- .../src/utils/utils.cpp | 50 +++---------------- 5 files changed, 21 insertions(+), 64 deletions(-) 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 640da2a2fc375..74ca91131966b 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 @@ -126,8 +126,8 @@ class NormalLaneChange : public LaneChangeBase std::vector calc_prepare_durations() const; - lane_change::TargetObjects getTargetObjects( - const FilteredByLanesExtendedObjects & predicted_objects, + lane_change::TargetObjects get_target_objects( + const FilteredByLanesExtendedObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const; FilteredByLanesExtendedObjects filterObjects() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index ed6c48d4a332e..08b4f809c464e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -140,7 +140,7 @@ std::vector calc_prepare_phase_metrics( std::vector calc_shift_phase_metrics( const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, - const double max_velocity, const double lon_accel, + const double max_path_velocity, const double lon_accel, const double max_length_threshold = std::numeric_limits::max()); /** 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 8379b32eca89b..411c71ecc4d61 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 @@ -84,13 +84,8 @@ std::vector replaceWithSortedIds( std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); -lanelet::ConstLanelets getTargetPreferredLanes( +lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction & direction, - const LaneChangeModuleType & type); - -lanelet::ConstLanelets getTargetNeighborLanes( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const LaneChangeModuleType & type); bool isPathInLanelets( 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 90f23fd4fb0b8..5f17c907bf4a9 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 @@ -44,7 +44,6 @@ namespace autoware::behavior_path_planner using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::create_lanes_polygon; using utils::path_safety_checker::isPolygonOverlapLanelet; -using utils::traffic_light::getDistanceToNextTrafficLight; namespace calculation = utils::lane_change::calculation; NormalLaneChange::NormalLaneChange( @@ -89,7 +88,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->current_lanes_path = route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes( + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); common_data_ptr_->lanes_ptr->current_lane_in_goal_section = @@ -558,7 +557,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const get_target_lanes(), getEgoPose(), &closest_lanelet)) { return prev_module_output_.reference_path; } - const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); + auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); if (reference_path.points.empty()) { return prev_module_output_.reference_path; } @@ -1012,7 +1011,7 @@ bool NormalLaneChange::get_prepare_segment( return true; } -lane_change::TargetObjects NormalLaneChange::getTargetObjects( +lane_change::TargetObjects NormalLaneChange::get_target_objects( const FilteredByLanesExtendedObjects & filtered_objects, [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { @@ -1362,7 +1361,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto current_velocity = getEgoVelocity(); const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); + const auto target_objects = get_target_objects(filtered_objects_, current_lanes); const auto prepare_phase_metrics = get_prepare_metrics(); @@ -1652,7 +1651,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); // remove terminal points because utils::clipPathLength() calculates extra long path reference_segment.points.pop_back(); - reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; + reference_segment.points.back().point.longitudinal_velocity_mps = + static_cast(minimum_lane_changing_velocity); const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, @@ -1671,7 +1671,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {true, true}; } - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); + const auto target_objects = get_target_objects(filtered_objects_, current_lanes); CollisionCheckDebugMap debug_data; @@ -1758,11 +1758,7 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const } // check relative angle - if (!utils::checkPathRelativeAngle(path, M_PI)) { - return false; - } - - return true; + return utils::checkPathRelativeAngle(path, M_PI); } bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) @@ -1907,7 +1903,7 @@ bool NormalLaneChange::calcAbortPath() PathWithLaneId aborting_path; aborting_path.points.insert( aborting_path.points.begin(), shifted_path.path.points.begin(), - shifted_path.path.points.begin() + abort_return_idx); + shifted_path.path.points.begin() + static_cast(abort_return_idx)); if (!reference_lane_segment.points.empty()) { abort_path.path = utils::combinePath(aborting_path, reference_lane_segment); 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 188f102bc27a6..6d80591e98b8c 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 @@ -14,10 +14,8 @@ #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" -#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" #include "autoware/behavior_path_lane_change_module/utils/path.hpp" -#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -52,7 +50,6 @@ #include #include #include -#include #include #include #include @@ -61,7 +58,6 @@ namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; @@ -117,10 +113,9 @@ void setPrepareVelocity( { if (current_velocity < prepare_velocity) { // acceleration - for (size_t i = 0; i < prepare_segment.points.size(); ++i) { - prepare_segment.points.at(i).point.longitudinal_velocity_mps = std::min( - prepare_segment.points.at(i).point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); + for (auto & point : prepare_segment.points) { + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); } } else { // deceleration @@ -142,7 +137,7 @@ std::vector getAccelerationValues( } constexpr double epsilon = 0.001; - const auto resolution = std::abs(max_acc - min_acc) / sampling_num; + const auto resolution = std::abs(max_acc - min_acc) / static_cast(sampling_num); std::vector sampled_values{min_acc}; for (double sampled_acc = min_acc + resolution; @@ -159,36 +154,7 @@ std::vector getAccelerationValues( return sampled_values; } -lanelet::ConstLanelets getTargetPreferredLanes( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction & direction, - const LaneChangeModuleType & type) -{ - if (type != LaneChangeModuleType::NORMAL) { - return target_lanes; - } - - const auto target_lane = - utils::lane_change::getLaneChangeTargetLane(route_handler, current_lanes, type, direction); - if (!target_lane) { - return target_lanes; - } - - const auto itr = std::find_if( - target_lanes.begin(), target_lanes.end(), - [&](const lanelet::ConstLanelet & lane) { return lane.id() == target_lane->id(); }); - - if (itr == target_lanes.end()) { - return target_lanes; - } - - const int target_id = std::distance(target_lanes.begin(), itr); - const lanelet::ConstLanelets target_preferred_lanes( - target_lanes.begin() + target_id, target_lanes.end()); - return target_preferred_lanes; -} - -lanelet::ConstLanelets getTargetNeighborLanes( +lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type) { @@ -459,7 +425,7 @@ std::vector generateDrivableLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) { const auto has_same_lane = - [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { + [](const lanelet::ConstLanelets & lanes, const lanelet::ConstLanelet & lane) { if (lanes.empty()) return false; const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end(); @@ -784,7 +750,7 @@ bool isParkedObject( most_left_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); if (sub_type.value() == "road_shoulder") { most_left_lanelet = ll; } @@ -805,7 +771,7 @@ bool isParkedObject( most_right_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); if (sub_type.value() == "road_shoulder") { most_right_lanelet = ll; } From 983e95e391139bb34a89e58e355153ab11ebd680 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 7 Nov 2024 11:48:00 +0900 Subject: [PATCH 11/29] fix(lane_change): enable cancel when ego in turn direction lane (#9124) * RT0-33893 add checks from prev intersection Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi * fix shadow variable Signed-off-by: Zulfaqar Azmi * fix logic Signed-off-by: Zulfaqar Azmi * update readme Signed-off-by: Zulfaqar Azmi * refactor get_ego_footprint Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../README.md | 1 + .../config/lane_change.param.yaml | 1 + .../scene.hpp | 3 + .../utils/data_structs.hpp | 10 ++ .../utils/utils.hpp | 59 ++++++++++-- .../src/manager.cpp | 2 + .../src/scene.cpp | 94 ++++++++++++++++--- .../src/utils/utils.cpp | 86 +++++++++++++++-- 8 files changed, 226 insertions(+), 30 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 a52d842b5dcb9..3b23dd8b4212b 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 @@ -823,6 +823,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `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 | | `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | | `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | | `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | 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 bdde481cb6d25..e75f82459343f 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 @@ -6,6 +6,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] lane_changing_lateral_jerk: 0.5 # [m/s3] 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 74ca91131966b..3b921639d3b18 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 @@ -223,6 +223,9 @@ class NormalLaneChange : public LaneChangeBase return common_data_ptr_->lanes_ptr->target; } + void update_dist_from_intersection(); + + std::vector path_after_intersection_; double stop_time_{0.0}; static constexpr double floating_err_th{1e-3}; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 6b4b7c4f2dc45..f3746a67784ed 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -39,6 +39,7 @@ using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; using route_handler::Direction; using route_handler::RouteHandler; +using universe_utils::Polygon2d; using utils::path_safety_checker::ExtendedPredictedObjects; struct LateralAccelerationMap @@ -110,6 +111,7 @@ struct Parameters // lane change 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}; double lane_changing_lateral_jerk{0.5}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; @@ -220,6 +222,7 @@ struct Lanes { bool current_lane_in_goal_section{false}; bool target_lane_in_goal_section{false}; + lanelet::ConstLanelet ego_lane; lanelet::ConstLanelets current; lanelet::ConstLanelets target_neighbor; lanelet::ConstLanelets target; @@ -324,6 +327,8 @@ struct MinMaxValue struct TransientData { MinMaxValue acc; // acceleration profile for accelerating lane change path + Polygon2d current_footprint; // ego's polygon at current pose + MinMaxValue lane_changing_length; // lane changing length for a single lane change MinMaxValue current_dist_buffer; // distance buffer computed backward from current lanes' terminal end @@ -332,6 +337,8 @@ struct TransientData double dist_to_terminal_end{ std::numeric_limits::min()}; // distance from ego base link to the current lanes' // terminal end + double dist_from_prev_intersection{std::numeric_limits::max()}; + // terminal end double dist_to_terminal_start{ std::numeric_limits::min()}; // distance from ego base link to the current lanes' // terminal start @@ -345,6 +352,9 @@ struct TransientData bool is_ego_near_current_terminal_start{false}; bool is_ego_stuck{false}; + + bool in_turn_direction_lane{false}; + bool in_intersection{false}; }; using RouteHandlerPtr = std::shared_ptr; 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 411c71ecc4d61..c204b2b51167f 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 @@ -44,6 +44,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker:: using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; +using autoware::universe_utils::LineString2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; @@ -198,13 +199,12 @@ rclcpp::Logger getLogger(const std::string & type); * The footprint is determined by the vehicle's pose and its dimensions, including the distance * from the base to the front and rear ends of the vehicle, as well as its width. * - * @param ego_pose The current pose of the ego vehicle. - * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal - * offset, rear overhang, and width. + * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions + * and pose information. * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ -Polygon2d getEgoCurrentFootprint(const Pose & ego_pose, const VehicleInfo & ego_info); +Polygon2d get_ego_footprint(const Pose & ego_pose, const VehicleInfo & ego_info); Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, bool left); @@ -223,7 +223,7 @@ Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, boo * * @return bool True if the polygon is within the intersection area, false otherwise. */ -bool isWithinIntersection( +bool is_within_intersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); @@ -240,7 +240,8 @@ bool isWithinIntersection( * @return bool True if the polygon is within a lane designated for turning, false if it is within a * straight lane or no turn direction is specified. */ -bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); +bool is_within_turn_direction_lanes( + const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); @@ -310,6 +311,52 @@ double get_min_dist_to_current_lanes_obj( bool has_blocking_target_object( const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, const double stop_arc_length, const PathWithLaneId & path); + +/** + * @brief Checks if the ego vehicle has passed any turn direction within an intersection. + * + * This function determines whether the ego vehicle has exited the intersection and + * turn lane area based on its distance from the previous intersection. It considers + * whether the ego vehicle is currently in an intersection and a turn lane. + * + * @param common_data_ptr Shared pointer to CommonData containing the transient data and + * lane-change parameters required for the distance's comparison. + * + * @return true if the ego vehicle has passed the intersection turn direction, false otherwise. + */ +bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr); + +/** + * @brief Retrieves the predicted paths of an object as 2D line strings. + * + * This function transforms each predicted path of an object into a LineString2d, representing + * a 2D sequence of points. Each point in the path is extracted from the predicted path's + * position and converted to a 2D point. + * + * @param object The predicted object whose paths will be converted into 2D line strings. + * + * @return std::vector A vector of 2D line strings representing the predicted paths + * of the object. + */ +std::vector get_line_string_paths(const ExtendedPredictedObject & object); + +/** + * @brief Determines if there is an object in the turn lane that could overtake the ego vehicle. + * + * This function checks for any trailing objects in the turn lane that may attempt to overtake + * the ego vehicle. The check is only applicable if the ego vehicle is still within a certain + * distance from the previous intersection's turn lane. It evaluates whether any of the predicted + * paths or the initial polygon of trailing objects overlap with the target lane polygon. + * + * @param common_data_ptr Shared pointer to CommonData containing lane and polygon information + * for the ego vehicle. + * @param trailing_objects A collection of predicted objects trailing the ego vehicle. + * + * @return true if there is an object in the turn lane with a potential to overtake, false + * otherwise. + */ +bool has_overtaking_turn_lane_object( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug 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 dd7382d8f7116..7c0dbfb6e9644 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 @@ -177,6 +177,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); p.backward_length_buffer_for_blocking_object = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); + p.backward_length_from_intersection = + getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); p.lane_changing_lateral_jerk = getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); p.lane_change_prepare_duration = 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 5f17c907bf4a9..2998c5860d85c 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 @@ -73,6 +73,13 @@ void NormalLaneChange::update_lanes(const bool is_approved) return; } + lanelet::ConstLanelet current_lane; + if (!common_data_ptr_->route_handler_ptr->getClosestLaneletWithinRoute( + common_data_ptr_->get_ego_pose(), ¤t_lane)) { + return; + } + + common_data_ptr_->lanes_ptr->ego_lane = current_lane; const auto is_same_lanes_with_prev_iteration = utils::lane_change::is_same_lane_with_prev_iteration( common_data_ptr_, current_lanes, target_lanes); @@ -91,6 +98,8 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); + common_data_ptr_->current_lanes_path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); common_data_ptr_->lanes_ptr->current_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(current_lanes.back()); common_data_ptr_->lanes_ptr->target_lane_in_goal_section = @@ -140,6 +149,18 @@ void NormalLaneChange::update_transient_data() transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + transient_data.current_footprint = utils::lane_change::get_ego_footprint( + common_data_ptr_->get_ego_pose(), common_data_ptr_->bpp_param_ptr->vehicle_info); + + const auto & ego_lane = common_data_ptr_->lanes_ptr->ego_lane; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + transient_data.in_intersection = utils::lane_change::is_within_intersection( + route_handler_ptr, ego_lane, transient_data.current_footprint); + transient_data.in_turn_direction_lane = + utils::lane_change::is_within_turn_direction_lanes(ego_lane, transient_data.current_footprint); + + update_dist_from_intersection(); + updateStopTime(); transient_data.is_ego_stuck = is_ego_stuck(); @@ -748,6 +769,10 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + if (common_data_ptr_->transient_data.in_turn_direction_lane) { + return true; + } + const auto nearest_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, @@ -1512,6 +1537,11 @@ bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck; + if (utils::lane_change::has_overtaking_turn_lane_object( + common_data_ptr_, filtered_objects_.target_lane_trailing)) { + throw std::logic_error("Ego is nearby intersection, and there might be overtaking vehicle."); + } + if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, @@ -1675,6 +1705,13 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; + const auto has_overtaking_object = utils::lane_change::has_overtaking_turn_lane_object( + common_data_ptr_, filtered_objects_.target_lane_trailing); + + if (has_overtaking_object) { + return {false, true}; + } + const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); @@ -2185,7 +2222,6 @@ void NormalLaneChange::updateStopTime() bool NormalLaneChange::check_prepare_phase() const { const auto & route_handler = getRouteHandler(); - const auto & vehicle_info = getCommonParam().vehicle_info; const auto check_in_general_lanes = lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; @@ -2198,24 +2234,52 @@ bool NormalLaneChange::check_prepare_phase() const return check_in_general_lanes; } - const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info); + const auto check_in_intersection = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection && + common_data_ptr_->transient_data.in_intersection; - const auto check_in_intersection = std::invoke([&]() { - if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) { - return false; - } + const auto check_in_turns = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns && + common_data_ptr_->transient_data.in_turn_direction_lane; - return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint); - }); + return check_in_intersection || check_in_turns || check_in_general_lanes; +} - const auto check_in_turns = std::invoke([&]() { - if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns) { - return false; - } +void NormalLaneChange::update_dist_from_intersection() +{ + auto & transient_data = common_data_ptr_->transient_data; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; - return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); - }); + if ( + transient_data.in_intersection && transient_data.in_turn_direction_lane && + path_after_intersection_.empty()) { + auto path_after_intersection = route_handler_ptr->getCenterLinePath( + common_data_ptr_->lanes_ptr->target_neighbor, 0.0, std::numeric_limits::max()); + path_after_intersection_ = std::move(path_after_intersection.points); + transient_data.dist_from_prev_intersection = 0.0; + return; + } - return check_in_intersection || check_in_turns || check_in_general_lanes; + if ( + transient_data.in_intersection || transient_data.in_turn_direction_lane || + path_after_intersection_.empty()) { + return; + } + + const auto & path_points = path_after_intersection_; + const auto & front_point = path_points.front().point.pose.position; + const auto & ego_position = common_data_ptr_->get_ego_pose().position; + transient_data.dist_from_prev_intersection = + calcSignedArcLength(path_points, front_point, ego_position); + + if ( + transient_data.dist_from_prev_intersection >= 0.0 && + transient_data.dist_from_prev_intersection <= + common_data_ptr_->lc_param_ptr->backward_length_from_intersection) { + return; + } + + path_after_intersection_.clear(); + transient_data.dist_from_prev_intersection = std::numeric_limits::max(); } } // namespace autoware::behavior_path_planner 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 6d80591e98b8c..94be371555d3b 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 @@ -1019,8 +1019,7 @@ rclcpp::Logger getLogger(const std::string & type) return rclcpp::get_logger("lane_change").get_child(type); } -Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info) +Polygon2d get_ego_footprint(const Pose & ego_pose, const VehicleInfo & ego_info) { const auto base_to_front = ego_info.max_longitudinal_offset_m; const auto base_to_rear = ego_info.rear_overhang_m; @@ -1037,23 +1036,32 @@ Point getEgoFrontVertex( return autoware::universe_utils::calcOffsetPose(ego_pose, lon_offset, lat_offset, 0.0).position; } -bool isWithinIntersection( +bool is_within_intersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) { const std::string id = lanelet.attributeOr("intersection_area", "else"); - if (id == "else") { + if (id == "else" || !std::atoi(id.c_str())) { return false; } - const auto lanelet_polygon = - route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + if (!route_handler || !route_handler->getLaneletMapPtr()) { + return false; + } + + const auto & polygon_layer = route_handler->getLaneletMapPtr()->polygonLayer; + const auto lanelet_polygon_opt = polygon_layer.find(std::atoi(id.c_str())); + if (lanelet_polygon_opt == polygon_layer.end()) { + return false; + } + const auto & lanelet_polygon = *lanelet_polygon_opt; return boost::geometry::within( polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); } -bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) +bool is_within_turn_direction_lanes( + const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) { const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); if (turn_direction == "else" || turn_direction == "straight") { @@ -1148,9 +1156,9 @@ bool is_ahead_of_ego( return dist_to_base_link >= 0.0; } - const auto ego_polygon = getEgoCurrentFootprint(current_ego_pose, ego_info).outer(); + const auto & current_footprint = common_data_ptr->transient_data.current_footprint.outer(); auto ego_min_dist_to_end = std::numeric_limits::max(); - for (const auto & ego_edge_point : ego_polygon) { + for (const auto & ego_edge_point : current_footprint) { const auto ego_edge = autoware::universe_utils::createPoint(ego_edge_point.x(), ego_edge_point.y(), 0.0); const auto dist_to_end = autoware::motion_utils::calcSignedArcLength( @@ -1316,6 +1324,66 @@ bool has_blocking_target_object( return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; }); } + +bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr) +{ + const auto & transient_data = common_data_ptr->transient_data; + if (transient_data.in_intersection && transient_data.in_turn_direction_lane) { + return false; + } + + return transient_data.dist_from_prev_intersection > + common_data_ptr->lc_param_ptr->backward_length_from_intersection; +} + +std::vector get_line_string_paths(const ExtendedPredictedObject & object) +{ + const auto to_linestring_2d = [](const auto & predicted_path) -> LineString2d { + LineString2d line_string; + const auto & path = predicted_path.path; + line_string.reserve(path.size()); + for (const auto & path_point : path) { + const auto point = universe_utils::fromMsg(path_point.pose.position).to_2d(); + line_string.push_back(point); + } + + return line_string; + }; + + const auto paths = object.predicted_paths; + std::vector line_strings; + std::transform(paths.begin(), paths.end(), std::back_inserter(line_strings), to_linestring_2d); + + return line_strings; +} + +bool has_overtaking_turn_lane_object( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects) +{ + // Note: This situation is only applicable if the ego is in a turn lane. + if (has_passed_intersection_turn_direction(common_data_ptr)) { + return false; + } + + const auto is_path_overlap_with_target = [&](const LineString2d & path) { + return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target.value()); + }; + + const auto is_object_overlap_with_target = [&](const auto & object) { + // to compensate for perception issue, or if object is from behind ego, and tries to overtake, + // but stop all of sudden + if (!boost::geometry::disjoint( + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->current.value())) { + return true; + } + + const auto paths = get_line_string_paths(object); + return std::any_of(paths.begin(), paths.end(), is_path_overlap_with_target); + }; + + return std::any_of( + trailing_objects.begin(), trailing_objects.end(), is_object_overlap_with_target); +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug From 11f21b358e695b58f5343c085005d78ab5c3543e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 20 Dec 2024 01:54:09 +0000 Subject: [PATCH 12/29] style(pre-commit): autofix --- .../behavior_path_lane_change_module/utils/data_structs.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index f3746a67784ed..f8391e82bf338 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -326,7 +326,7 @@ struct MinMaxValue struct TransientData { - MinMaxValue acc; // acceleration profile for accelerating lane change path + MinMaxValue acc; // acceleration profile for accelerating lane change path Polygon2d current_footprint; // ego's polygon at current pose MinMaxValue lane_changing_length; // lane changing length for a single lane change From 03d49c5e3917c6d4d1e07ea6e89c33acb453cbbc Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Mon, 11 Nov 2024 11:53:36 +0900 Subject: [PATCH 13/29] fix(lane_change): correct computation of maximum lane changing length threshold (#9279) fix computation of maximum lane changing length threshold Signed-off-by: mohammad alqudah --- .../src/scene.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) 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 2998c5860d85c..03e7419215f7c 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 @@ -1341,7 +1341,6 @@ std::vector NormalLaneChange::get_lane_changing_metrics( const double shift_length, const double dist_to_reg_element) const { const auto & route_handler = getRouteHandler(); - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto & transient_data = common_data_ptr_->transient_data; const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, @@ -1352,12 +1351,8 @@ std::vector NormalLaneChange::get_lane_changing_metrics( transient_data.is_ego_near_current_terminal_start ? transient_data.dist_to_terminal_end - prep_metric.length : std::min(transient_data.dist_to_terminal_end, dist_to_reg_element) - prep_metric.length; - auto target_lane_buffer = lane_change_parameters_->lane_change_finish_judge_buffer + - transient_data.next_dist_buffer.min; - if (std::abs(route_handler->getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { - target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; - } - max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + max_length = + std::min(max_length, dist_lc_start_to_end_of_lanes - transient_data.next_dist_buffer.min); return max_length; }); From 39a6625f75fb09648279ddffc4ebddb53686cff5 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 11 Nov 2024 17:23:53 +0900 Subject: [PATCH 14/29] fix(lane_change): extending lane change path for multiple lane change (RT1-8427) (#9268) * RT1-8427 extending lc path for multiple lc Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.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> --- .../src/scene.cpp | 20 +++++-------------- 1 file changed, 5 insertions(+), 15 deletions(-) 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 03e7419215f7c..e799c05f6c393 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 @@ -589,13 +589,6 @@ std::optional NormalLaneChange::extendPath() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto path = status_.lane_change_path.path; - const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; - - const auto dist = calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); - - if (dist < 0.0) { - return std::nullopt; - } auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto & transient_data = common_data_ptr_->transient_data; @@ -607,22 +600,21 @@ std::optional NormalLaneChange::extendPath() forward_path_length) { return std::nullopt; } + const auto dist_to_end_of_path = + lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; - const auto is_goal_in_target = getRouteHandler()->isInGoalRouteSection(target_lanes.back()); - - if (is_goal_in_target) { + if (common_data_ptr_->lanes_ptr->target_lane_in_goal_section) { const auto goal_pose = getRouteHandler()->getGoalPose(); const auto dist_to_goal = lanelet::utils::getArcCoordinates(target_lanes, goal_pose).length; - const auto dist_to_end_of_path = - lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; return getRouteHandler()->getCenterLinePath(target_lanes, dist_to_end_of_path, dist_to_goal); } lanelet::ConstLanelet next_lane; if (!getRouteHandler()->getNextLaneletWithinRoute(target_lanes.back(), &next_lane)) { - return std::nullopt; + return getRouteHandler()->getCenterLinePath( + target_lanes, dist_to_end_of_path, transient_data.target_lane_length); } target_lanes.push_back(next_lane); @@ -638,8 +630,6 @@ std::optional NormalLaneChange::extendPath() const auto dist_to_target_pose = lanelet::utils::getArcCoordinates(target_lanes, target_pose).length; - const auto dist_to_end_of_path = - lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; return getRouteHandler()->getCenterLinePath( target_lanes, dist_to_end_of_path, dist_to_target_pose); From 114f8d66c335bc48fa3eae84b81587ea68a893cf Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 12 Nov 2024 13:17:04 +0900 Subject: [PATCH 15/29] refactor(lane_change): remove std::optional from lanes polygon (#9288) Signed-off-by: Zulfaqar Azmi --- .../utils/data_structs.hpp | 6 ++-- .../utils/utils.hpp | 7 ++--- .../src/scene.cpp | 20 ++++++------- .../src/utils/utils.cpp | 30 +++++++++---------- 4 files changed, 31 insertions(+), 32 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index f8391e82bf338..513fc2fd29da1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -311,9 +311,9 @@ struct PathSafetyStatus struct LanesPolygon { - std::optional current; - std::optional target; - std::optional expanded_target; + lanelet::BasicPolygon2d current; + lanelet::BasicPolygon2d target; + lanelet::BasicPolygon2d expanded_target; lanelet::BasicPolygon2d target_neighbor; std::vector preceding_target; }; 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 c204b2b51167f..d55c903f2516e 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 @@ -150,16 +150,15 @@ std::optional getLeadingStaticObjectIdx( const std::vector & objects, const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); -std::optional createPolygon( +lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); -bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, - const std::optional & lanes_polygon); +bool is_collided_polygons_in_lanelet( + const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon); /** * @brief Generates expanded lanelets based on the given direction and offsets. 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 e799c05f6c393..a6dd8f70828e0 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 @@ -506,7 +506,7 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr); const auto distance_to_last_fit_width = std::invoke([&]() -> double { - const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; 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); @@ -724,7 +724,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const if (has_passed_end_pose) { const auto & lanes_polygon = common_data_ptr_->lanes_polygon_ptr->target; return !boost::geometry::disjoint( - lanes_polygon.value(), + lanes_polygon, lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(current_pose.position))); } @@ -751,7 +751,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } - const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; if (!utils::isEgoWithinOriginalLane( curr_lanes_poly, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { @@ -827,7 +827,7 @@ bool NormalLaneChange::isAbleToStopSafely() const const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); @@ -1166,7 +1166,7 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( const auto & route_handler = getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto check_optional_polygon = [](const auto & object, const auto & polygon) { - return polygon && isPolygonOverlapLanelet(object, *polygon); + return !polygon.empty() && isPolygonOverlapLanelet(object, polygon); }; // get backward lanes @@ -2043,7 +2043,7 @@ bool NormalLaneChange::is_collided( const auto & current_polygon = lanes_polygon_ptr->current; const auto & expanded_target_polygon = lanes_polygon_ptr->target; - if (!current_polygon.has_value() || !expanded_target_polygon.has_value()) { + if (current_polygon.empty() || expanded_target_polygon.empty()) { return !is_collided; } @@ -2083,9 +2083,9 @@ bool NormalLaneChange::is_collided( } const auto collision_in_current_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_polygon); - const auto collision_in_target_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); + utils::lane_change::is_collided_polygons_in_lanelet(collided_polygons, current_polygon); + const auto collision_in_target_lanes = utils::lane_change::is_collided_polygons_in_lanelet( + collided_polygons, expanded_target_polygon); if (!collision_in_current_lanes && !collision_in_target_lanes) { utils::path_safety_checker::updateCollisionCheckDebugMap( @@ -2171,7 +2171,7 @@ bool NormalLaneChange::is_valid_start_point( const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; - const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target.value(); + const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target; return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || boost::geometry::covered_by(lc_start_point, target_lane_poly); 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 94be371555d3b..a88333b4031f8 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 @@ -951,12 +951,13 @@ std::optional getLeadingStaticObjectIdx( return leading_obj_idx; } -std::optional createPolygon( +lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) { if (lanes.empty()) { return {}; } + const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist); return lanelet::utils::to2D(polygon_3d).basicPolygon(); } @@ -994,12 +995,11 @@ ExtendedPredictedObject transform( return extended_object; } -bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, - const std::optional & lanes_polygon) +bool is_collided_polygons_in_lanelet( + const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon) { const auto is_in_lanes = [&](const auto & collided_polygon) { - return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); + return !lanes_polygon.empty() && !boost::geometry::disjoint(collided_polygon, lanes_polygon); }; return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); @@ -1078,28 +1078,28 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) LanesPolygon lanes_polygon; lanes_polygon.current = - utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(lanes->current, 0.0, std::numeric_limits::max()); lanes_polygon.target = - utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(lanes->target, 0.0, std::numeric_limits::max()); const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset, lc_param_ptr->lane_expansion_right_offset); - lanes_polygon.expanded_target = utils::lane_change::createPolygon( + lanes_polygon.expanded_target = utils::lane_change::create_polygon( expanded_target_lanes, 0.0, std::numeric_limits::max()); - lanes_polygon.target_neighbor = *utils::lane_change::createPolygon( + lanes_polygon.target_neighbor = utils::lane_change::create_polygon( lanes->target_neighbor, 0.0, std::numeric_limits::max()); lanes_polygon.preceding_target.reserve(lanes->preceding_target.size()); for (const auto & preceding_lane : lanes->preceding_target) { auto lane_polygon = - utils::lane_change::createPolygon(preceding_lane, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(preceding_lane, 0.0, std::numeric_limits::max()); - if (lane_polygon) { - lanes_polygon.preceding_target.push_back(*lane_polygon); + if (!lane_polygon.empty()) { + lanes_polygon.preceding_target.push_back(lane_polygon); } } return lanes_polygon; @@ -1314,7 +1314,7 @@ bool has_blocking_target_object( // 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())) { + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target)) { return false; } @@ -1366,14 +1366,14 @@ bool has_overtaking_turn_lane_object( } const auto is_path_overlap_with_target = [&](const LineString2d & path) { - return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target.value()); + return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target); }; const auto is_object_overlap_with_target = [&](const auto & object) { // to compensate for perception issue, or if object is from behind ego, and tries to overtake, // but stop all of sudden if (!boost::geometry::disjoint( - object.initial_polygon, common_data_ptr->lanes_polygon_ptr->current.value())) { + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->current)) { return true; } From 0b5012061ce91b63888c160afe8040a9af6e10b1 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 22 Nov 2024 16:30:22 +0900 Subject: [PATCH 16/29] refactor(lane_change): separate target lane leading based on obj behavior (#9372) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * separate target lane leading objects based on behavior (RT1-8532) Signed-off-by: Zulfaqar Azmi * fixed overlapped filtering and lanes debug marker Signed-off-by: Zulfaqar Azmi * combine filteredObjects function Signed-off-by: Zulfaqar Azmi * renaming functions and type Signed-off-by: Zulfaqar Azmi * update some logic to check is stopped Signed-off-by: Zulfaqar Azmi * rename expanded to stopped_outside_boundary Signed-off-by: Zulfaqar Azmi * Include docstring Signed-off-by: Zulfaqar Azmi * rename stopped_outside_boundary → stopped_at_bound Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi * spell-check Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../scene.hpp | 7 +- .../utils/base_class.hpp | 2 +- .../utils/data_structs.hpp | 34 ++- .../utils/debug_structs.hpp | 8 +- .../utils/markers.hpp | 4 +- .../utils/utils.hpp | 46 ++- .../package.xml | 1 + .../src/scene.cpp | 275 ++++++------------ .../src/utils/markers.cpp | 52 ++-- .../src/utils/utils.cpp | 139 +++++---- .../path_safety_checker/objects_filtering.hpp | 11 +- .../path_safety_checker/objects_filtering.cpp | 9 +- 12 files changed, 283 insertions(+), 305 deletions(-) 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 3b921639d3b18..2242f6a647c0a 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 @@ -127,16 +127,13 @@ class NormalLaneChange : public LaneChangeBase std::vector calc_prepare_durations() const; lane_change::TargetObjects get_target_objects( - const FilteredByLanesExtendedObjects & filtered_objects, + const FilteredLanesObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const; - FilteredByLanesExtendedObjects filterObjects() const; + FilteredLanesObjects filter_objects() const; void filterOncomingObjects(PredictedObjects & objects) const; - FilteredByLanesObjects filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; - bool get_prepare_segment( PathWithLaneId & prepare_segment, const double prepare_length) const override; 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 aaf9df7144d74..ef11802ca7e0a 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 @@ -275,7 +275,7 @@ class LaneChangeBase std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; lane_change::CommonDataPtr common_data_ptr_{}; - FilteredByLanesExtendedObjects filtered_objects_{}; + FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 513fc2fd29da1..b39ae75bbb78e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -262,25 +262,28 @@ struct Info } }; -template -struct LanesObjects +struct TargetLaneLeadingObjects { - Object current_lane{}; - Object target_lane_leading{}; - Object target_lane_trailing{}; - Object other_lane{}; - - LanesObjects() = default; - LanesObjects( - Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane) - : current_lane(std::move(current_lane)), - target_lane_leading(std::move(target_lane_leading)), - target_lane_trailing(std::move(target_lane_trailing)), - other_lane(std::move(other_lane)) + ExtendedPredictedObjects moving; + ExtendedPredictedObjects stopped; + + // for objects outside of target lanes, but close to its boundaries + ExtendedPredictedObjects stopped_at_bound; + + [[nodiscard]] size_t size() const { + return moving.size() + stopped.size() + stopped_at_bound.size(); } }; +struct FilteredLanesObjects +{ + ExtendedPredictedObjects others; + ExtendedPredictedObjects current_lane; + ExtendedPredictedObjects target_lane_trailing; + TargetLaneLeadingObjects target_lane_leading; +}; + struct TargetObjects { ExtendedPredictedObjects leading; @@ -416,8 +419,7 @@ using LaneChangeStates = lane_change::States; using LaneChangePhaseInfo = lane_change::PhaseInfo; using LaneChangePhaseMetrics = lane_change::PhaseMetrics; using LaneChangeInfo = lane_change::Info; -using FilteredByLanesObjects = lane_change::LanesObjects>; -using FilteredByLanesExtendedObjects = lane_change::LanesObjects; +using FilteredLanesObjects = lane_change::FilteredLanesObjects; using LateralAccelerationMap = lane_change::LateralAccelerationMap; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp index fc51a82a8a842..a28b8523a75c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -35,7 +35,7 @@ struct Debug LaneChangePaths valid_paths; CollisionCheckDebugMap collision_check_objects; CollisionCheckDebugMap collision_check_objects_after_approval; - FilteredByLanesExtendedObjects filtered_objects; + FilteredLanesObjects filtered_objects; geometry_msgs::msg::Polygon execution_area; geometry_msgs::msg::Pose ego_pose; lanelet::ConstLanelets current_lanes; @@ -55,9 +55,11 @@ struct Debug collision_check_objects.clear(); collision_check_objects_after_approval.clear(); filtered_objects.current_lane.clear(); - filtered_objects.target_lane_leading.clear(); filtered_objects.target_lane_trailing.clear(); - filtered_objects.other_lane.clear(); + filtered_objects.target_lane_leading.moving.clear(); + filtered_objects.target_lane_leading.stopped.clear(); + filtered_objects.target_lane_leading.stopped_at_bound.clear(); + filtered_objects.others.clear(); execution_area.points.clear(); current_lanes.clear(); target_lanes.clear(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index d403e7e1436c7..c95b388a2e4a4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -29,7 +29,7 @@ namespace marker_utils::lane_change_markers { -using autoware::behavior_path_planner::FilteredByLanesExtendedObjects; +using autoware::behavior_path_planner::FilteredLanesObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; @@ -40,7 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); MarkerArray showFilteredObjects( - const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns); + const FilteredLanesObjects & filtered_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); MarkerArray createDebugMarkerArray( 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 d55c903f2516e..db61a91c831f8 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 @@ -54,6 +54,7 @@ using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; +using behavior_path_planner::lane_change::TargetLaneLeadingObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -154,8 +155,7 @@ lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); ExtendedPredictedObject transform( - const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters); + const PredictedObject & object, const LaneChangeParameters & lane_change_parameters); bool is_collided_polygons_in_lanelet( const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon); @@ -254,17 +254,14 @@ Pose to_pose( bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object); + const ExtendedPredictedObject & object); bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object); + const ExtendedPredictedObject & object); double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose); -ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects); - double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, const bool ignore_intersection = false); @@ -288,7 +285,7 @@ double get_distance_to_next_regulatory_element( * 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 CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects, const double dist_to_target_lane_start, const PathWithLaneId & path); /** @@ -308,8 +305,8 @@ double get_min_dist_to_current_lanes_obj( * otherwise, false. */ bool has_blocking_target_object( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, - const double stop_arc_length, const PathWithLaneId & path); + const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length, + const PathWithLaneId & path); /** * @brief Checks if the ego vehicle has passed any turn direction within an intersection. @@ -356,6 +353,35 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & */ bool has_overtaking_turn_lane_object( const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects); + +/** + * @brief Filters objects based on their positions and velocities relative to the ego vehicle and + * the target lane. + * + * This function evaluates whether an object should be classified as a leading or trailing object + * in the context of a lane change. Objects are filtered based on their lateral distance from + * the ego vehicle, velocity, and whether they are within the target lane or its expanded + * boundaries. + * + * @param common_data_ptr Shared pointer to CommonData containing information about current lanes, + * vehicle dimensions, lane polygons, and behavior parameters. + * @param object An extended predicted object representing a potential obstacle in the environment. + * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the + * current lanes. + * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle. + * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of + * the lane. + * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or + * outside boundaries). + * @param trailing_objects Reference to a collection for storing trailing objects. + * + * @return true if the object is classified as either leading or trailing, false otherwise. + */ +bool filter_target_lane_objects( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, + const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, + const bool before_terminal, TargetLaneLeadingObjects & leading_objects, + ExtendedPredictedObjects & trailing_objects); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index 74d11dbcb1e13..7599817df37da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -26,6 +26,7 @@ autoware_rtc_interface autoware_universe_utils pluginlib + range-v3 rclcpp tier4_planning_msgs visualization_msgs 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 a6dd8f70828e0..69c09b148b6d6 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 @@ -28,6 +28,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -43,8 +47,8 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::create_lanes_polygon; -using utils::path_safety_checker::isPolygonOverlapLanelet; namespace calculation = utils::lane_change::calculation; +using utils::path_safety_checker::filter::velocity_filter; NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, @@ -109,6 +113,18 @@ void NormalLaneChange::update_lanes(const bool is_approved) *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), common_data_ptr_->lc_param_ptr->backward_lane_length); + lane_change_debug_.current_lanes = common_data_ptr_->lanes_ptr->current; + lane_change_debug_.target_lanes = common_data_ptr_->lanes_ptr->target; + + lane_change_debug_.target_backward_lanes.clear(); + ranges::for_each( + common_data_ptr_->lanes_ptr->preceding_target, + [&](const lanelet::ConstLanelets & preceding_lanes) { + ranges::insert( + lane_change_debug_.target_backward_lanes, lane_change_debug_.target_backward_lanes.end(), + preceding_lanes); + }); + *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } @@ -185,7 +201,7 @@ void NormalLaneChange::update_transient_data() void NormalLaneChange::update_filtered_objects() { - filtered_objects_ = filterObjects(); + filtered_objects_ = filter_objects(); } void NormalLaneChange::updateLaneChangeStatus() @@ -560,7 +576,7 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) // [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); + filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path); if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { set_stop_pose(dist_to_terminal_stop, path); @@ -1027,30 +1043,31 @@ bool NormalLaneChange::get_prepare_segment( } lane_change::TargetObjects NormalLaneChange::get_target_objects( - const FilteredByLanesExtendedObjects & filtered_objects, + const FilteredLanesObjects & filtered_objects, [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { - ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; + ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading.moving; + auto insert_leading_objects = [&](const auto & objects) { + ranges::actions::insert(leading_objects, leading_objects.end(), objects); + }; + + insert_leading_objects(filtered_objects.target_lane_leading.stopped); + insert_leading_objects(filtered_objects.target_lane_leading.stopped_at_bound); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { - leading_objects.insert( - leading_objects.end(), filtered_objects.current_lane.begin(), - filtered_objects.current_lane.end()); + insert_leading_objects(filtered_objects.current_lane); } const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; if (chk_obj_in_other_lanes) { - leading_objects.insert( - leading_objects.end(), filtered_objects.other_lane.begin(), - filtered_objects.other_lane.end()); + insert_leading_objects(filtered_objects.others); } return {leading_objects, filtered_objects.target_lane_trailing}; } -FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const +FilteredLanesObjects NormalLaneChange::filter_objects() const { - const auto & route_handler = getRouteHandler(); auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->object_types_to_check); @@ -1065,64 +1082,81 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto & current_lanes = get_current_lanes(); - - if (current_lanes.empty()) { + if (!common_data_ptr_->is_lanes_available()) { return {}; } - const auto & target_lanes = get_target_lanes(); + const auto & current_pose = common_data_ptr_->get_ego_pose(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - if (target_lanes.empty()) { - return {}; - } + const auto & current_lanes_ref_path = common_data_ptr_->current_lanes_path; - const auto & path = common_data_ptr_->current_lanes_path; + const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); + FilteredLanesObjects filtered_objects; + const auto reserve_size = objects.objects.size(); + filtered_objects.current_lane.reserve(reserve_size); + auto & target_lane_leading = filtered_objects.target_lane_leading; + target_lane_leading.stopped.reserve(reserve_size); + target_lane_leading.moving.reserve(reserve_size); + target_lane_leading.stopped_at_bound.reserve(reserve_size); + filtered_objects.target_lane_trailing.reserve(reserve_size); + filtered_objects.others.reserve(reserve_size); - const auto is_within_vel_th = [](const auto & object) -> bool { - constexpr double min_vel_th = 1.0; - constexpr double max_vel_th = std::numeric_limits::max(); - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); - }; + const auto stopped_obj_vel_th = common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; - utils::path_safety_checker::filterObjects( - filtered_by_lanes_objects.target_lane_trailing, - [&](const PredictedObject & object) { return is_within_vel_th(object); }); + for (const auto & object : objects.objects) { + auto ext_object = utils::lane_change::transform(object, *common_data_ptr_->lc_param_ptr); + const auto & ext_obj_pose = ext_object.initial_pose; + ext_object.dist_from_ego = autoware::motion_utils::calcSignedArcLength( + current_lanes_ref_path.points, current_pose.position, ext_obj_pose.position); - if (lane_change_parameters_->check_objects_on_other_lanes) { - utils::path_safety_checker::filterObjects( - filtered_by_lanes_objects.other_lane, [&](const PredictedObject & object) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); - return is_within_vel_th(object) && ahead_of_ego; - }); + const auto is_before_terminal = + utils::lane_change::is_before_terminal(common_data_ptr_, current_lanes_ref_path, ext_object); + + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, ext_object); + + if (utils::lane_change::filter_target_lane_objects( + common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego, + is_before_terminal, target_lane_leading, filtered_objects.target_lane_trailing)) { + 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 && + !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); + continue; + } + + filtered_objects.others.push_back(ext_object); } - // 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); - return is_within_vel_th(object) && ahead_of_ego; - }); + const auto dist_comparator = [](const auto & obj1, const auto & obj2) { + return obj1.dist_from_ego < obj2.dist_from_ego; + }; + + // There are no use cases for other lane objects yet, so to save some computation time, we dont + // have to sort them. + ranges::sort(filtered_objects.current_lane, dist_comparator); + ranges::sort(target_lane_leading.stopped_at_bound, dist_comparator); + ranges::sort(target_lane_leading.stopped, dist_comparator); + ranges::sort(target_lane_leading.moving, dist_comparator); + ranges::sort(filtered_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) { + return !dist_comparator(obj1, obj2); + }); + + lane_change_debug_.filtered_objects = filtered_objects; - const auto target_lane_leading_extended_objects = - utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_leading); - const auto target_lane_trailing_extended_objects = - utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing); - const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.current_lane); - const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.other_lane); - - FilteredByLanesExtendedObjects lane_change_target_objects( - current_lane_extended_objects, target_lane_leading_extended_objects, - target_lane_trailing_extended_objects, other_lane_extended_objects); - lane_change_debug_.filtered_objects = lane_change_target_objects; - return lane_change_target_objects; + return filtered_objects; } void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const @@ -1139,7 +1173,8 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const const auto is_stopped_object = [](const auto & object) -> bool { constexpr double min_vel_th = -0.5; constexpr double max_vel_th = 0.5; - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); + return velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, min_vel_th, max_vel_th); }; utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { @@ -1152,122 +1187,6 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const -{ - std::vector target_lane_leading_objects; - std::vector target_lane_trailing_objects; - std::vector current_lane_objects; - std::vector other_lane_objects; - - const auto & current_pose = getEgoPose(); - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto & route_handler = getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto check_optional_polygon = [](const auto & object, const auto & polygon) { - return !polygon.empty() && isPolygonOverlapLanelet(object, polygon); - }; - - // get backward lanes - const auto & target_backward_lanes = common_data_ptr_->lanes_ptr->preceding_target; - - { - lane_change_debug_.current_lanes = current_lanes; - lane_change_debug_.target_lanes = target_lanes; - - // TODO(Azu) change the type to std::vector - lane_change_debug_.target_backward_lanes.clear(); - std::for_each( - target_backward_lanes.begin(), target_backward_lanes.end(), - [&](const lanelet::ConstLanelets & target_backward_lane) { - lane_change_debug_.target_backward_lanes.insert( - lane_change_debug_.target_backward_lanes.end(), target_backward_lane.begin(), - target_backward_lane.end()); - }); - } - - const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; - const auto dist_ego_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - - const auto reserve_size = objects.objects.size(); - current_lane_objects.reserve(reserve_size); - target_lane_leading_objects.reserve(reserve_size); - target_lane_trailing_objects.reserve(reserve_size); - other_lane_objects.reserve(reserve_size); - - for (const auto & object : objects.objects) { - const auto is_lateral_far = std::invoke([&]() -> bool { - const auto dist_object_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet( - current_lanes, object.kinematics.initial_pose_with_covariance.pose); - const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; - return std::abs(lateral) > (common_parameters.vehicle_width / 2); - }); - - const auto is_before_terminal = [&]() { - return utils::lane_change::is_before_terminal( - common_data_ptr_, current_lanes_ref_path, object); - }; - - if ( - check_optional_polygon(object, lanes_polygon.target) && is_lateral_far && - is_before_terminal()) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); - if (ahead_of_ego) { - target_lane_leading_objects.push_back(object); - } else { - target_lane_trailing_objects.push_back(object); - } - continue; - } - - if ( - check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && - is_before_terminal()) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); - const auto stopped_obj_vel_th = - common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < stopped_obj_vel_th) { - if (ahead_of_ego) { - target_lane_leading_objects.push_back(object); - continue; - } - } - } - - const auto is_overlap_target_backward = std::invoke([&]() -> bool { - const auto check_backward_polygon = [&object](const auto & target_backward_polygon) { - return isPolygonOverlapLanelet(object, target_backward_polygon); - }; - return std::any_of( - lanes_polygon.preceding_target.begin(), lanes_polygon.preceding_target.end(), - check_backward_polygon); - }); - - // check if the object intersects with target backward lanes - if (is_overlap_target_backward) { - target_lane_trailing_objects.push_back(object); - continue; - } - - if (check_optional_polygon(object, lanes_polygon.current)) { - // check only the objects that are in front of the ego vehicle - current_lane_objects.push_back(object); - continue; - } - - other_lane_objects.push_back(object); - } - - return { - current_lane_objects, target_lane_leading_objects, target_lane_trailing_objects, - other_lane_objects}; -} - PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -1529,7 +1448,7 @@ bool NormalLaneChange::check_candidate_path_safety( if ( !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, + common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, lane_change_debug_.collision_check_objects)) { throw std::logic_error( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); @@ -1698,7 +1617,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const } const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index eb64493b0c728..9158a9bd38b23 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -101,36 +101,34 @@ MarkerArray createLaneChangingVirtualWallMarker( } MarkerArray showFilteredObjects( - const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns) + const FilteredLanesObjects & filtered_objects, const std::string & ns) { int32_t update_id = 0; - auto current_marker = marker_utils::showFilteredObjects( - filtered_objects.current_lane, ns, colors::yellow(), update_id); - update_id += static_cast(current_marker.markers.size()); - auto target_leading_marker = marker_utils::showFilteredObjects( - filtered_objects.target_lane_leading, ns, colors::aqua(), update_id); - update_id += static_cast(target_leading_marker.markers.size()); - auto target_trailing_marker = marker_utils::showFilteredObjects( - filtered_objects.target_lane_trailing, ns, colors::blue(), update_id); - update_id += static_cast(target_trailing_marker.markers.size()); - auto other_marker = marker_utils::showFilteredObjects( - filtered_objects.other_lane, ns, colors::medium_orchid(), update_id); - MarkerArray marker_array; - std::move( - current_marker.markers.begin(), current_marker.markers.end(), - std::back_inserter(marker_array.markers)); - std::move( - target_leading_marker.markers.begin(), target_leading_marker.markers.end(), - std::back_inserter(marker_array.markers)); - - std::move( - target_trailing_marker.markers.begin(), target_trailing_marker.markers.end(), - std::back_inserter(marker_array.markers)); - - std::move( - other_marker.markers.begin(), other_marker.markers.end(), - std::back_inserter(marker_array.markers)); + auto reserve_size = filtered_objects.current_lane.size() + filtered_objects.others.size() + + filtered_objects.target_lane_leading.size() + + filtered_objects.target_lane_trailing.size(); + marker_array.markers.reserve(2 * reserve_size); + auto add_objects_to_marker = + [&](const ExtendedPredictedObjects & objects, const ColorRGBA & color) { + if (objects.empty()) { + return; + } + + auto marker = marker_utils::showFilteredObjects(objects, ns, color, update_id); + update_id += static_cast(marker.markers.size()); + std::move( + marker.markers.begin(), marker.markers.end(), std::back_inserter(marker_array.markers)); + }; + + add_objects_to_marker(filtered_objects.current_lane, colors::yellow()); + add_objects_to_marker(filtered_objects.target_lane_leading.moving, colors::aqua()); + add_objects_to_marker(filtered_objects.target_lane_leading.stopped, colors::light_steel_blue()); + add_objects_to_marker(filtered_objects.target_lane_trailing, colors::blue()); + add_objects_to_marker( + filtered_objects.target_lane_leading.stopped_at_bound, colors::light_pink()); + add_objects_to_marker(filtered_objects.others, colors::medium_orchid()); + return marker_array; } 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 a88333b4031f8..2abd07d991e80 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 @@ -25,6 +25,7 @@ #include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include #include #include #include @@ -33,6 +34,7 @@ #include #include #include +#include #include #include @@ -963,9 +965,7 @@ lanelet::BasicPolygon2d create_polygon( } ExtendedPredictedObject transform( - const PredictedObject & object, - [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters) + const PredictedObject & object, const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object(object); @@ -1139,21 +1139,15 @@ Pose to_pose( bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object) + const ExtendedPredictedObject & object) { - const auto & current_ego_pose = common_data_ptr->get_ego_pose(); - - const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; - - const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength( - path.points, current_ego_pose.position, obj_position); const auto & ego_info = common_data_ptr->bpp_param_ptr->vehicle_info; const auto lon_dev = std::max( ego_info.max_longitudinal_offset_m + ego_info.rear_overhang_m, object.shape.dimensions.x); // we don't always have to check the distance accurately. - if (std::abs(dist_to_base_link) > lon_dev) { - return dist_to_base_link >= 0.0; + if (std::abs(object.dist_from_ego) > lon_dev) { + return object.dist_from_ego >= 0.0; } const auto & current_footprint = common_data_ptr->transient_data.current_footprint.outer(); @@ -1166,9 +1160,8 @@ bool is_ahead_of_ego( ego_min_dist_to_end = std::min(dist_to_end, ego_min_dist_to_end); } - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); auto current_min_dist_to_end = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon) { + for (const auto & polygon_p : object.initial_polygon.outer()) { const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( path.points, obj_p, path.points.back().point.pose.position); @@ -1179,7 +1172,7 @@ bool is_ahead_of_ego( bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object) + const ExtendedPredictedObject & object) { const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; const auto & lanes_ptr = common_data_ptr->lanes_ptr; @@ -1188,7 +1181,7 @@ bool is_before_terminal( : path.points.back().point.pose.position; double current_max_dist = std::numeric_limits::lowest(); - const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & obj_position = object.initial_pose.position; const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength(path.points, obj_position, terminal_position); // we don't always have to check the distance accurately. @@ -1196,8 +1189,7 @@ bool is_before_terminal( return dist_to_base_link >= 0.0; } - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - for (const auto & polygon_p : obj_polygon) { + for (const auto & polygon_p : object.initial_polygon.outer()) { const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_obj_to_terminal = autoware::motion_utils::calcSignedArcLength(path.points, obj_p, terminal_position); @@ -1217,22 +1209,6 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose)); } -ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects) -{ - ExtendedPredictedObjects extended_objects; - extended_objects.reserve(objects.size()); - - const auto & bpp_param = *common_data_ptr->bpp_param_ptr; - const auto & lc_param = *common_data_ptr->lc_param_ptr; - std::transform( - objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) { - return utils::lane_change::transform(object, bpp_param, lc_param); - }); - - return extended_objects; -} - double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk, const bool ignore_intersection) @@ -1261,7 +1237,7 @@ double get_distance_to_next_regulatory_element( } double get_min_dist_to_current_lanes_obj( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects, const double dist_to_target_lane_start, const PathWithLaneId & path) { const auto & path_points = path.points; @@ -1301,28 +1277,15 @@ double get_min_dist_to_current_lanes_obj( } bool has_blocking_target_object( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, - const double stop_arc_length, const PathWithLaneId & path) + const TargetLaneLeadingObjects & target_leading_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)) { - 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; - }); + return ranges::any_of(target_leading_objects.stopped, [&](const auto & object) { + 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; + }); } bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr) @@ -1384,6 +1347,70 @@ bool has_overtaking_turn_lane_object( return std::any_of( trailing_objects.begin(), trailing_objects.end(), is_object_overlap_with_target); } + +bool filter_target_lane_objects( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, + const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, + const bool before_terminal, TargetLaneLeadingObjects & leading_objects, + ExtendedPredictedObjects & trailing_objects) +{ + using behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m; + const auto & lanes_polygon = *common_data_ptr->lanes_polygon_ptr; + const auto stopped_obj_vel_th = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold; + + const auto is_lateral_far = std::invoke([&]() -> bool { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, object.initial_pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (vehicle_width / 2); + }); + + const auto is_stopped = velocity_filter( + object.initial_twist, -std::numeric_limits::epsilon(), stopped_obj_vel_th); + if (is_lateral_far && before_terminal) { + const auto in_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target); + if (in_target_lanes) { + if (!ahead_of_ego && !is_stopped) { + trailing_objects.push_back(object); + return true; + } + + if (ahead_of_ego) { + if (is_stopped) { + leading_objects.stopped.push_back(object); + } else { + leading_objects.moving.push_back(object); + } + return true; + } + } + + // Check if the object is positioned outside the lane boundary but still close to its edge. + const auto in_expanded_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.expanded_target); + + if (in_expanded_target_lanes && is_stopped && ahead_of_ego) { + leading_objects.stopped_at_bound.push_back(object); + return true; + } + } + + const auto is_overlap_target_backward = + ranges::any_of(lanes_polygon.preceding_target, [&](const auto & target_backward_polygon) { + return !boost::geometry::disjoint(object.initial_polygon, target_backward_polygon); + }); + + // check if the object intersects with target backward lanes + if (is_overlap_target_backward && !is_stopped) { + trailing_objects.push_back(object); + return true; + } + + return false; +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index f73f989174b54..a10534ddf60bb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -34,8 +34,15 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter using autoware_perception_msgs::msg::PredictedObject; using tier4_planning_msgs::msg::PathPointWithLaneId; -bool velocity_filter( - const PredictedObject & object, double velocity_threshold, double max_velocity); +/** + * @brief Filters object based on velocity. + * + * @param twist The twist of predicted object to filter. + * @param velocity_threshold Lower bound + * @param max_velocity Upper bound + * @return Returns true when the object is within a certain velocity range. + */ +bool velocity_filter(const Twist & object_twist, double velocity_threshold, double max_velocity); bool position_filter( PredictedObject & object, const std::vector & path_points, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 25b307ab2cc4d..be888bba001b9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -26,11 +26,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { -bool velocity_filter(const PredictedObject & object, double velocity_threshold, double max_velocity) +bool velocity_filter(const Twist & object_twist, double velocity_threshold, double max_velocity) { - const auto v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); return (velocity_threshold < v_norm && v_norm < max_velocity); } @@ -146,7 +144,8 @@ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, double velocity_threshold, double max_velocity) { const auto filter = [&](const auto & object) { - return filter::velocity_filter(object, velocity_threshold, max_velocity); + return filter::velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, velocity_threshold, max_velocity); }; auto filtered = objects; From ae4acf2791b355f586b0beed36d6f1a8d66f68ba Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 21 Oct 2024 10:38:39 +0900 Subject: [PATCH 17/29] feat(lane_change): add unit test for normal lane change class (RT1-7970) (#9090) * RT1-7970 testing base class Signed-off-by: Zulfaqar Azmi * additional test Signed-off-by: Zulfaqar Azmi * Added update lanes Signed-off-by: Zulfaqar Azmi * check path generation Signed-off-by: Zulfaqar Azmi * check is lane change required Signed-off-by: Zulfaqar Azmi * fix PRs comment Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../CMakeLists.txt | 1 + .../manager.hpp | 3 + .../utils/base_class.hpp | 3 + .../src/manager.cpp | 19 +- .../test/test_lane_change_scene.cpp | 229 ++++++++++++++++++ .../scene_module_manager_interface.hpp | 1 + .../src/utils/utils.cpp | 13 +- 7 files changed, 261 insertions(+), 8 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt index 509f38d52dd45..194c56cbdc2ba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -18,6 +18,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${PROJECT_NAME} test/test_behavior_path_planner_node_interface.cpp test/test_lane_change_utils.cpp + # test/test_lane_change_scene.cpp ) target_link_libraries(test_${PROJECT_NAME} diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp index 6e6d267dc445f..2a1862de6a27e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp @@ -27,6 +27,7 @@ namespace autoware::behavior_path_planner { +using autoware::behavior_path_planner::lane_change::LCParamPtr; using autoware::route_handler::Direction; class LaneChangeModuleManager : public SceneModuleManagerInterface @@ -44,6 +45,8 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + static LCParamPtr set_params(rclcpp::Node * node, const std::string & node_name); + protected: void initParams(rclcpp::Node * node); 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 ef11802ca7e0a..e5c7fcb6d621e 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 @@ -35,6 +35,7 @@ #include #include +class TestNormalLaneChange; namespace autoware::behavior_path_planner { using autoware::route_handler::Direction; @@ -296,6 +297,8 @@ class LaneChangeBase mutable rclcpp::Clock clock_{RCL_ROS_TIME}; mutable std::shared_ptr time_keeper_; + + friend class ::TestNormalLaneChange; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ 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 7c0dbfb6e9644..04371e8ff3f37 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 @@ -34,7 +34,7 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) initParams(node); } -void LaneChangeModuleManager::initParams(rclcpp::Node * node) +LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::string & node_name) { using autoware::universe_utils::getOrDeclareParameter; @@ -190,7 +190,7 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( - node->get_logger().get_child(name()), + node->get_logger().get_child(node_name), "Lane change buffer must be more than 1 meter. Modifying the buffer."); } @@ -205,7 +205,7 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) lateral_acc_velocity.size() != min_lateral_acc.size() || lateral_acc_velocity.size() != max_lateral_acc.size()) { RCLCPP_ERROR( - node->get_logger().get_child(name()), + node->get_logger().get_child(node_name), "Lane change lateral acceleration map has invalid size."); exit(EXIT_FAILURE); } @@ -261,7 +261,7 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // validation of parameters if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { RCLCPP_FATAL_STREAM( - node->get_logger().get_child(name()), + node->get_logger().get_child(node_name), "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " << p.longitudinal_acc_sampling_num << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl @@ -286,19 +286,24 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.rss_params.longitudinal_velocity_delta_time > p.rss_params_for_abort.longitudinal_velocity_delta_time) { RCLCPP_FATAL_STREAM( - node->get_logger().get_child(name()), + node->get_logger().get_child(node_name), "abort parameter might be loose... Terminating the program..."); exit(EXIT_FAILURE); } } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( - node->get_logger().get_child(name()), + node->get_logger().get_child(node_name), "cancel.delta_time: " << p.cancel.delta_time << ", is too short. This could cause a danger behavior."); } - parameters_ = std::make_shared(p); + return std::make_shared(p); +} + +void LaneChangeModuleManager::initParams(rclcpp::Node * node) +{ + parameters_ = set_params(node, name()); } std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() 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 new file mode 100644 index 0000000000000..01b4c451ebf9e --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -0,0 +1,229 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_lane_change_module/manager.hpp" +#include "autoware/behavior_path_lane_change_module/scene.hpp" +#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware_test_utils/mock_data_parser.hpp" + +#include + +#include + +#include + +using autoware::behavior_path_planner::LaneChangeModuleManager; +using autoware::behavior_path_planner::LaneChangeModuleType; +using autoware::behavior_path_planner::NormalLaneChange; +using autoware::behavior_path_planner::PlannerData; +using autoware::behavior_path_planner::lane_change::CommonDataPtr; +using autoware::behavior_path_planner::lane_change::LCParamPtr; +using autoware::behavior_path_planner::lane_change::RouteHandlerPtr; +using autoware::route_handler::Direction; +using autoware::route_handler::RouteHandler; +using autoware::test_utils::get_absolute_path_to_config; +using autoware::test_utils::get_absolute_path_to_lanelet_map; +using autoware::test_utils::get_absolute_path_to_route; +using autoware_map_msgs::msg::LaneletMapBin; +using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathWithLaneId; + +class TestNormalLaneChange : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + init_param(); + init_module(); + } + + void init_param() + { + auto node_options = get_node_options(); + auto node = rclcpp::Node(name_, node_options); + planner_data_->init_parameters(node); + lc_param_ptr_ = LaneChangeModuleManager::set_params(&node, node.get_name()); + planner_data_->route_handler = init_route_handler(); + + ego_pose_ = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); + planner_data_->self_odometry = set_odometry(ego_pose_); + planner_data_->dynamic_object = + std::make_shared(); + } + + void init_module() + { + normal_lane_change_ = + std::make_shared(lc_param_ptr_, lc_type_, lc_direction_); + normal_lane_change_->setData(planner_data_); + set_previous_approved_path(); + } + + [[nodiscard]] const CommonDataPtr & get_common_data_ptr() const + { + return normal_lane_change_->common_data_ptr_; + } + + [[nodiscard]] rclcpp::NodeOptions get_node_options() const + { + auto node_options = rclcpp::NodeOptions{}; + + const auto common_param = + get_absolute_path_to_config(test_utils_dir_, "test_common.param.yaml"); + const auto nearest_search_param = + get_absolute_path_to_config(test_utils_dir_, "test_nearest_search.param.yaml"); + const auto vehicle_info_param = + get_absolute_path_to_config(test_utils_dir_, "test_vehicle_info.param.yaml"); + + std::string bpp_dir{"autoware_behavior_path_planner"}; + const auto bpp_param = get_absolute_path_to_config(bpp_dir, "behavior_path_planner.param.yaml"); + const auto drivable_area_expansion_param = + get_absolute_path_to_config(bpp_dir, "drivable_area_expansion.param.yaml"); + const auto scene_module_manager_param = + get_absolute_path_to_config(bpp_dir, "scene_module_manager.param.yaml"); + + std::string lc_dir{"autoware_behavior_path_lane_change_module"}; + const auto lc_param = get_absolute_path_to_config(lc_dir, "lane_change.param.yaml"); + + autoware::test_utils::updateNodeOptions( + node_options, {common_param, nearest_search_param, vehicle_info_param, bpp_param, + drivable_area_expansion_param, scene_module_manager_param, lc_param}); + return node_options; + } + + [[nodiscard]] RouteHandlerPtr init_route_handler() const + { + std::string autoware_route_handler_dir{"autoware_route_handler"}; + std::string lane_change_right_test_route_filename{"lane_change_test_route.yaml"}; + std::string lanelet_map_filename{"2km_test.osm"}; + const auto lanelet2_path = + get_absolute_path_to_lanelet_map(test_utils_dir_, lanelet_map_filename); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(lanelet2_path, 5.0); + auto route_handler_ptr = std::make_shared(map_bin_msg); + const auto rh_test_route = + get_absolute_path_to_route(autoware_route_handler_dir, lane_change_right_test_route_filename); + route_handler_ptr->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route)); + + return route_handler_ptr; + } + + static std::shared_ptr set_odometry(const Pose & pose) + { + nav_msgs::msg::Odometry odom; + odom.pose.pose = pose; + return std::make_shared(odom); + } + + void set_previous_approved_path() + { + normal_lane_change_->prev_module_output_.path = create_previous_approved_path(); + } + + [[nodiscard]] PathWithLaneId create_previous_approved_path() const + { + const auto & common_data_ptr = get_common_data_ptr(); + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + lanelet::ConstLanelet closest_lane; + const auto current_pose = planner_data_->self_odometry->pose.pose; + route_handler_ptr->getClosestLaneletWithinRoute(current_pose, &closest_lane); + const auto backward_distance = common_data_ptr->bpp_param_ptr->backward_path_length; + const auto forward_distance = common_data_ptr->bpp_param_ptr->forward_path_length; + const auto current_lanes = route_handler_ptr->getLaneletSequence( + closest_lane, current_pose, backward_distance, forward_distance); + + return route_handler_ptr->getCenterLinePath( + current_lanes, 0.0, std::numeric_limits::max()); + } + + void TearDown() override + { + normal_lane_change_ = nullptr; + lc_param_ptr_ = nullptr; + planner_data_ = nullptr; + rclcpp::shutdown(); + } + + LCParamPtr lc_param_ptr_; + std::shared_ptr normal_lane_change_; + std::shared_ptr planner_data_ = std::make_shared(); + LaneChangeModuleType lc_type_{LaneChangeModuleType::NORMAL}; + Direction lc_direction_{Direction::RIGHT}; + std::string name_{"test_lane_change_scene"}; + std::string test_utils_dir_{"autoware_test_utils"}; + Pose ego_pose_; +}; + +TEST_F(TestNormalLaneChange, testBaseClassInitialize) +{ + const auto type = normal_lane_change_->getModuleType(); + const auto type_str = normal_lane_change_->getModuleTypeStr(); + + ASSERT_EQ(type, LaneChangeModuleType::NORMAL); + const auto is_type_str = type_str == "NORMAL"; + ASSERT_TRUE(is_type_str); + + ASSERT_EQ(normal_lane_change_->getDirection(), Direction::RIGHT); + + ASSERT_TRUE(get_common_data_ptr()); + + ASSERT_TRUE(get_common_data_ptr()->is_data_available()); + ASSERT_FALSE(get_common_data_ptr()->is_lanes_available()); +} + +TEST_F(TestNormalLaneChange, testUpdateLanes) +{ + constexpr auto is_approved = true; + + normal_lane_change_->update_lanes(is_approved); + + ASSERT_FALSE(get_common_data_ptr()->is_lanes_available()); + + normal_lane_change_->update_lanes(!is_approved); + + ASSERT_TRUE(get_common_data_ptr()->is_lanes_available()); +} + +TEST_F(TestNormalLaneChange, testGetPathWhenInvalid) +{ + constexpr auto is_approved = true; + normal_lane_change_->update_lanes(!is_approved); + normal_lane_change_->update_filtered_objects(); + normal_lane_change_->update_transient_data(); + normal_lane_change_->updateLaneChangeStatus(); + const auto & lc_status = normal_lane_change_->getLaneChangeStatus(); + + ASSERT_FALSE(lc_status.is_valid_path); +} + +TEST_F(TestNormalLaneChange, testGetPathWhenValid) +{ + 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(); + normal_lane_change_->update_transient_data(); + const auto lane_change_required = normal_lane_change_->isLaneChangeRequired(); + + ASSERT_TRUE(lane_change_required); + + normal_lane_change_->updateLaneChangeStatus(); + const auto & lc_status = normal_lane_change_->getLaneChangeStatus(); + + ASSERT_TRUE(lc_status.is_valid_path); +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index ceecb7b02408a..60254c692e5d8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -327,6 +327,7 @@ class SceneModuleManagerInterface } } +protected: virtual std::unique_ptr createNewSceneModuleInstance() = 0; rclcpp::Node * node_ = nullptr; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 929d3ef5894ee..3100c409ecab6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -1255,8 +1255,19 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data) { + if (path.points.empty() || !planner_data) { + return {}; + } + const auto & route_handler = planner_data->route_handler; - const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & self_odometry = planner_data->self_odometry; + + if (!route_handler || !self_odometry) { + return {}; + } + + const auto & current_pose = self_odometry->pose.pose; + const auto & p = planner_data->parameters; std::set lane_ids; From e60ed8ab4456cae026de064489a6978cb9ab9a9f Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 23 Oct 2024 11:59:09 +0900 Subject: [PATCH 18/29] refactor(lane_change): refactor longitudinal acceleration sampling (#9091) * fix calc_all_max_lc_lengths function Signed-off-by: mohammad alqudah * remove unused functions Signed-off-by: mohammad alqudah * remove limit on velocity in calc_all_max_lc_lengths function Signed-off-by: mohammad alqudah * sample longitudinal acceleration separately for each prepater duration Signed-off-by: mohammad alqudah * refactor prepare phase metrics calculation Signed-off-by: mohammad alqudah * check for zero value prepare duration Signed-off-by: mohammad alqudah * refactor calc_lon_acceleration_samples function Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../scene.hpp | 11 - .../utils/calculation.hpp | 19 +- .../utils/data_structs.hpp | 4 +- .../utils/utils.hpp | 11 - .../src/scene.cpp | 145 ++--------- .../src/utils/calculation.cpp | 225 ++++++++++++------ .../src/utils/utils.cpp | 48 ---- 7 files changed, 179 insertions(+), 284 deletions(-) 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 2242f6a647c0a..550925c4a10fc 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 @@ -120,12 +120,6 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_terminal_turn_signal_info() const final; - std::vector sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; - - std::vector calc_prepare_durations() const; - lane_change::TargetObjects get_target_objects( const FilteredLanesObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const; @@ -204,11 +198,6 @@ class NormalLaneChange : public LaneChangeBase bool check_prepare_phase() const; - double calcMaximumLaneChangeLength( - const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; - - std::pair calcCurrentMinMaxAcceleration() const; - 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/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 08b4f809c464e..4abe35206fd01 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -27,6 +27,8 @@ using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::MinMaxValue; using behavior_path_planner::lane_change::PhaseMetrics; +static constexpr double eps = 0.001; + double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, const Pose & src_pose); @@ -103,14 +105,6 @@ double calc_ego_dist_to_lanes_start( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -double calc_maximum_lane_change_length( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc); - -double calc_maximum_lane_change_length( - const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, - const double max_acc); - double calc_lane_changing_acceleration( const double initial_lane_changing_velocity, const double max_path_velocity, const double lane_changing_time, const double prepare_longitudinal_acc); @@ -132,10 +126,13 @@ double calc_phase_length( const double velocity, const double maximum_velocity, const double acceleration, const double duration); +std::vector calc_lon_acceleration_samples( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration); + std::vector calc_prepare_phase_metrics( - const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, - const std::vector & lon_accel_values, const double current_velocity, - const double min_length_threshold = 0.0, + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double max_path_velocity, const double min_length_threshold = 0.0, const double max_length_threshold = std::numeric_limits::max()); std::vector calc_shift_phase_metrics( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index b39ae75bbb78e..46c56e176d0d3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -329,7 +329,6 @@ struct MinMaxValue struct TransientData { - MinMaxValue acc; // acceleration profile for accelerating lane change path Polygon2d current_footprint; // ego's polygon at current pose MinMaxValue lane_changing_length; // lane changing length for a single lane change @@ -353,6 +352,9 @@ struct TransientData lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes + size_t current_path_seg_idx; // index of nearest segment to ego along current path + double current_path_velocity; // velocity of the current path at the ego position along the path + bool is_ego_near_current_terminal_start{false}; bool is_ego_stuck{false}; 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 db61a91c831f8..a34b4a390cbb4 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 @@ -66,20 +66,9 @@ bool is_mandatory_lane_change(const ModuleType lc_type); double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); -double calcMinimumAcceleration( - const double current_velocity, const double min_longitudinal_acc, - const LaneChangeParameters & lane_change_parameters); - -double calcMaximumAcceleration( - const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); -std::vector getAccelerationValues( - const double min_acc, const double max_acc, const size_t sampling_num); - std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); 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 69c09b148b6d6..ae1421ec922b7 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 @@ -137,7 +137,15 @@ void NormalLaneChange::update_transient_data() } auto & transient_data = common_data_ptr_->transient_data; - std::tie(transient_data.acc.min, transient_data.acc.max) = calcCurrentMinMaxAcceleration(); + + const auto & p = *common_data_ptr_->bpp_param_ptr; + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_output_.path.points, common_data_ptr_->get_ego_pose(), + p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); + transient_data.current_path_velocity = + prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps; + transient_data.current_path_seg_idx = nearest_seg_idx; std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); @@ -180,8 +188,6 @@ void NormalLaneChange::update_transient_data() updateStopTime(); transient_data.is_ego_stuck = is_ego_stuck(); - RCLCPP_DEBUG( - logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); RCLCPP_DEBUG( logger_, "lane_changing_length - min: %.4f, max: %.4f", transient_data.lane_changing_length.min, transient_data.lane_changing_length.max); @@ -343,11 +349,9 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto terminal_turn_signal_info = get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end); - const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; - const size_t current_nearest_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const auto nearest_dist_threshold = common_param.ego_nearest_dist_threshold; + const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; + const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx; return getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, @@ -890,6 +894,7 @@ bool NormalLaneChange::isAbortState() const lane_change_debug_.is_abort = true; return true; } + int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const { const auto get_opposite_direction = @@ -897,114 +902,6 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } -std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() const -{ - const auto & p = getCommonParam(); - - const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc); - const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc); - - const auto ego_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_module_output_.path.points, getEgoPose(), p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold); - const auto max_path_velocity = - prev_module_output_.path.points.at(ego_seg_idx).point.longitudinal_velocity_mps; - - // calculate minimum and maximum acceleration - const auto min_acc = utils::lane_change::calcMinimumAcceleration( - getEgoVelocity(), vehicle_min_acc, *lane_change_parameters_); - const auto max_acc = utils::lane_change::calcMaximumAcceleration( - getEgoVelocity(), max_path_velocity, vehicle_max_acc, *lane_change_parameters_); - - return {min_acc, max_acc}; -} - -std::vector NormalLaneChange::sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const -{ - // TODO(Azu): sampler should work even when we're not approaching terminal - if (prev_module_output_.path.points.empty()) { - return {}; - } - - const auto & route_handler = *getRouteHandler(); - const auto current_pose = getEgoPose(); - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; - - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - - // if max acc is not positive, then we do the normal sampling - if (max_acc <= 0.0) { - RCLCPP_DEBUG( - logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); - } - - // calculate maximum lane change length - // TODO(Azu) Double check why it's failing with transient data - const auto current_max_dist_buffer = - calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - - if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { - RCLCPP_DEBUG( - logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, - max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); - } - - // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (common_data_ptr_->transient_data.is_ego_stuck) { - auto clock = rclcpp::Clock(RCL_ROS_TIME); - RCLCPP_INFO_THROTTLE( - logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, - max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); - } - - // if maximum lane change length is less than length to goal or the end of target lanes, only - // sample max acc - if (route_handler.isInGoalRouteSection(target_lanes.back())) { - const auto goal_pose = route_handler.getGoalPose(); - if (current_max_dist_buffer < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { - RCLCPP_DEBUG( - logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); - return {max_acc}; - } - } else if (current_max_dist_buffer < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - RCLCPP_DEBUG( - logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); - return {max_acc}; - } - - RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); - return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); -} - -std::vector NormalLaneChange::calc_prepare_durations() const -{ - const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front + - lc_param_ptr->min_length_for_turn_signal_activation; - const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; - - // TODO(Azu) this check seems to cause scenario failures. - if (common_data_ptr_->transient_data.dist_to_terminal_start >= threshold) { - return {max_prepare_duration}; - } - - std::vector prepare_durations; - constexpr double step = 0.5; - - for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { - prepare_durations.push_back(duration); - } - - return prepare_durations; -} - bool NormalLaneChange::get_prepare_segment( PathWithLaneId & prepare_segment, const double prepare_length) const { @@ -1227,22 +1124,14 @@ std::vector NormalLaneChange::get_prepare_metrics() cons const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto current_velocity = getEgoVelocity(); - - // get sampling acceleration values - const auto longitudinal_acc_sampling_values = - sampleLongitudinalAccValues(current_lanes, target_lanes); - - const auto prepare_durations = calc_prepare_durations(); - - RCLCPP_DEBUG( - logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", - prepare_durations.size(), longitudinal_acc_sampling_values.size()); + // set speed limit to be current path velocity; + const auto max_path_velocity = common_data_ptr_->transient_data.current_path_velocity; const auto dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); return calculation::calc_prepare_phase_metrics( - common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, - dist_to_target_start, common_data_ptr_->transient_data.dist_to_terminal_start); + common_data_ptr_, current_velocity, max_path_velocity, dist_to_target_start, + common_data_ptr_->transient_data.dist_to_terminal_start); } std::vector NormalLaneChange::get_lane_changing_metrics( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index b94434f03e80c..e93096dc18314 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -140,59 +140,26 @@ double calc_ego_dist_to_lanes_start( return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); } -double calc_maximum_lane_change_length( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc) +double calc_minimum_acceleration( + const LaneChangeParameters & lane_change_parameters, const double current_velocity, + const double min_acc_threshold, const double prepare_duration) { - if (shift_intervals.empty()) { - return 0.0; - } - - const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto t_prepare = lane_change_parameters.lane_change_prepare_duration; - - auto vel = current_velocity; - - const auto calc_sum = [&](double sum, double shift_interval) { - // prepare section - const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; - vel = vel + max_acc * t_prepare; - - // lane changing section - const auto [min_lat_acc, max_lat_acc] = - lane_change_parameters.lane_change_lat_acc_map.find(vel); - const auto t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); - const auto lane_changing_length = - vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; - - vel = vel + max_acc * t_lane_changing; - return sum + (prepare_length + lane_changing_length + finish_judge_buffer); - }; - - const auto total_length = - std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); - - const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); + if (prepare_duration < eps) return -std::abs(min_acc_threshold); + const double min_lc_velocity = lane_change_parameters.minimum_lane_changing_velocity; + const double acc = (min_lc_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, -std::abs(min_acc_threshold), -eps); } -double calc_maximum_lane_change_length( - const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, - const double max_acc) +double calc_maximum_acceleration( + const double prepare_duration, const double current_velocity, const double current_max_velocity, + const double max_acc_threshold) { - const auto shift_intervals = - common_data_ptr->route_handler_ptr->getLateralIntervalsToPreferredLane( - current_terminal_lanelet); - const auto vel = std::max( - common_data_ptr->get_ego_speed(), - common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity); - return calc_maximum_lane_change_length( - vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc); + if (prepare_duration < eps) return max_acc_threshold; + const double acc = (current_max_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, 0.0, max_acc_threshold); } -std::vector calc_all_min_lc_lengths( +std::vector calc_min_lane_change_lengths( const LCParamPtr & lc_param_ptr, const std::vector & shift_intervals) { if (shift_intervals.empty()) { @@ -219,7 +186,7 @@ std::vector calc_all_min_lc_lengths( return min_lc_lengths; } -std::vector calc_all_max_lc_lengths( +std::vector calc_max_lane_change_lengths( const CommonDataPtr & common_data_ptr, const std::vector & shift_intervals) { if (shift_intervals.empty()) { @@ -229,21 +196,28 @@ std::vector calc_all_max_lc_lengths( const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; const auto t_prepare = lc_param_ptr->lane_change_prepare_duration; - const auto max_acc = common_data_ptr->transient_data.acc.max; + const auto current_velocity = common_data_ptr->get_ego_speed(); + const auto path_velocity = common_data_ptr->transient_data.current_path_velocity; - const auto limit_vel = [&](const auto vel) { - const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; - return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); - }; + const auto max_acc = calc_maximum_acceleration( + t_prepare, current_velocity, path_velocity, lc_param_ptr->max_longitudinal_acc); + + // TODO(Quda, Azu): should probably limit upper bound of velocity as well, but + // disabled due failing evaluation tests. + // const auto limit_vel = [&](const auto vel) { + // const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; + // return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); + // }; - auto vel = limit_vel(common_data_ptr->get_ego_speed()); + auto vel = + std::max(common_data_ptr->get_ego_speed(), lc_param_ptr->minimum_lane_changing_velocity); std::vector max_lc_lengths{}; const auto max_lc_length = [&](const auto shift_interval) { // prepare section - vel = limit_vel(vel + max_acc * t_prepare); const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + vel = vel + max_acc * t_prepare; // lane changing section const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); @@ -252,7 +226,7 @@ std::vector calc_all_max_lc_lengths( const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; - vel = limit_vel(vel + max_acc * t_lane_changing); + vel = vel + max_acc * t_lane_changing; return prepare_length + lane_changing_length; }; @@ -262,17 +236,16 @@ std::vector calc_all_max_lc_lengths( return max_lc_lengths; } -double calc_distance_buffer( - const LCParamPtr & lc_param_ptr, const std::vector & min_lc_lengths) +double calc_distance_buffer(const LCParamPtr & lc_param_ptr, const std::vector & lc_lengths) { - if (min_lc_lengths.empty()) { + if (lc_lengths.empty()) { return std::numeric_limits::max(); } const auto finish_judge_buffer = lc_param_ptr->lane_change_finish_judge_buffer; const auto backward_buffer = calc_stopping_distance(lc_param_ptr); - const auto lengths_sum = std::accumulate(min_lc_lengths.begin(), min_lc_lengths.end(), 0.0); - const auto num_of_lane_changes = static_cast(min_lc_lengths.size()); + const auto lengths_sum = std::accumulate(lc_lengths.begin(), lc_lengths.end(), 0.0); + const auto num_of_lane_changes = static_cast(lc_lengths.size()); return lengths_sum + (num_of_lane_changes * finish_judge_buffer) + ((num_of_lane_changes - 1.0) * backward_buffer); } @@ -297,19 +270,19 @@ std::pair calc_lc_length_and_dist_buffer( return {}; } const auto shift_intervals = calculation::calc_shift_intervals(common_data_ptr, lanes); - const auto all_min_lc_lengths = - calculation::calc_all_min_lc_lengths(common_data_ptr->lc_param_ptr, shift_intervals); + const auto min_lc_lengths = + calculation::calc_min_lane_change_lengths(common_data_ptr->lc_param_ptr, shift_intervals); const auto min_lc_length = - !all_min_lc_lengths.empty() ? all_min_lc_lengths.front() : std::numeric_limits::max(); + !min_lc_lengths.empty() ? min_lc_lengths.front() : std::numeric_limits::max(); const auto min_dist_buffer = - calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_min_lc_lengths); + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, min_lc_lengths); - const auto all_max_lc_lengths = - calculation::calc_all_max_lc_lengths(common_data_ptr, shift_intervals); + const auto max_lc_lengths = + calculation::calc_max_lane_change_lengths(common_data_ptr, shift_intervals); const auto max_lc_length = - !all_max_lc_lengths.empty() ? all_max_lc_lengths.front() : std::numeric_limits::max(); + !max_lc_lengths.empty() ? max_lc_lengths.front() : std::numeric_limits::max(); const auto max_dist_buffer = - calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_max_lc_lengths); + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, max_lc_lengths); return {{min_lc_length, max_lc_length}, {min_dist_buffer, max_dist_buffer}}; } @@ -324,6 +297,85 @@ double calc_phase_length( return std::min(length_with_acceleration, length_with_max_velocity); } +std::pair calc_min_max_acceleration( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration) +{ + const auto & lc_params = *common_data_ptr->lc_param_ptr; + const auto & bpp_params = *common_data_ptr->bpp_param_ptr; + const auto current_ego_velocity = common_data_ptr->get_ego_speed(); + + const auto min_accel_threshold = std::max(bpp_params.min_acc, lc_params.min_longitudinal_acc); + const auto max_accel_threshold = std::min(bpp_params.max_acc, lc_params.max_longitudinal_acc); + + // calculate minimum and maximum acceleration + const auto min_acc = calc_minimum_acceleration( + lc_params, current_ego_velocity, min_accel_threshold, prepare_duration); + const auto max_acc = calc_maximum_acceleration( + prepare_duration, current_ego_velocity, max_path_velocity, max_accel_threshold); + + return {min_acc, max_acc}; +} + +std::vector calc_acceleration_values( + const double min_accel, const double max_accel, const double sampling_num) +{ + if (min_accel > max_accel) return {}; + + if (max_accel - min_accel < eps) { + return {min_accel}; + } + + const auto resolution = std::abs(max_accel - min_accel) / sampling_num; + + std::vector sampled_values{min_accel}; + for (double accel = min_accel + resolution; accel < max_accel + eps; accel += resolution) { + // check whether if we need to add 0.0 + if (sampled_values.back() < -eps && accel > eps) { + sampled_values.push_back(0.0); + } + + sampled_values.push_back(accel); + } + std::reverse(sampled_values.begin(), sampled_values.end()); + + return sampled_values; +} + +std::vector calc_lon_acceleration_samples( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration) +{ + const auto & transient_data = common_data_ptr->transient_data; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); + const auto sampling_num = common_data_ptr->lc_param_ptr->longitudinal_acc_sampling_num; + + const auto [min_accel, max_accel] = + calc_min_max_acceleration(common_data_ptr, max_path_velocity, prepare_duration); + + const auto is_sampling_required = std::invoke([&]() -> bool { + if (max_accel < 0.0 || transient_data.is_ego_stuck) return true; + + const auto max_dist_buffer = transient_data.current_dist_buffer.max; + if (max_dist_buffer > transient_data.dist_to_terminal_end) return true; + + const auto dist_to_target_lane_end = + common_data_ptr->lanes_ptr->target_lane_in_goal_section + ? utils::getSignedDistance(current_pose, goal_pose, target_lanes) + : utils::getDistanceToEndOfLane(current_pose, target_lanes); + + return max_dist_buffer >= dist_to_target_lane_end; + }); + + if (is_sampling_required) { + return calc_acceleration_values(min_accel, max_accel, sampling_num); + } + + return {max_accel}; +} + double calc_lane_changing_acceleration( const double initial_lane_changing_velocity, const double max_path_velocity, const double lane_changing_time, const double prepare_longitudinal_acc) @@ -337,10 +389,32 @@ double calc_lane_changing_acceleration( prepare_longitudinal_acc); } +std::vector calc_prepare_durations(const CommonDataPtr & common_data_ptr) +{ + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto threshold = common_data_ptr->bpp_param_ptr->base_link2front + + lc_param_ptr->min_length_for_turn_signal_activation; + const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; + + // TODO(Azu) this check seems to cause scenario failures. + if (common_data_ptr->transient_data.dist_to_terminal_start >= threshold) { + return {max_prepare_duration}; + } + + std::vector prepare_durations; + constexpr double step = 0.5; + + for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { + prepare_durations.push_back(duration); + } + + return prepare_durations; +} + std::vector calc_prepare_phase_metrics( - const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, - const std::vector & lon_accel_values, const double current_velocity, - const double min_length_threshold, const double max_length_threshold) + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double max_path_velocity, const double min_length_threshold, + const double max_length_threshold) { const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; @@ -358,9 +432,12 @@ std::vector calc_prepare_phase_metrics( return false; }; - metrics.reserve(prepare_durations.size() * lon_accel_values.size()); + const auto prepare_durations = calc_prepare_durations(common_data_ptr); + for (const auto & prepare_duration : prepare_durations) { - for (const auto & lon_accel : lon_accel_values) { + const auto lon_accel_samples = + calc_lon_acceleration_samples(common_data_ptr, max_path_velocity, prepare_duration); + for (const auto & lon_accel : lon_accel_samples) { const auto prepare_velocity = std::clamp(current_velocity + lon_accel * prepare_duration, min_lc_vel, max_vel); 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 2abd07d991e80..bbbc120b8a904 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 @@ -91,25 +91,6 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -double calcMinimumAcceleration( - const double current_velocity, const double min_longitudinal_acc, - const LaneChangeParameters & lane_change_parameters) -{ - const double min_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; - const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const double acc = (min_lane_changing_velocity - current_velocity) / prepare_duration; - return std::clamp(acc, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); -} - -double calcMaximumAcceleration( - const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters) -{ - const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const double acc = (current_max_velocity - current_velocity) / prepare_duration; - return std::clamp(acc, 0.0, max_longitudinal_acc); -} - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { @@ -127,35 +108,6 @@ void setPrepareVelocity( } } -std::vector getAccelerationValues( - const double min_acc, const double max_acc, const size_t sampling_num) -{ - if (min_acc > max_acc) { - return {}; - } - - if (max_acc - min_acc < std::numeric_limits::epsilon()) { - return {min_acc}; - } - - constexpr double epsilon = 0.001; - const auto resolution = std::abs(max_acc - min_acc) / static_cast(sampling_num); - - std::vector sampled_values{min_acc}; - for (double sampled_acc = min_acc + resolution; - sampled_acc < max_acc + std::numeric_limits::epsilon(); sampled_acc += resolution) { - // check whether if we need to add 0.0 - if (sampled_values.back() < -epsilon && sampled_acc > epsilon) { - sampled_values.push_back(0.0); - } - - sampled_values.push_back(sampled_acc); - } - std::reverse(sampled_values.begin(), sampled_values.end()); - - return sampled_values; -} - lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type) From 6115e7b9ba522bcd6fa80f606414981e6119ce65 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 27 Sep 2024 14:59:11 +0900 Subject: [PATCH 19/29] feat(lane_change): modify lane change target boundary check to consider velocity (#8961) * check if candidate path footprint exceeds target lane boundary when lc velocity is above minimum Signed-off-by: mohammad alqudah * move functions to relevant module Signed-off-by: mohammad alqudah * suppress unused function cppcheck Signed-off-by: mohammad alqudah * minor change Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../src/interface.cpp | 3 + .../src/manager.cpp | 4 +- .../src/manager.hpp | 4 +- .../src/scene.cpp | 47 +++++++++++++++- .../utils/utils.hpp | 15 +---- .../src/scene.cpp | 12 ++++ .../src/utils/utils.cpp | 56 +------------------ 7 files changed, 66 insertions(+), 75 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index fffb86767b0a8..d47f76e399b4c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -44,11 +44,14 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( { } +// cppcheck-suppress unusedFunction bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && module_type_->isValidPath(); } + +// cppcheck-suppress unusedFunction void AvoidanceByLaneChangeInterface::processOnEntry() { waitApproval(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index ddcfda50d18c4..040b71b7b380b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -187,8 +187,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) avoidance_parameters_ = std::make_shared(p); } -std::unique_ptr -AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +// cppcheck-suppress unusedFunction +SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp index 95609a430ac80..44f842a67c236 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp @@ -32,6 +32,8 @@ using autoware::behavior_path_planner::LaneChangeModuleManager; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::SceneModuleInterface; +using SMIPtr = std::unique_ptr; + class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager { public: @@ -44,7 +46,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override; + SMIPtr createNewSceneModuleInstance() override; private: std::shared_ptr avoidance_parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index e854a3919c925..a5fdf5a9f0bdf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -34,14 +34,55 @@ #include #include +namespace +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Point32 p; + p.x = static_cast(pose.position.x); + p.y = static_cast(pose.position.y); + p.z = static_cast(pose.position.z); + return p; +}; + +geometry_msgs::msg::Polygon create_execution_area( + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & pose, double additional_lon_offset, double additional_lat_offset) +{ + const double & base_to_front = vehicle_info.max_longitudinal_offset_m; + const double & width = vehicle_info.vehicle_width_m; + const double & base_to_rear = vehicle_info.rear_overhang_m; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + additional_lon_offset; + const double backward_lon_offset = -base_to_rear; + const double lat_offset = width / 2.0 + additional_lat_offset; + + const auto p1 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + geometry_msgs::msg::Polygon polygon; + + polygon.points.push_back(create_point32(p1)); + polygon.points.push_back(create_point32(p2)); + polygon.points.push_back(create_point32(p3)); + polygon.points.push_back(create_point32(p4)); + + return polygon; +} +} // namespace + namespace autoware::behavior_path_planner { using autoware::behavior_path_planner::Direction; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::ObjectInfo; using autoware::behavior_path_planner::Point2d; -using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea; -namespace utils = autoware::behavior_path_planner::utils; AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, @@ -85,7 +126,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); const auto minimum_lane_change_length = calc_minimum_dist_buffer(); - lane_change_debug_.execution_area = createExecutionArea( + lane_change_debug_.execution_area = create_execution_area( getCommonParam().vehicle_info, getEgoPose(), std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); 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 a34b4a390cbb4..29b3bf01ef1fc 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 @@ -83,7 +83,7 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -bool pathFootprintExceedsTargetLaneBound( +bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); @@ -237,10 +237,6 @@ bool is_same_lane_with_prev_iteration( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation); - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const ExtendedPredictedObject & object); @@ -373,13 +369,4 @@ bool filter_target_lane_objects( ExtendedPredictedObjects & trailing_objects); } // namespace autoware::behavior_path_planner::utils::lane_change -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); - -geometry_msgs::msg::Polygon createExecutionArea( - const VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, - double additional_lat_offset); -} // namespace autoware::behavior_path_planner::utils::lane_change::debug - #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ 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 ae1421ec922b7..783246602ae4c 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 @@ -1343,6 +1343,18 @@ bool NormalLaneChange::check_candidate_path_safety( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); } + const auto lc_start_velocity = candidate_path.info.velocity.prepare; + const auto min_lc_velocity = lane_change_parameters_->minimum_lane_changing_velocity; + constexpr double margin = 0.1; + // path is unsafe if it exceeds target lane boundary with a high velocity + if ( + lane_change_parameters_->enable_target_lane_bound_check && + lc_start_velocity > min_lc_velocity + margin && + utils::lane_change::path_footprint_exceeds_target_lane_bound( + common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) { + throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); + } + constexpr size_t decel_sampling_num = 1; const auto safety_check_with_normal_rss = isLaneChangePathSafe( candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num, 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 bbbc120b8a904..b50159555a25f 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 @@ -154,7 +154,7 @@ bool isPathInLanelets( return true; } -bool pathFootprintExceedsTargetLaneBound( +bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin) { @@ -1079,16 +1079,6 @@ bool is_same_lane_with_prev_iteration( (prev_target_lanes.back().id() == prev_target_lanes.back().id()); } -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation) -{ - Pose pose; - pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); - pose.orientation = orientation; - return pose; -} - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const ExtendedPredictedObject & object) @@ -1364,47 +1354,3 @@ bool filter_target_lane_objects( return false; } } // namespace autoware::behavior_path_planner::utils::lane_change - -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Point32 p; - p.x = static_cast(pose.position.x); - p.y = static_cast(pose.position.y); - p.z = static_cast(pose.position.z); - return p; -}; - -geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset) -{ - const double & base_to_front = vehicle_info.max_longitudinal_offset_m; - const double & width = vehicle_info.vehicle_width_m; - const double & base_to_rear = vehicle_info.rear_overhang_m; - - // if stationary object, extend forward and backward by the half of lon length - const double forward_lon_offset = base_to_front + additional_lon_offset; - const double backward_lon_offset = -base_to_rear; - const double lat_offset = width / 2.0 + additional_lat_offset; - - const auto p1 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); - geometry_msgs::msg::Polygon polygon; - - polygon.points.push_back(create_point32(p1)); - polygon.points.push_back(create_point32(p2)); - polygon.points.push_back(create_point32(p3)); - polygon.points.push_back(create_point32(p4)); - - return polygon; -} - -} // namespace autoware::behavior_path_planner::utils::lane_change::debug From 7d080e32f6350b4d4c96c8d73a556f4daeaa9229 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Tue, 26 Nov 2024 14:10:21 +0900 Subject: [PATCH 20/29] refactor(lane_change): refactor lane change parameters (#9403) * refactor lane change parameters Signed-off-by: mohammad alqudah * update lane change param yaml Signed-off-by: mohammad alqudah * update lane change README Signed-off-by: mohammad alqudah * regroup some parameters Signed-off-by: mohammad alqudah * run pre-commit prettier step Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * apply pre-commit checks Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../README.md | 41 +- .../config/lane_change.param.yaml | 51 +-- .../utils/data_structs.hpp | 135 +----- .../utils/parameters.hpp | 169 ++++++++ .../src/manager.cpp | 398 ++++++++---------- .../src/scene.cpp | 89 ++-- .../src/utils/calculation.cpp | 52 +-- .../src/utils/utils.cpp | 18 +- 8 files changed, 473 insertions(+), 480 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp 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 3b23dd8b4212b..6ebe79c180e95 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 @@ -525,8 +525,6 @@ The ego vehicle may need to secure ample inter-vehicle distance ahead of the tar ![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) -The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. - #### If the lane is blocked and multiple lane changes When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance can be computed from shift length and minimum lane changing velocity. @@ -820,25 +818,22 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | Name | Unit | Type | Description | Default value | | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | | `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `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 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `trajectory.prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `trajectory.minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `trajectory.lon_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | +| `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | | `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | | `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | -| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | -| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | -| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | +| `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | +| `lateral_acceleration.max_values` | [m/s2] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | ### Parameter to judge if lane change is completed @@ -891,14 +886,14 @@ The following parameters are used to judge lane change completion. | Name | Unit | Type | Description | Default value | | :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `enable_collision_check_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | -| `enable_collision_check_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | -| `enable_collision_check_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | -| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | -| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | -| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | -| `safety_check.collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | +| `collision_check.enable_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | +| `collision_check.enable_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | +| `collision_check.enable_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | +| `collision_check.check_current_lanes` | [-] | boolean | If true, the lane change module always checks objects in the current lanes for collision assessment. If false, it only checks objects in the current lanes when the ego vehicle is stuck. | false | +| `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false | +| `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### safety constraints during lane change path is computed @@ -910,7 +905,7 @@ The following parameters are used to judge lane change completion. | `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | | `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.execution.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | #### safety constraints specifically for stopped or parked vehicles 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 e75f82459343f..41810ea4a80bf 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 @@ -1,42 +1,35 @@ /**: ros__parameters: lane_change: - backward_lane_length: 200.0 #[m] - prepare_duration: 4.0 # [s] - + backward_lane_length: 200.0 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] - lane_changing_lateral_jerk: 0.5 # [m/s3] - - minimum_lane_changing_velocity: 2.78 # [m/s] - prediction_time_resolution: 0.5 # [s] - 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) - - # longitudinal acceleration - min_longitudinal_acc: -1.0 - max_longitudinal_acc: 1.0 - skip_process: - longitudinal_distance_diff_threshold: - prepare: 0.5 - lane_changing: 0.5 + # trajectory generation + trajectory: + prepare_duration: 4.0 + lateral_jerk: 0.5 + min_longitudinal_acc: -1.0 + max_longitudinal_acc: 1.0 + th_prepare_length_diff: 1.0 + th_lane_changing_length_diff: 1.0 + min_lane_changing_velocity: 2.78 + lon_acc_sampling_num: 5 + lat_acc_sampling_num: 3 # safety check safety_check: allow_loose_check_for_cancel: true enable_target_lane_bound_check: true - collision_check_yaw_diff_threshold: 3.1416 + stopped_object_velocity_threshold: 1.0 # [m/s] execution: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -104,14 +97,16 @@ stop_time: 3.0 # [s] # collision check - enable_collision_check_for_prepare_phase: - general_lanes: false - intersection: true - turns: true - stopped_object_velocity_threshold: 1.0 # [m/s] - check_objects_on_current_lanes: true - check_objects_on_other_lanes: true - use_all_predicted_path: true + collision_check: + enable_for_prepare_phase: + general_lanes: false + intersection: true + turns: true + prediction_time_resolution: 0.5 + yaw_diff_threshold: 3.1416 + check_current_lanes: false + check_other_lanes: false + use_all_predicted_paths: false # lane change cancel cancel: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 46c56e176d0d3..a83f78f616d4b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -14,6 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#include "autoware/behavior_path_lane_change_module/utils/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -42,140 +43,6 @@ using route_handler::RouteHandler; using universe_utils::Polygon2d; using utils::path_safety_checker::ExtendedPredictedObjects; -struct LateralAccelerationMap -{ - std::vector base_vel; - std::vector base_min_acc; - std::vector base_max_acc; - - void add(const double velocity, const double min_acc, const double max_acc) - { - if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { - return; - } - - size_t idx = 0; - for (size_t i = 0; i < base_vel.size(); ++i) { - if (velocity < base_vel.at(i)) { - break; - } - idx = i + 1; - } - - base_vel.insert(base_vel.begin() + idx, velocity); - base_min_acc.insert(base_min_acc.begin() + idx, min_acc); - base_max_acc.insert(base_max_acc.begin() + idx, max_acc); - } - - std::pair find(const double velocity) const - { - if (!base_vel.empty() && velocity < base_vel.front()) { - return std::make_pair(base_min_acc.front(), base_max_acc.front()); - } - if (!base_vel.empty() && velocity > base_vel.back()) { - return std::make_pair(base_min_acc.back(), base_max_acc.back()); - } - - const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); - const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); - - return std::make_pair(min_acc, max_acc); - } -}; - -struct CancelParameters -{ - bool enable_on_prepare_phase{true}; - bool enable_on_lane_changing_phase{false}; - double delta_time{1.0}; - double duration{5.0}; - double max_lateral_jerk{10.0}; - double overhang_tolerance{0.0}; - - // unsafe_hysteresis_threshold will be compare with the number of detected unsafe instance. If the - // number of unsafe exceeds unsafe_hysteresis_threshold, the lane change will be cancelled or - // aborted. - int unsafe_hysteresis_threshold{2}; - - int deceleration_sampling_num{5}; -}; - -struct Parameters -{ - // trajectory generation - double backward_lane_length{200.0}; - double prediction_time_resolution{0.5}; - int longitudinal_acc_sampling_num{10}; - int lateral_acc_sampling_num{10}; - - // lane change 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}; - double lane_changing_lateral_jerk{0.5}; - double minimum_lane_changing_velocity{5.6}; - double lane_change_prepare_duration{4.0}; - LateralAccelerationMap lane_change_lat_acc_map; - - // 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}; - - // acceleration data - double min_longitudinal_acc{-1.0}; - double max_longitudinal_acc{1.0}; - - double skip_process_lon_diff_th_prepare{0.5}; - double skip_process_lon_diff_th_lane_changing{1.0}; - - // collision check - bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; - bool enable_collision_check_for_prepare_phase_in_intersection{true}; - bool enable_collision_check_for_prepare_phase_in_turns{true}; - double stopped_object_velocity_threshold{0.1}; - bool check_objects_on_current_lanes{true}; - bool check_objects_on_other_lanes{true}; - bool use_all_predicted_path{false}; - double lane_expansion_left_offset{0.0}; - double lane_expansion_right_offset{0.0}; - - // regulatory elements - bool regulate_on_crosswalk{false}; - bool regulate_on_intersection{false}; - bool regulate_on_traffic_light{false}; - - // ego vehicle stuck detection - double stop_velocity_threshold{0.1}; - double stop_time_threshold{3.0}; - - // true by default for all objects - utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; - - // safety check - bool allow_loose_check_for_cancel{true}; - bool enable_target_lane_bound_check{true}; - double collision_check_yaw_diff_threshold{3.1416}; - utils::path_safety_checker::RSSparams rss_params{}; - utils::path_safety_checker::RSSparams rss_params_for_parked{}; - utils::path_safety_checker::RSSparams rss_params_for_abort{}; - utils::path_safety_checker::RSSparams rss_params_for_stuck{}; - - // abort - CancelParameters cancel{}; - - // finish judge parameter - double lane_change_finish_judge_buffer{3.0}; - double finish_judge_lateral_threshold{0.2}; - double finish_judge_lateral_angle_deviation{autoware::universe_utils::deg2rad(3.0)}; - - // debug marker - bool publish_debug_marker{false}; -}; - enum class States { Normal = 0, Cancel, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp new file mode 100644 index 0000000000000..112225d51bbdb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp @@ -0,0 +1,169 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ + +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner::lane_change +{ +using utils::path_safety_checker::ObjectTypesToCheck; +using utils::path_safety_checker::RSSparams; + +struct LateralAccelerationMap +{ + std::vector base_vel; + std::vector base_min_acc; + std::vector base_max_acc; + + void add(const double velocity, const double min_acc, const double max_acc) + { + if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { + return; + } + + size_t idx = 0; + for (size_t i = 0; i < base_vel.size(); ++i) { + if (velocity < base_vel.at(i)) { + break; + } + idx = i + 1; + } + + base_vel.insert(base_vel.begin() + idx, velocity); + base_min_acc.insert(base_min_acc.begin() + idx, min_acc); + base_max_acc.insert(base_max_acc.begin() + idx, max_acc); + } + + std::pair find(const double velocity) const + { + if (!base_vel.empty() && velocity < base_vel.front()) { + return std::make_pair(base_min_acc.front(), base_max_acc.front()); + } + if (!base_vel.empty() && velocity > base_vel.back()) { + return std::make_pair(base_min_acc.back(), base_max_acc.back()); + } + + const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + + return std::make_pair(min_acc, max_acc); + } +}; + +struct CancelParameters +{ + bool enable_on_prepare_phase{true}; + bool enable_on_lane_changing_phase{false}; + double delta_time{1.0}; + double duration{5.0}; + double max_lateral_jerk{10.0}; + double overhang_tolerance{0.0}; + + // th_unsafe_hysteresis will be compare with the number of detected unsafe instance. If the + // number of unsafe exceeds th_unsafe_hysteresis, the lane change will be cancelled or + // aborted. + int th_unsafe_hysteresis{2}; + + int deceleration_sampling_num{5}; +}; + +struct CollisionCheckParameters +{ + bool enable_for_prepare_phase_in_general_lanes{false}; + bool enable_for_prepare_phase_in_intersection{true}; + bool enable_for_prepare_phase_in_turns{true}; + bool check_current_lane{true}; + bool check_other_lanes{true}; + bool use_all_predicted_paths{false}; + double th_yaw_diff{3.1416}; + double prediction_time_resolution{0.5}; +}; + +struct SafetyParameters +{ + bool enable_loose_check_for_cancel{true}; + bool enable_target_lane_bound_check{true}; + double th_stopped_object_velocity{0.1}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; + RSSparams rss_params{}; + RSSparams rss_params_for_parked{}; + RSSparams rss_params_for_abort{}; + RSSparams rss_params_for_stuck{}; + ObjectTypesToCheck target_object_types{}; + CollisionCheckParameters collision_check{}; +}; + +struct TrajectoryParameters +{ + double prepare_duration{4.0}; + double lateral_jerk{0.5}; + double min_longitudinal_acc{-1.0}; + double max_longitudinal_acc{1.0}; + double th_prepare_length_diff{0.5}; + double th_lane_changing_length_diff{0.5}; + double min_lane_changing_velocity{5.6}; + int lon_acc_sampling_num{10}; + int lat_acc_sampling_num{10}; + LateralAccelerationMap lat_acc_map{}; +}; + +struct Parameters +{ + TrajectoryParameters trajectory{}; + SafetyParameters safety{}; + CancelParameters cancel{}; + + // lane change parameters + double backward_lane_length{200.0}; + 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}; + + // parked vehicle + double object_check_min_road_shoulder_width{0.5}; + double th_object_shiftable_ratio{0.6}; + + // turn signal + double min_length_for_turn_signal_activation{10.0}; + + // regulatory elements + bool regulate_on_crosswalk{false}; + bool regulate_on_intersection{false}; + bool regulate_on_traffic_light{false}; + + // ego vehicle stuck detection + double th_stop_velocity{0.1}; + double th_stop_time{3.0}; + + // finish judge parameter + double lane_change_finish_judge_buffer{3.0}; + double th_finish_judge_lateral_diff{0.2}; + double th_finish_judge_yaw_diff{autoware_utils::deg2rad(3.0)}; + + // debug marker + bool publish_debug_marker{false}; +}; + +} // namespace autoware::behavior_path_planner::lane_change + +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ 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 04371e8ff3f37..9315549f5b586 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 @@ -43,49 +43,72 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s const auto parameter = [](std::string && name) { return "lane_change." + name; }; // trajectory generation - p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); - p.prediction_time_resolution = - getOrDeclareParameter(*node, parameter("prediction_time_resolution")); - p.longitudinal_acc_sampling_num = - getOrDeclareParameter(*node, parameter("longitudinal_acceleration_sampling_num")); - p.lateral_acc_sampling_num = - getOrDeclareParameter(*node, parameter("lateral_acceleration_sampling_num")); + { + p.trajectory.prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.prepare_duration")); + p.trajectory.lateral_jerk = + getOrDeclareParameter(*node, parameter("trajectory.lateral_jerk")); + p.trajectory.min_longitudinal_acc = + getOrDeclareParameter(*node, parameter("trajectory.min_longitudinal_acc")); + p.trajectory.max_longitudinal_acc = + getOrDeclareParameter(*node, parameter("trajectory.max_longitudinal_acc")); + p.trajectory.th_prepare_length_diff = + getOrDeclareParameter(*node, parameter("trajectory.th_prepare_length_diff")); + p.trajectory.th_lane_changing_length_diff = + getOrDeclareParameter(*node, parameter("trajectory.th_lane_changing_length_diff")); + p.trajectory.min_lane_changing_velocity = + getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); + p.trajectory.lon_acc_sampling_num = + getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); + p.trajectory.lat_acc_sampling_num = + getOrDeclareParameter(*node, parameter("trajectory.lat_acc_sampling_num")); + + const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.trajectory.min_lane_changing_velocity = + std::min(p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.prepare_duration); + + // validation of trajectory parameters + if (p.trajectory.lon_acc_sampling_num < 1 || p.trajectory.lat_acc_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(node_name), + "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " + << p.trajectory.lon_acc_sampling_num + << "Given lateral parameter: " << p.trajectory.lat_acc_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + // lateral acceleration map + const auto lateral_acc_velocity = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); + const auto min_lateral_acc = getOrDeclareParameter>( + *node, parameter("lateral_acceleration.min_values")); + const auto max_lateral_acc = getOrDeclareParameter>( + *node, parameter("lateral_acceleration.max_values")); + if ( + lateral_acc_velocity.size() != min_lateral_acc.size() || + lateral_acc_velocity.size() != max_lateral_acc.size()) { + RCLCPP_ERROR( + node->get_logger().get_child(node_name), + "Lane change lateral acceleration map has invalid size."); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { + p.trajectory.lat_acc_map.add( + lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); + } + } // parked vehicle detection p.object_check_min_road_shoulder_width = getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); - p.object_shiftable_ratio_threshold = + p.th_object_shiftable_ratio = getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); // turn signal p.min_length_for_turn_signal_activation = getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); - p.length_ratio_for_turn_signal_deactivation = - getOrDeclareParameter(*node, parameter("length_ratio_for_turn_signal_deactivation")); - - // acceleration - p.min_longitudinal_acc = getOrDeclareParameter(*node, parameter("min_longitudinal_acc")); - p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); - - // collision check - p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter( - *node, parameter("enable_collision_check_for_prepare_phase.general_lanes")); - p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter( - *node, parameter("enable_collision_check_for_prepare_phase.intersection")); - p.enable_collision_check_for_prepare_phase_in_turns = - getOrDeclareParameter(*node, parameter("enable_collision_check_for_prepare_phase.turns")); - p.stopped_object_velocity_threshold = - getOrDeclareParameter(*node, parameter("stopped_object_velocity_threshold")); - p.check_objects_on_current_lanes = - getOrDeclareParameter(*node, parameter("check_objects_on_current_lanes")); - p.check_objects_on_other_lanes = - getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); - p.use_all_predicted_path = - getOrDeclareParameter(*node, parameter("use_all_predicted_path")); - p.lane_expansion_left_offset = - getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); - p.lane_expansion_right_offset = - getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); + // lane change regulations p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = @@ -94,99 +117,85 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("regulation.traffic_light")); // ego vehicle stuck detection - p.stop_velocity_threshold = - getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); - p.stop_time_threshold = - getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); - - // safety check - p.allow_loose_check_for_cancel = - getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); - p.enable_target_lane_bound_check = - getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); - p.collision_check_yaw_diff_threshold = getOrDeclareParameter( - *node, parameter("safety_check.collision_check_yaw_diff_threshold")); - - p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); - p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); - p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.execution.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.execution.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.execution.lateral_distance_max_threshold")); - - p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); - p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); - p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.parked.longitudinal_velocity_delta_time")); - p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.parked.expected_front_deceleration")); - p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.parked.expected_rear_deceleration")); - p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.parked.rear_vehicle_reaction_time")); - p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin")); - p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.parked.lateral_distance_max_threshold")); - - p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); - p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); - p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.cancel.expected_front_deceleration")); - p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.cancel.expected_rear_deceleration")); - p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); - - p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold")); - p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time")); - p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.stuck.expected_front_deceleration")); - p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.stuck.expected_rear_deceleration")); - p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter( - *node, parameter("safety_check.stuck.rear_vehicle_reaction_time")); - p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter( - *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin")); - p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( - *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + p.th_stop_velocity = getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); + p.th_stop_time = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + + // safety + { + p.safety.enable_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.safety.enable_target_lane_bound_check = + getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); + p.safety.th_stopped_object_velocity = getOrDeclareParameter( + *node, parameter("safety_check.stopped_object_velocity_threshold")); + p.safety.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.safety.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); + + // collision check + p.safety.collision_check.enable_for_prepare_phase_in_general_lanes = + getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.general_lanes")); + p.safety.collision_check.enable_for_prepare_phase_in_intersection = getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.intersection")); + p.safety.collision_check.enable_for_prepare_phase_in_turns = getOrDeclareParameter( + *node, parameter("collision_check.enable_for_prepare_phase.turns")); + p.safety.collision_check.check_current_lane = + getOrDeclareParameter(*node, parameter("collision_check.check_current_lanes")); + p.safety.collision_check.check_other_lanes = + getOrDeclareParameter(*node, parameter("collision_check.check_other_lanes")); + p.safety.collision_check.use_all_predicted_paths = + getOrDeclareParameter(*node, parameter("collision_check.use_all_predicted_paths")); + p.safety.collision_check.prediction_time_resolution = + getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); + p.safety.collision_check.th_yaw_diff = + getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + + // rss check + auto set_rss_params = [&](auto & params, const std::string & prefix) { + params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter(prefix + ".longitudinal_distance_min_threshold")); + params.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter(prefix + ".longitudinal_velocity_delta_time")); + params.front_vehicle_deceleration = + getOrDeclareParameter(*node, parameter(prefix + ".expected_front_deceleration")); + params.rear_vehicle_deceleration = + getOrDeclareParameter(*node, parameter(prefix + ".expected_rear_deceleration")); + params.rear_vehicle_reaction_time = + getOrDeclareParameter(*node, parameter(prefix + ".rear_vehicle_reaction_time")); + params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); + params.lateral_distance_max_threshold = + getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); + }; + set_rss_params(p.safety.rss_params, "safety_check.execution"); + set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); + set_rss_params(p.safety.rss_params_for_abort, "safety_check.cancel"); + set_rss_params(p.safety.rss_params_for_stuck, "safety_check.stuck"); + + // target object types + const std::string ns = "lane_change.target_object."; + p.safety.target_object_types.check_car = getOrDeclareParameter(*node, ns + "car"); + p.safety.target_object_types.check_truck = getOrDeclareParameter(*node, ns + "truck"); + p.safety.target_object_types.check_bus = getOrDeclareParameter(*node, ns + "bus"); + p.safety.target_object_types.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); + p.safety.target_object_types.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); + p.safety.target_object_types.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.safety.target_object_types.check_motorcycle = + getOrDeclareParameter(*node, ns + "motorcycle"); + p.safety.target_object_types.check_pedestrian = + getOrDeclareParameter(*node, ns + "pedestrian"); + } // lane change parameters - const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); p.backward_length_buffer_for_blocking_object = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); p.backward_length_from_intersection = getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); - p.lane_changing_lateral_jerk = - getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); - p.lane_change_prepare_duration = - getOrDeclareParameter(*node, parameter("prepare_duration")); - p.minimum_lane_changing_velocity = - getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); - p.minimum_lane_changing_velocity = - std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -194,41 +203,6 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s "Lane change buffer must be more than 1 meter. Modifying the buffer."); } - // lateral acceleration map for lane change - const auto lateral_acc_velocity = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); - const auto min_lateral_acc = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.min_values")); - const auto max_lateral_acc = - getOrDeclareParameter>(*node, parameter("lateral_acceleration.max_values")); - if ( - lateral_acc_velocity.size() != min_lateral_acc.size() || - lateral_acc_velocity.size() != max_lateral_acc.size()) { - RCLCPP_ERROR( - node->get_logger().get_child(node_name), - "Lane change lateral acceleration map has invalid size."); - exit(EXIT_FAILURE); - } - for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { - p.lane_change_lat_acc_map.add( - lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); - } - - // target object - { - const std::string ns = "lane_change.target_object."; - p.object_types_to_check.check_car = getOrDeclareParameter(*node, ns + "car"); - p.object_types_to_check.check_truck = getOrDeclareParameter(*node, ns + "truck"); - p.object_types_to_check.check_bus = getOrDeclareParameter(*node, ns + "bus"); - p.object_types_to_check.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); - p.object_types_to_check.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); - p.object_types_to_check.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); - p.object_types_to_check.check_motorcycle = - getOrDeclareParameter(*node, ns + "motorcycle"); - p.object_types_to_check.check_pedestrian = - getOrDeclareParameter(*node, ns + "pedestrian"); - } - // lane change cancel p.cancel.enable_on_prepare_phase = getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); @@ -240,7 +214,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("cancel.max_lateral_jerk")); p.cancel.overhang_tolerance = getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); - p.cancel.unsafe_hysteresis_threshold = + p.cancel.th_unsafe_hysteresis = getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); p.cancel.deceleration_sampling_num = getOrDeclareParameter(*node, parameter("cancel.deceleration_sampling_num")); @@ -248,43 +222,35 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // finish judge parameters p.lane_change_finish_judge_buffer = getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); - p.finish_judge_lateral_threshold = + p.th_finish_judge_lateral_diff = getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); const auto finish_judge_lateral_angle_deviation = getOrDeclareParameter(*node, parameter("finish_judge_lateral_angle_deviation")); - p.finish_judge_lateral_angle_deviation = + p.th_finish_judge_yaw_diff = autoware::universe_utils::deg2rad(finish_judge_lateral_angle_deviation); // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); - // validation of parameters - if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - node->get_logger().get_child(node_name), - "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " - << p.longitudinal_acc_sampling_num - << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - // validation of safety check parameters - // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // if loose check is not enabled, lane change module will keep on chattering and canceling, and // false positive situation might occur - if (!p.allow_loose_check_for_cancel) { + if (!p.safety.enable_loose_check_for_cancel) { if ( - p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || - p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || - p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || - p.rss_params.rear_vehicle_safety_time_margin > - p.rss_params_for_abort.rear_vehicle_safety_time_margin || - p.rss_params.lateral_distance_max_threshold > - p.rss_params_for_abort.lateral_distance_max_threshold || - p.rss_params.longitudinal_distance_min_threshold > - p.rss_params_for_abort.longitudinal_distance_min_threshold || - p.rss_params.longitudinal_velocity_delta_time > - p.rss_params_for_abort.longitudinal_velocity_delta_time) { + p.safety.rss_params.front_vehicle_deceleration > + p.safety.rss_params_for_abort.front_vehicle_deceleration || + p.safety.rss_params.rear_vehicle_deceleration > + p.safety.rss_params_for_abort.rear_vehicle_deceleration || + p.safety.rss_params.rear_vehicle_reaction_time > + p.safety.rss_params_for_abort.rear_vehicle_reaction_time || + p.safety.rss_params.rear_vehicle_safety_time_margin > + p.safety.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.safety.rss_params.lateral_distance_max_threshold > + p.safety.rss_params_for_abort.lateral_distance_max_threshold || + p.safety.rss_params.longitudinal_distance_min_threshold > + p.safety.rss_params_for_abort.longitudinal_distance_min_threshold || + p.safety.rss_params.longitudinal_velocity_delta_time > + p.safety.rss_params_for_abort.longitudinal_velocity_delta_time) { RCLCPP_FATAL_STREAM( node->get_logger().get_child(node_name), "abort parameter might be loose... Terminating the program..."); @@ -323,8 +289,6 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "backward_lane_length", p->backward_lane_length); - updateParam(parameters, ns + "prepare_duration", p->lane_change_prepare_duration); - updateParam( parameters, ns + "backward_length_buffer_for_end_of_lane", p->backward_length_buffer_for_end_of_lane); @@ -335,59 +299,57 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorlane_change_finish_judge_buffer); updateParam( - parameters, ns + "lane_changing_lateral_jerk", p->lane_changing_lateral_jerk); + parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); + updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + } + { + const std::string ns = "lane_change.trajectory."; + updateParam(parameters, ns + "prepare_duration", p->trajectory.prepare_duration); + updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); updateParam( - parameters, ns + "minimum_lane_changing_velocity", p->minimum_lane_changing_velocity); + parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); + // longitudinal acceleration updateParam( - parameters, ns + "prediction_time_resolution", p->prediction_time_resolution); - + parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc); + updateParam( + parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); int longitudinal_acc_sampling_num = 0; - updateParam( - parameters, ns + "longitudinal_acceleration_sampling_num", longitudinal_acc_sampling_num); + updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { - p->longitudinal_acc_sampling_num = longitudinal_acc_sampling_num; + p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; } int lateral_acc_sampling_num = 0; - updateParam( - parameters, ns + "lateral_acceleration_sampling_num", lateral_acc_sampling_num); + updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { - p->lateral_acc_sampling_num = lateral_acc_sampling_num; + p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; } updateParam( - parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - - // longitudinal acceleration - updateParam(parameters, ns + "min_longitudinal_acc", p->min_longitudinal_acc); - updateParam(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc); - } - - { - const std::string ns = "lane_change.skip_process.longitudinal_distance_diff_threshold."; - updateParam(parameters, ns + "prepare", p->skip_process_lon_diff_th_prepare); + parameters, ns + "th_prepare_length_diff", p->trajectory.th_prepare_length_diff); updateParam( - parameters, ns + "lane_changing", p->skip_process_lon_diff_th_lane_changing); + parameters, ns + "th_lane_changing_length_diff", p->trajectory.th_lane_changing_length_diff); } { const std::string ns = "lane_change.safety_check.lane_expansion."; - updateParam(parameters, ns + "left_offset", p->lane_expansion_left_offset); - updateParam(parameters, ns + "right_offset", p->lane_expansion_right_offset); + updateParam(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); + updateParam(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); } { const std::string ns = "lane_change.target_object."; - updateParam(parameters, ns + "car", p->object_types_to_check.check_car); - updateParam(parameters, ns + "truck", p->object_types_to_check.check_truck); - updateParam(parameters, ns + "bus", p->object_types_to_check.check_bus); - updateParam(parameters, ns + "trailer", p->object_types_to_check.check_trailer); - updateParam(parameters, ns + "unknown", p->object_types_to_check.check_unknown); - updateParam(parameters, ns + "bicycle", p->object_types_to_check.check_bicycle); - updateParam(parameters, ns + "motorcycle", p->object_types_to_check.check_motorcycle); - updateParam(parameters, ns + "pedestrian", p->object_types_to_check.check_pedestrian); + updateParam(parameters, ns + "car", p->safety.target_object_types.check_car); + updateParam(parameters, ns + "truck", p->safety.target_object_types.check_truck); + updateParam(parameters, ns + "bus", p->safety.target_object_types.check_bus); + updateParam(parameters, ns + "trailer", p->safety.target_object_types.check_trailer); + updateParam(parameters, ns + "unknown", p->safety.target_object_types.check_unknown); + updateParam(parameters, ns + "bicycle", p->safety.target_object_types.check_bicycle); + updateParam( + parameters, ns + "motorcycle", p->safety.target_object_types.check_motorcycle); + updateParam( + parameters, ns + "pedestrian", p->safety.target_object_types.check_pedestrian); } { @@ -399,8 +361,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "velocity", p->stop_velocity_threshold); - updateParam(parameters, ns + "stop_time", p->stop_time_threshold); + updateParam(parameters, ns + "velocity", p->th_stop_velocity); + updateParam(parameters, ns + "stop_time", p->th_stop_time); } { @@ -425,7 +387,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); updateParam(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance); updateParam( - parameters, ns + "unsafe_hysteresis_threshold", p->cancel.unsafe_hysteresis_threshold); + parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); 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 783246602ae4c..1884c1f7704fd 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 @@ -287,7 +287,7 @@ bool NormalLaneChange::is_near_regulatory_element() const if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) return false; - const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; if (only_tl) { RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); @@ -554,7 +554,8 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) // 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); + 0.0, lc_param_ptr->trajectory.min_lane_changing_velocity, + lc_param_ptr->safety.rss_params_for_parked); const auto stop_margin = transient_data.lane_changing_length.min + lc_param_ptr->backward_length_buffer_for_blocking_object + rss_dist + @@ -751,13 +752,13 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto yaw_deviation_to_centerline = utils::lane_change::calc_angle_to_lanelet_segment(target_lanes, current_pose); - if (yaw_deviation_to_centerline > lane_change_parameters_->finish_judge_lateral_angle_deviation) { + if (yaw_deviation_to_centerline > lane_change_parameters_->th_finish_judge_yaw_diff) { return false; } const auto & arc_length = common_data_ptr_->transient_data.target_lanes_ego_arc; const auto reach_target_lane = - std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; + std::abs(arc_length.distance) < lane_change_parameters_->th_finish_judge_lateral_diff; lane_change_debug_.distance_to_lane_change_finished = arc_length.distance; @@ -789,7 +790,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const planner_data_->parameters.ego_nearest_yaw_threshold); const double ego_velocity = - std::max(getEgoVelocity(), lane_change_parameters_->minimum_lane_changing_velocity); + std::max(getEgoVelocity(), lane_change_parameters_->trajectory.min_lane_changing_velocity); const double estimated_travel_dist = ego_velocity * lane_change_parameters_->cancel.delta_time; double dist = 0.0; @@ -950,12 +951,14 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( insert_leading_objects(filtered_objects.target_lane_leading.stopped); insert_leading_objects(filtered_objects.target_lane_leading.stopped_at_bound); - const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; + const auto chk_obj_in_curr_lanes = + lane_change_parameters_->safety.collision_check.check_current_lane; if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { insert_leading_objects(filtered_objects.current_lane); } - const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; + const auto chk_obj_in_other_lanes = + lane_change_parameters_->safety.collision_check.check_other_lanes; if (chk_obj_in_other_lanes) { insert_leading_objects(filtered_objects.others); } @@ -967,7 +970,7 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const { auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( - objects, lane_change_parameters_->object_types_to_check); + objects, lane_change_parameters_->safety.target_object_types); if (objects.objects.empty()) { return {}; @@ -1002,10 +1005,10 @@ 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 = common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; + 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, *common_data_ptr_->lc_param_ptr); + auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_); const auto & ext_obj_pose = ext_object.initial_pose; ext_object.dist_from_ego = autoware::motion_utils::calcSignedArcLength( current_lanes_ref_path.points, current_pose.position, ext_obj_pose.position); @@ -1184,9 +1187,9 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto prepare_phase_metrics = get_prepare_metrics(); candidate_paths.reserve( - prepare_phase_metrics.size() * lane_change_parameters_->lateral_acc_sampling_num); + prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); - const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; const auto dist_to_next_regulatory_element = utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); @@ -1195,12 +1198,12 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) if (candidate_paths.empty()) return true; const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length); - if (prep_diff > lane_change_parameters_->skip_process_lon_diff_th_prepare) return true; + if (prep_diff > lane_change_parameters_->trajectory.th_prepare_length_diff) return true; if (!check_lc) return false; const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); - return lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing; + return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; }; for (const auto & prep_metric : prepare_phase_metrics) { @@ -1344,11 +1347,11 @@ bool NormalLaneChange::check_candidate_path_safety( } const auto lc_start_velocity = candidate_path.info.velocity.prepare; - const auto min_lc_velocity = lane_change_parameters_->minimum_lane_changing_velocity; + const auto min_lc_velocity = lane_change_parameters_->trajectory.min_lane_changing_velocity; constexpr double margin = 0.1; // path is unsafe if it exceeds target lane boundary with a high velocity if ( - lane_change_parameters_->enable_target_lane_bound_check && + lane_change_parameters_->safety.enable_target_lane_bound_check && lc_start_velocity > min_lc_velocity + margin && utils::lane_change::path_footprint_exceeds_target_lane_bound( common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) { @@ -1357,12 +1360,12 @@ bool NormalLaneChange::check_candidate_path_safety( constexpr size_t decel_sampling_num = 1; const auto safety_check_with_normal_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num, - lane_change_debug_.collision_check_objects); + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params, + decel_sampling_num, lane_change_debug_.collision_check_objects); if (!safety_check_with_normal_rss.is_safe && is_stuck) { const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, decel_sampling_num, lane_change_debug_.collision_check_objects); return safety_check_with_stuck_rss.is_safe; } @@ -1390,7 +1393,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto & route_handler = *getRouteHandler(); const auto minimum_lane_changing_velocity = - lane_change_parameters_->minimum_lane_changing_velocity; + lane_change_parameters_->trajectory.min_lane_changing_velocity; const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); @@ -1431,10 +1434,10 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( target_lanes, lane_changing_start_pose.value()); const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + lane_change_parameters_->trajectory.lat_acc_map.find(minimum_lane_changing_velocity); const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + shift_length, lane_change_parameters_->trajectory.lateral_jerk, max_lateral_acc); const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; const auto target_segment = getTargetSegment( @@ -1526,7 +1529,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const } const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->rss_params_for_abort, + path, target_objects, lane_change_parameters_->safety.rss_params_for_abort, static_cast(lane_change_parameters_->cancel.deceleration_sampling_num), debug_data); { // only for debug purpose @@ -1560,7 +1563,7 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( } unsafe_hysteresis_count_ = 0; } - if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) { + if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.th_unsafe_hysteresis) { RCLCPP_DEBUG( logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__, (approved_path_safety_status.is_safe ? "safe" : "UNSAFE")); @@ -1622,7 +1625,7 @@ bool NormalLaneChange::calcAbortPath() const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = - std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); + std::max(lane_change_parameters_->trajectory.min_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; @@ -1706,7 +1709,7 @@ bool NormalLaneChange::calcAbortPath() shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); const auto lateral_acc_range = - lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); + lane_change_parameters_->trajectory.lat_acc_map.find(current_velocity); const double & max_lateral_acc = lateral_acc_range.second; path_shifter.setLateralAccelerationLimit(max_lateral_acc); @@ -1824,16 +1827,16 @@ bool NormalLaneChange::has_collision_with_decel_patterns( acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), [&](double n) { return lane_changing_acc + n * acc_resolution; }); + const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; const auto all_collided = std::all_of( acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path( common_data_ptr_, lane_change_path, acceleration); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = - (obj.initial_twist.linear.x <= lane_change_parameters_->stopped_object_velocity_threshold) - ? lane_change_parameters_->rss_params_for_parked - : rss_param; + const auto selected_rss_param = (obj.initial_twist.linear.x <= stopped_obj_vel_th) + ? lane_change_parameters_->safety.rss_params_for_parked + : rss_param; return is_collided( lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase, debug_data); @@ -1872,11 +1875,11 @@ bool NormalLaneChange::is_collided( constexpr auto collision_check_yaw_diff_threshold{M_PI}; constexpr auto hysteresis_factor{1.0}; const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - obj, lane_change_parameters_->use_all_predicted_path); + obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths); const auto safety_check_max_vel = get_max_velocity_for_safety_check(); const auto & bpp_param = *common_data_ptr_->bpp_param_ptr; - const double velocity_threshold = lane_change_parameters_->stopped_object_velocity_threshold; + const double velocity_threshold = lane_change_parameters_->safety.th_stopped_object_velocity; const double prepare_duration = lane_change_path.info.duration.prepare; const double start_time = check_prepare_phase ? 0.0 : prepare_duration; @@ -1938,13 +1941,13 @@ bool NormalLaneChange::is_ego_stuck() const universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->stop_velocity_threshold) { + if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->th_stop_velocity) { RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); return false; } // Ego is just stopped, not sure it is in stuck yet. - if (getStopTime() < lc_param_ptr->stop_time_threshold) { + if (getStopTime() < lc_param_ptr->th_stop_time) { RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); return false; } @@ -1952,8 +1955,8 @@ bool NormalLaneChange::is_ego_stuck() const // Check if any stationary object exist in obstacle_check_distance const auto & current_lanes_path = common_data_ptr_->current_lanes_path; const auto & ego_pose = common_data_ptr_->get_ego_pose(); - const auto rss_dist = - calcRssDistance(0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params); + const auto rss_dist = calcRssDistance( + 0.0, lc_param_ptr->trajectory.min_lane_changing_velocity, lc_param_ptr->safety.rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be // determined, even though you are stuck by an obstacle. If it is too long, @@ -1970,7 +1973,7 @@ bool NormalLaneChange::is_ego_stuck() const // Note: it needs chattering prevention. if ( std::abs(object.initial_twist.linear.x) > - lc_param_ptr->stopped_object_velocity_threshold) { // check if stationary + lc_param_ptr->safety.th_stopped_object_velocity) { // check if stationary return false; } @@ -2008,14 +2011,14 @@ void NormalLaneChange::updateStopTime() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_vel = getEgoVelocity(); - if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { + if (std::abs(current_vel) > lane_change_parameters_->th_stop_velocity) { stop_time_ = 0.0; } else { const double duration = stop_watch_.toc("stop_time"); // clip stop time - if (stop_time_ + duration * 0.001 > lane_change_parameters_->stop_time_threshold) { + if (stop_time_ + duration * 0.001 > lane_change_parameters_->th_stop_time) { constexpr double eps = 0.1; - stop_time_ = lane_change_parameters_->stop_time_threshold + eps; + stop_time_ = lane_change_parameters_->th_stop_time + eps; } else { stop_time_ += duration * 0.001; } @@ -2029,7 +2032,7 @@ bool NormalLaneChange::check_prepare_phase() const const auto & route_handler = getRouteHandler(); const auto check_in_general_lanes = - lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_general_lanes; lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { @@ -2040,11 +2043,11 @@ bool NormalLaneChange::check_prepare_phase() const } const auto check_in_intersection = - lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection && + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_intersection && common_data_ptr_->transient_data.in_intersection; const auto check_in_turns = - lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns && + lane_change_parameters_->safety.collision_check.enable_for_prepare_phase_in_turns && common_data_ptr_->transient_data.in_turn_direction_lane; return check_in_intersection || check_in_turns || check_in_general_lanes; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index e93096dc18314..0d84bc7c850da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -48,8 +48,8 @@ double calc_dist_from_pose_to_terminal_end( double calc_stopping_distance(const LCParamPtr & lc_param_ptr) { // v^2 = u^2 + 2ad - const auto min_lc_vel = lc_param_ptr->minimum_lane_changing_velocity; - const auto min_lon_acc = lc_param_ptr->min_longitudinal_acc; + const auto min_lc_vel = lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto min_lon_acc = lc_param_ptr->trajectory.min_longitudinal_acc; const auto min_back_dist = std::abs((min_lc_vel * min_lc_vel) / (2 * min_lon_acc)); const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; @@ -103,7 +103,7 @@ double calc_dist_to_last_fit_width( double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { - const auto max_prepare_duration = common_data_ptr->lc_param_ptr->lane_change_prepare_duration; + const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.prepare_duration; const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel; return max_prepare_duration * ego_max_speed; @@ -145,7 +145,7 @@ double calc_minimum_acceleration( const double min_acc_threshold, const double prepare_duration) { if (prepare_duration < eps) return -std::abs(min_acc_threshold); - const double min_lc_velocity = lane_change_parameters.minimum_lane_changing_velocity; + const double min_lc_velocity = lane_change_parameters.trajectory.min_lane_changing_velocity; const double acc = (min_lc_velocity - current_velocity) / prepare_duration; return std::clamp(acc, -std::abs(min_acc_threshold), -eps); } @@ -166,10 +166,10 @@ std::vector calc_min_lane_change_lengths( return {}; } - const auto min_vel = lc_param_ptr->minimum_lane_changing_velocity; - const auto min_max_lat_acc = lc_param_ptr->lane_change_lat_acc_map.find(min_vel); + const auto min_vel = lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto min_max_lat_acc = lc_param_ptr->trajectory.lat_acc_map.find(min_vel); const auto max_lat_acc = std::get<1>(min_max_lat_acc); - const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; + const auto lat_jerk = lc_param_ptr->trajectory.lateral_jerk; std::vector min_lc_lengths{}; min_lc_lengths.reserve(shift_intervals.size()); @@ -193,24 +193,23 @@ std::vector calc_max_lane_change_lengths( return {}; } - const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; - const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; - const auto t_prepare = lc_param_ptr->lane_change_prepare_duration; + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto lat_jerk = params.lateral_jerk; + const auto t_prepare = params.prepare_duration; const auto current_velocity = common_data_ptr->get_ego_speed(); const auto path_velocity = common_data_ptr->transient_data.current_path_velocity; const auto max_acc = calc_maximum_acceleration( - t_prepare, current_velocity, path_velocity, lc_param_ptr->max_longitudinal_acc); + t_prepare, current_velocity, path_velocity, params.max_longitudinal_acc); // TODO(Quda, Azu): should probably limit upper bound of velocity as well, but // disabled due failing evaluation tests. // const auto limit_vel = [&](const auto vel) { // const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; - // return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); + // return std::clamp(vel, params.min_lane_changing_velocity, max_global_vel); // }; - auto vel = - std::max(common_data_ptr->get_ego_speed(), lc_param_ptr->minimum_lane_changing_velocity); + auto vel = std::max(common_data_ptr->get_ego_speed(), params.min_lane_changing_velocity); std::vector max_lc_lengths{}; @@ -220,7 +219,7 @@ std::vector calc_max_lane_change_lengths( vel = vel + max_acc * t_prepare; // lane changing section - const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); + const auto [min_lat_acc, max_lat_acc] = params.lat_acc_map.find(vel); const auto t_lane_changing = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = @@ -305,8 +304,10 @@ std::pair calc_min_max_acceleration( const auto & bpp_params = *common_data_ptr->bpp_param_ptr; const auto current_ego_velocity = common_data_ptr->get_ego_speed(); - const auto min_accel_threshold = std::max(bpp_params.min_acc, lc_params.min_longitudinal_acc); - const auto max_accel_threshold = std::min(bpp_params.max_acc, lc_params.max_longitudinal_acc); + const auto min_accel_threshold = + std::max(bpp_params.min_acc, lc_params.trajectory.min_longitudinal_acc); + const auto max_accel_threshold = + std::min(bpp_params.max_acc, lc_params.trajectory.max_longitudinal_acc); // calculate minimum and maximum acceleration const auto min_acc = calc_minimum_acceleration( @@ -350,7 +351,7 @@ std::vector calc_lon_acceleration_samples( const auto & current_pose = common_data_ptr->get_ego_pose(); const auto & target_lanes = common_data_ptr->lanes_ptr->target; const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); - const auto sampling_num = common_data_ptr->lc_param_ptr->longitudinal_acc_sampling_num; + const auto sampling_num = common_data_ptr->lc_param_ptr->trajectory.lon_acc_sampling_num; const auto [min_accel, max_accel] = calc_min_max_acceleration(common_data_ptr, max_path_velocity, prepare_duration); @@ -394,7 +395,7 @@ std::vector calc_prepare_durations(const CommonDataPtr & common_data_ptr const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto threshold = common_data_ptr->bpp_param_ptr->base_link2front + lc_param_ptr->min_length_for_turn_signal_activation; - const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; + const auto max_prepare_duration = lc_param_ptr->trajectory.prepare_duration; // TODO(Azu) this check seems to cause scenario failures. if (common_data_ptr->transient_data.dist_to_terminal_start >= threshold) { @@ -416,7 +417,7 @@ std::vector calc_prepare_phase_metrics( const double max_path_velocity, const double min_length_threshold, const double max_length_threshold) { - const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity; const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; std::vector metrics; @@ -463,14 +464,15 @@ std::vector calc_shift_phase_metrics( const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, const double max_path_velocity, const double lon_accel, const double max_length_threshold) { - const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity; const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = - common_data_ptr->lc_param_ptr->lane_change_lat_acc_map.find(initial_velocity); - const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / - common_data_ptr->lc_param_ptr->lateral_acc_sampling_num; + common_data_ptr->lc_param_ptr->trajectory.lat_acc_map.find(initial_velocity); + const auto lateral_acc_resolution = + std::abs(max_lateral_acc - min_lateral_acc) / + common_data_ptr->lc_param_ptr->trajectory.lat_acc_sampling_num; std::vector metrics; @@ -488,7 +490,7 @@ std::vector calc_shift_phase_metrics( for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; lat_acc += lateral_acc_resolution) { const auto lane_changing_duration = PathShifter::calcShiftTimeFromJerk( - shift_length, common_data_ptr->lc_param_ptr->lane_changing_lateral_jerk, lat_acc); + shift_length, common_data_ptr->lc_param_ptr->trajectory.lateral_jerk, lat_acc); const double lane_changing_accel = calc_lane_changing_acceleration( initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); 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 b50159555a25f..c66b770cfa0a1 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 @@ -623,7 +623,7 @@ std::vector convert_to_predicted_path( const auto duration = lane_change_path.info.duration.sum(); const auto prepare_time = lane_change_path.info.duration.prepare; const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; - const auto resolution = lc_param_ptr->prediction_time_resolution; + const auto resolution = lc_param_ptr->safety.collision_check.prediction_time_resolution; std::vector predicted_path; // prepare segment @@ -787,8 +787,7 @@ bool passed_parked_objects( const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = - lane_change_parameters.object_shiftable_ratio_threshold; + const auto & object_shiftable_ratio_threshold = lane_change_parameters.th_object_shiftable_ratio; const auto & current_lane_path = common_data_ptr->current_lanes_path; if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { @@ -921,7 +920,8 @@ ExtendedPredictedObject transform( { ExtendedPredictedObject extended_object(object); - const auto & time_resolution = lane_change_parameters.prediction_time_resolution; + const auto & time_resolution = + lane_change_parameters.safety.collision_check.prediction_time_resolution; const double obj_vel_norm = std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); @@ -1035,10 +1035,10 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) lanes_polygon.target = utils::lane_change::create_polygon(lanes->target, 0.0, std::numeric_limits::max()); - const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto & params = common_data_ptr->lc_param_ptr->safety; const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset, - lc_param_ptr->lane_expansion_right_offset); + lanes->target, common_data_ptr->direction, params.lane_expansion_left_offset, + params.lane_expansion_right_offset); lanes_polygon.expanded_target = utils::lane_change::create_polygon( expanded_target_lanes, 0.0, std::numeric_limits::max()); @@ -1187,7 +1187,7 @@ double get_min_dist_to_current_lanes_obj( 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) { + if (obj_v > common_data_ptr->lc_param_ptr->th_stop_velocity) { continue; } @@ -1300,7 +1300,7 @@ bool filter_target_lane_objects( const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m; const auto & lanes_polygon = *common_data_ptr->lanes_polygon_ptr; - const auto stopped_obj_vel_th = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold; + const auto stopped_obj_vel_th = common_data_ptr->lc_param_ptr->safety.th_stopped_object_velocity; const auto is_lateral_far = std::invoke([&]() -> bool { const auto dist_object_to_current_lanes_center = From fa3b39825239bdaf10054d6b26ce1f42caafe328 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Tue, 26 Nov 2024 20:54:30 +0900 Subject: [PATCH 21/29] fix(autoware_behavior_path_lane_change_module): fix clang-diagnostic-overloaded-virtual (#9400) Signed-off-by: kobayu858 --- .../autoware/behavior_path_lane_change_module/interface.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index fd9375c7e9f75..eb30adef4eba4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -94,6 +94,8 @@ class LaneChangeInterface : public SceneModuleInterface MarkerArray getModuleVirtualWall() override; protected: + using SceneModuleInterface::updateRTCStatus; + std::shared_ptr parameters_; std::unique_ptr module_type_; From f82e01f4958c4fa541c1096bb3f308083f86288f Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Tue, 26 Nov 2024 21:00:31 +0900 Subject: [PATCH 22/29] fix(autoware_behavior_path_lane_change_module): fix clang-diagnostic-error (#9402) Signed-off-by: kobayu858 --- .../src/utils/calculation.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 0d84bc7c850da..7783079dd3bb4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -353,8 +353,10 @@ std::vector calc_lon_acceleration_samples( const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); const auto sampling_num = common_data_ptr->lc_param_ptr->trajectory.lon_acc_sampling_num; - const auto [min_accel, max_accel] = + const auto min_max_accel = calc_min_max_acceleration(common_data_ptr, max_path_velocity, prepare_duration); + const auto & min_accel = min_max_accel.first; + const auto & max_accel = min_max_accel.second; const auto is_sampling_required = std::invoke([&]() -> bool { if (max_accel < 0.0 || transient_data.is_ego_stuck) return true; From 3bfd52bc192aa2c483b9f59708bff7cc041cf4a3 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 28 Nov 2024 17:16:24 +0900 Subject: [PATCH 23/29] fix(autoware_behavior_path_lane_change_module): fix clang-diagnostic-unused-variable (#9401) Signed-off-by: kobayu858 --- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 1 - 1 file changed, 1 deletion(-) 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 1884c1f7704fd..ee9c95bafb6c4 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 @@ -1141,7 +1141,6 @@ std::vector NormalLaneChange::get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, const double shift_length, const double dist_to_reg_element) const { - const auto & route_handler = getRouteHandler(); const auto & transient_data = common_data_ptr_->transient_data; const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, From 31eec09a743747a4aebfdf3db6450b30dea9014c Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Thu, 28 Nov 2024 11:29:59 +0900 Subject: [PATCH 24/29] feat(lane_change): improve delay lane change logic (#9480) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * implement function to check if lane change delay is required Signed-off-by: mohammad alqudah * refactor function isParkedObject Signed-off-by: mohammad alqudah * refactor delay lane change parameters Signed-off-by: mohammad alqudah * update lc param yaml Signed-off-by: mohammad alqudah * separate target lane leading objects based on behavior (RT1-8532) Signed-off-by: Zulfaqar Azmi * fixed overlapped filtering and lanes debug marker Signed-off-by: Zulfaqar Azmi * combine filteredObjects function Signed-off-by: Zulfaqar Azmi * renaming functions and type Signed-off-by: Zulfaqar Azmi * update some logic to check is stopped Signed-off-by: Zulfaqar Azmi * rename expanded to stopped_outside_boundary Signed-off-by: Zulfaqar Azmi * Include docstring Signed-off-by: Zulfaqar Azmi * rename stopped_outside_boundary → stopped_at_bound Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi * spell-check Signed-off-by: Zulfaqar Azmi * add docstring for function is_delay_lane_change Signed-off-by: mohammad alqudah * remove unused functions Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * add delay parameters to README Signed-off-by: mohammad alqudah * add documentation for delay lane change behavior Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * run pre-commit checks Signed-off-by: mohammad alqudah * only check for delay lc if feature is enabled Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Signed-off-by: Zulfaqar Azmi Co-authored-by: Zulfaqar Azmi Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../README.md | 76 +- .../config/lane_change.param.yaml | 11 +- .../images/delay_lane_change_1.drawio.svg | 657 +++++++++++++++++ .../images/delay_lane_change_2.drawio.svg | 625 ++++++++++++++++ .../images/delay_lane_change_3.drawio.svg | 683 ++++++++++++++++++ .../images/delay_lane_change_4.drawio.svg | 683 ++++++++++++++++++ .../images/delay_lane_change_5.drawio.svg | 677 +++++++++++++++++ .../utils/parameters.hpp | 9 + .../utils/utils.hpp | 29 +- .../src/manager.cpp | 15 +- .../src/scene.cpp | 8 +- .../src/utils/utils.cpp | 198 ++--- 12 files changed, 3495 insertions(+), 176 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg 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 6ebe79c180e95..d24a89bf03000 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 @@ -351,6 +351,71 @@ stop @enduml ``` +#### Delay Lane Change Check + +In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle. +To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected. + +1. The distance from object to terminal end is sufficient to perform lane change +2. The distance to object is less than the lane changing length +3. The distance from object to next object is sufficient to perform lane change + +If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked. + +The following flow chart illustrates the delay lane change check. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #White + +start +if (Is target objects, candidate path, OR current lane path empty?) then (yes) + #LightPink:Return false; + stop +else (no) +endif + +:Start checking objects from closest to furthest; +repeat + if (Is distance from object to terminal sufficient) then (yes) + else (no) + #LightPink:Return false; + stop + endif + + if (Is distance to object less than lane changing length) then (yes) + else (no) + if (Is only check parked vehicles and vehicle is not parked) then (yes) + else (no) + if(Is last object OR distance to next object is sufficient) then (yes) + #LightGreen: Return true; + stop + else (no) + endif + endif + endif + repeat while (Is finished checking all objects) is (FALSE) + +#LightPink: Return false; +stop + +@enduml +``` + +The following figures demonstrate different situations under which will or will not be triggered: + +1. Delay lane change will be triggered as there is sufficient distance ahead. + ![delay lane change 1](./images/delay_lane_change_1.drawio.svg) +2. Delay lane change will NOT be triggered as there is no sufficient distance ahead + ![delay lane change 2](./images/delay_lane_change_2.drawio.svg) +3. Delay lane change will be triggered by fist NPC as there is sufficient distance ahead. + ![delay lane change 3](./images/delay_lane_change_3.drawio.svg) +4. Delay lane change will be triggered by second NPC as there is sufficient distance ahead + ![delay lane change 4](./images/delay_lane_change_4.drawio.svg) +5. Delay lane change will NOT be triggered as there is no sufficient distance ahead. + ![delay lane change 5](./images/delay_lane_change_5.drawio.svg) + #### Candidate Path's Safety check See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) @@ -828,8 +893,6 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | | `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | | `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | -| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | | `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | @@ -860,6 +923,15 @@ The following parameters are used to judge lane change completion. | `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | | `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | +### Delay Lane Change + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------ | ---- | ------ | ----------------------------------------------------------------------------------------------------- | ------------- | +| `delay_lane_change.enable` | [-] | bool | Flag to enable/disable lane change delay feature | true | +| `delay_lane_change.check_only_parked_vehicle` | [-] | bool | Flag to limit delay feature for only parked vehicles | false | +| `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | +| `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | + ### Collision checks #### Target Objects 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 41810ea4a80bf..ad2d72f0140d0 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 @@ -6,10 +6,6 @@ backward_length_buffer_for_blocking_object: 3.0 # [m] backward_length_from_intersection: 5.0 # [m] - # 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] @@ -25,6 +21,13 @@ lon_acc_sampling_num: 5 lat_acc_sampling_num: 3 + # delay lane change + delay_lane_change: + enable: true + check_only_parked_vehicle: false + min_road_shoulder_width: 0.5 # [m] + th_parked_vehicle_shift_ratio: 0.6 + # safety check safety_check: allow_loose_check_for_cancel: true diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg new file mode 100644 index 0000000000000..f5f3e4e88559b --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg @@ -0,0 +1,657 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg new file mode 100644 index 0000000000000..8237ac312aadb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg @@ -0,0 +1,625 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg new file mode 100644 index 0000000000000..2bcbfb5cdb93d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg new file mode 100644 index 0000000000000..b38092183db14 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg new file mode 100644 index 0000000000000..d4abb53a84999 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg @@ -0,0 +1,677 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp index 112225d51bbdb..a7ea5a848b660 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp @@ -127,11 +127,20 @@ struct TrajectoryParameters LateralAccelerationMap lat_acc_map{}; }; +struct DelayParameters +{ + bool enable{true}; + bool check_only_parked_vehicle{false}; + double min_road_shoulder_width{0.5}; + double th_parked_vehicle_shift_ratio{0.6}; +}; + struct Parameters { TrajectoryParameters trajectory{}; SafetyParameters safety{}; CancelParameters cancel{}; + DelayParameters delay{}; // lane change parameters double backward_lane_length{200.0}; 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 29b3bf01ef1fc..24ca63f496938 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 @@ -131,14 +131,29 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); -bool passed_parked_objects( +/** + * @brief Checks if delaying of lane change maneuver is necessary + * + * @details Scans through the provided target objects (assumed to be ordered from closest to + * furthest), and returns true if any of the objects satisfy the following conditions: + * - Not near the end of current lanes + * - There is sufficient distance from object to next one to do lane change + * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects + * which pass isParkedObject() check will be considered. + * + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, + * and transient data. + * @param lane_change_path Candidate lane change path to apply checks on. + * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include + * target lane leading static objects). + * @param object_debug Collision check debug struct to be updated if any of the target objects + * satisfy the conditions. + * @return bool True if conditions to delay lane change are met + */ +bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug); - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); + const std::vector & target_objects, + CollisionCheckDebugMap & object_debug); lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); 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 9315549f5b586..05870cb35151a 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 @@ -99,12 +99,6 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } } - // parked vehicle detection - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); - p.th_object_shiftable_ratio = - getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); - // turn signal p.min_length_for_turn_signal_activation = getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); @@ -203,6 +197,15 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s "Lane change buffer must be more than 1 meter. Modifying the buffer."); } + // lane change delay + p.delay.enable = getOrDeclareParameter(*node, parameter("delay_lane_change.enable")); + p.delay.check_only_parked_vehicle = + getOrDeclareParameter(*node, parameter("delay_lane_change.check_only_parked_vehicle")); + p.delay.min_road_shoulder_width = + getOrDeclareParameter(*node, parameter("delay_lane_change.min_road_shoulder_width")); + p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( + *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); + // lane change cancel p.cancel.enable_on_prepare_phase = getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); 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 ee9c95bafb6c4..766b2f5725661 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 @@ -1338,7 +1338,7 @@ bool NormalLaneChange::check_candidate_path_safety( } if ( - !is_stuck && !utils::lane_change::passed_parked_objects( + !is_stuck && utils::lane_change::is_delay_lane_change( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, lane_change_debug_.collision_check_objects)) { throw std::logic_error( @@ -1519,10 +1519,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {false, true}; } - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data); - - if (!has_passed_parked_objects) { + if (utils::lane_change::is_delay_lane_change( + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data)) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); return {false, false}; } 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 c66b770cfa0a1..ae66cb704b824 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 @@ -671,9 +671,6 @@ bool isParkedObject( // +: object position // o: nearest point on centerline - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; - const double object_vel_norm = std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { @@ -692,49 +689,15 @@ bool isParkedObject( const double lat_dist = autoware::motion_utils::calcLateralOffset(path.points, object_pose.position); - lanelet::BasicLineString2d bound; - double center_to_bound_buffer = 0.0; - if (lat_dist > 0.0) { - // left side vehicle - const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet); - const auto most_left_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute most_left_sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - bound = most_left_lanelet.leftBound2d().basicLineString(); - if (most_left_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } else { - // right side vehicle - const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet); - const auto most_right_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages( - most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute most_right_sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - bound = most_right_lanelet.rightBound2d().basicLineString(); - if (most_right_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } + const auto most_side_lanelet = + lat_dist > 0.0 ? route_handler.getMostLeftLanelet(closest_lanelet, false, true) + : route_handler.getMostRightLanelet(closest_lanelet, false, true); + const auto bound = lat_dist > 0.0 ? most_side_lanelet.leftBound2d().basicLineString() + : most_side_lanelet.rightBound2d().basicLineString(); + const lanelet::Attribute lanelet_sub_type = + most_side_lanelet.attribute(lanelet::AttributeName::Subtype); + const auto center_to_bound_buffer = + lanelet_sub_type.value() == "road_shoulder" ? 0.0 : object_check_min_road_shoulder_width; return isParkedObject( closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold); @@ -779,129 +742,60 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } -bool passed_parked_objects( +bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug) + const ExtendedPredictedObjects & target_objects, CollisionCheckDebugMap & object_debug) { - const auto route_handler = *common_data_ptr->route_handler_ptr; - const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; - const auto & object_check_min_road_shoulder_width = - lane_change_parameters.object_check_min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = lane_change_parameters.th_object_shiftable_ratio; const auto & current_lane_path = common_data_ptr->current_lanes_path; + const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; - if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { - return true; - } - - const auto leading_obj_idx = getLeadingStaticObjectIdx( - route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold); - if (!leading_obj_idx) { - return true; - } - - const auto & leading_obj = objects.at(*leading_obj_idx); - auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); - const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); - if (leading_obj_poly.outer().empty()) { - return true; + if ( + !delay_lc_param.enable || target_objects.empty() || lane_change_path.path.points.empty() || + current_lane_path.points.empty()) { + return false; } - const auto & current_path_end = current_lane_path.points.back().point.pose.position; - const auto dist_to_path_end = [&](const auto & src_point) { - if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { - const auto goal_pose = route_handler.getGoalPose(); - return motion_utils::calcSignedArcLength( - current_lane_path.points, src_point, goal_pose.position); - } - return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); + const auto dist_to_end = common_data_ptr->transient_data.dist_to_terminal_end; + const auto dist_buffer = common_data_ptr->transient_data.current_dist_buffer.min; + auto is_near_end = [&dist_to_end, &dist_buffer](const ExtendedPredictedObject & obj) { + const auto dist_obj_to_end = dist_to_end - obj.dist_from_ego; + return dist_obj_to_end <= dist_buffer; }; - const auto min_dist_to_end_of_current_lane = std::invoke([&]() { - auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); - for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); - const auto dist = dist_to_path_end(obj_p); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - } - return min_dist_to_end_of_current_lane; - }); - - // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { - return true; - } - - const auto current_pose = common_data_ptr->get_ego_pose(); - const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); - - if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { - return true; - } - - debug.second.unsafe_reason = "delay lane change"; - utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return false; -} - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - 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() || objects.empty()) { - return {}; - } - - const auto & lane_change_start = lane_change_path.info.lane_changing_start; - const auto & path_end = path.points.back(); - - double dist_lc_start_to_leading_obj = 0.0; - std::optional leading_obj_idx = std::nullopt; - for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { - const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose; - - // ignore non-static object - // TODO(shimizu): parametrize threshold - const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); - if (obj_vel_norm > 1.0) { - continue; - } + const auto ego_vel = common_data_ptr->get_ego_speed(); + const auto min_lon_acc = common_data_ptr->lc_param_ptr->trajectory.min_longitudinal_acc; + const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc)); + auto is_sufficient_gap = [&gap_threshold](const auto & current_obj, const auto & next_obj) { + const auto curr_obj_half_length = current_obj.shape.dimensions.x; + const auto next_obj_half_length = next_obj.shape.dimensions.x; + const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; + const auto gap_length = dist_current_to_next - curr_obj_half_length - next_obj_half_length; + return gap_length > gap_threshold; + }; - // ignore non-parked object - if (!isParkedObject( - path, route_handler, obj, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold)) { - continue; - } + for (auto it = target_objects.begin(); it < target_objects.end(); ++it) { + if (is_near_end(*it)) break; - const double dist_back_to_obj = autoware::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; - } + if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue; - const double dist_lc_start_to_obj = autoware::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 + if ( + delay_lc_param.check_only_parked_vehicle && + !isParkedObject( + lane_change_path.path, *common_data_ptr->route_handler_ptr, *it, + delay_lc_param.min_road_shoulder_width, delay_lc_param.th_parked_vehicle_shift_ratio)) { 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; + auto next_it = std::next(it); + if (next_it == target_objects.end() || is_sufficient_gap(*it, *next_it)) { + auto debug = utils::path_safety_checker::createObjectDebug(*it); + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return true; } } - return leading_obj_idx; + return false; } lanelet::BasicPolygon2d create_polygon( From f935b5acc38e039eacdbceeb743f08c9a0b41d4f Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Thu, 5 Dec 2024 08:13:07 +0900 Subject: [PATCH 25/29] feat(lane_changing): improve computation of lane changing acceleration (#9545) * allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * run pre-commit check Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * fix format Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../README.md | 1 + .../config/lane_change.param.yaml | 1 + .../utils/calculation.hpp | 5 +++-- .../utils/parameters.hpp | 1 + .../src/manager.cpp | 4 ++++ .../src/utils/calculation.cpp | 14 ++++++++++---- 6 files changed, 20 insertions(+), 6 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 d24a89bf03000..499327e3b8483 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 @@ -893,6 +893,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | | `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | | `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `trajectory.lane_changing_decel_factor` | [-] | double | longitudinal deceleration factor during lane changing phase | 0.5 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | | `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | 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 ad2d72f0140d0..94a5e7f5d6f39 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 @@ -20,6 +20,7 @@ min_lane_changing_velocity: 2.78 lon_acc_sampling_num: 5 lat_acc_sampling_num: 3 + lane_changing_decel_factor: 0.5 # delay lane change delay_lane_change: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 4abe35206fd01..9e669e87aad0f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -106,8 +106,9 @@ double calc_ego_dist_to_lanes_start( const lanelet::ConstLanelets & target_lanes); double calc_lane_changing_acceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); + const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity, + const double max_path_velocity, const double lane_changing_time, + const double prepare_longitudinal_acc); /** * @brief Calculates the distance required during a lane change operation. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp index a7ea5a848b660..19f5293ef45e2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp @@ -122,6 +122,7 @@ struct TrajectoryParameters double th_prepare_length_diff{0.5}; double th_lane_changing_length_diff{0.5}; double min_lane_changing_velocity{5.6}; + double lane_changing_decel_factor{0.5}; int lon_acc_sampling_num{10}; int lat_acc_sampling_num{10}; LateralAccelerationMap lat_acc_map{}; 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 05870cb35151a..f597a71e41ed2 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 @@ -58,6 +58,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("trajectory.th_lane_changing_length_diff")); p.trajectory.min_lane_changing_velocity = getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); + p.trajectory.lane_changing_decel_factor = + getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); p.trajectory.lon_acc_sampling_num = getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); p.trajectory.lat_acc_sampling_num = @@ -317,6 +319,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.min_longitudinal_acc); updateParam( parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); + updateParam( + parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); int longitudinal_acc_sampling_num = 0; updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 7783079dd3bb4..d0f5e4ee2b12a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -380,11 +380,17 @@ std::vector calc_lon_acceleration_samples( } double calc_lane_changing_acceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc) + const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity, + const double max_path_velocity, const double lane_changing_time, + const double prepare_longitudinal_acc) { if (prepare_longitudinal_acc <= 0.0) { - return 0.0; + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto lane_changing_acc = + common_data_ptr->transient_data.is_ego_near_current_terminal_start + ? prepare_longitudinal_acc * params.lane_changing_decel_factor + : 0.0; + return lane_changing_acc; } return std::clamp( @@ -495,7 +501,7 @@ std::vector calc_shift_phase_metrics( shift_length, common_data_ptr->lc_param_ptr->trajectory.lateral_jerk, lat_acc); const double lane_changing_accel = calc_lane_changing_acceleration( - initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); + common_data_ptr, initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); const auto lane_changing_length = calculation::calc_phase_length( initial_velocity, max_vel, lane_changing_accel, lane_changing_duration); From 3791695ddf4d0e0545511c5a56cfc7ee6a5f6b9a Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Thu, 5 Dec 2024 09:05:24 +0900 Subject: [PATCH 26/29] feat(lane_change): reduce prepare duration when blinker has been activated (#9185) * add minimum prepare duration parameter Signed-off-by: mohammad alqudah * reduce prepare duration according to signal activation time Signed-off-by: mohammad alqudah * chore: update CODEOWNERS (#9203) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions * refactor(time_utils): prefix package and namespace with autoware (#9173) * refactor(time_utils): prefix package and namespace with autoware Signed-off-by: Esteve Fernandez * refactor(time_utils): prefix package and namespace with autoware Signed-off-by: Esteve Fernandez * style(pre-commit): autofix --------- Signed-off-by: Esteve Fernandez Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat(rtc_interface): add requested field (#9202) * add requested feature Signed-off-by: Go Sakayori * Update planning/autoware_rtc_interface/test/test_rtc_interface.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Go Sakayori Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * fix(mpc_lateral_controller): correctly resample the MPC trajectory yaws (#9199) Signed-off-by: Maxime CLEMENT * fix(bpp): prevent accessing nullopt (#9204) fix(bpp): calcDistanceToRedTrafficLight null Signed-off-by: Shumpei Wakabayashi * refactor(autoware_map_based_prediction): split pedestrian and bicycle predictor (#9201) * refactor: grouping functions Signed-off-by: Taekjin LEE * refactor: grouping parameters Signed-off-by: Taekjin LEE * refactor: rename member road_users_history to road_users_history_ Signed-off-by: Taekjin LEE * refactor: separate util functions Signed-off-by: Taekjin LEE * refactor: Add predictor_vru.cpp and utils.cpp to map_based_prediction_node Signed-off-by: Taekjin LEE * refactor: Add explicit template instantiation for removeOldObjectsHistory function Signed-off-by: Taekjin LEE * refactor: Add tf2_geometry_msgs to data_structure Signed-off-by: Taekjin LEE * refactor: Remove unused variables and functions in map_based_prediction_node.cpp Signed-off-by: Taekjin LEE * Update perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp * Apply suggestions from code review * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: Mamoru Sobue Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * refactor(ndt_scan_matcher, ndt_omp): move ndt_omp into ndt_scan_matcher (#8912) * Moved ndt_omp into ndt_scan_matcher Signed-off-by: Shintaro Sakoda * Added Copyright Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda * Fixed cast style Signed-off-by: Shintaro Sakoda * Fixed include Signed-off-by: Shintaro Sakoda * Fixed honorific title Signed-off-by: Shintaro Sakoda * Fixed honorific title Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed hierarchy Signed-off-by: Shintaro Sakoda * Fixed NVTP to NVTL Signed-off-by: Shintaro Sakoda * Added cspell:ignore Signed-off-by: Shintaro Sakoda * Fixed miss spell Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda * Renamed applyFilter Signed-off-by: Shintaro Sakoda * Moved ***_impl.hpp from include/ to src/ Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed variable scope Signed-off-by: Shintaro Sakoda * Fixed to pass by reference Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat(autoware_test_utils): add traffic light msgs parser (#9177) Signed-off-by: Mamoru Sobue * modify implementation to compute and keep actual prepare duration in transient data Signed-off-by: mohammad alqudah * if LC path is approved, set prepare duration in transient data from approved path prepare duration Signed-off-by: mohammad alqudah * change default value of LC param min_prepare_duration Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * add function to set signal activation time, add docstring for function calc_actual_prepare_duration Signed-off-by: mohammad alqudah * check for zero value max_acc to avoid division by zero Signed-off-by: mohammad alqudah * chore: rename codeowners file Signed-off-by: tomoya.kimura * chore: rename codeowners file Signed-off-by: tomoya.kimura * chore: rename codeowners file Signed-off-by: tomoya.kimura * allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * fix units Signed-off-by: mohammad alqudah * allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * run pre-commit check Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * fix format Signed-off-by: mohammad alqudah * allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * run pre-commit check Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * fix format Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Signed-off-by: Esteve Fernandez Signed-off-by: Go Sakayori Signed-off-by: Maxime CLEMENT Signed-off-by: Shumpei Wakabayashi Signed-off-by: Taekjin LEE Signed-off-by: Shintaro Sakoda Signed-off-by: Mamoru Sobue Signed-off-by: tomoya.kimura Co-authored-by: awf-autoware-bot[bot] <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Co-authored-by: github-actions Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Go Sakayori Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Taekjin LEE Co-authored-by: Mamoru Sobue Co-authored-by: SakodaShintaro Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: tomoya.kimura --- .github/{CODEOWNERS => _CODEOWNERS} | 0 .../README.md | 3 +- .../config/lane_change.param.yaml | 3 +- .../scene.hpp | 2 +- .../utils/base_class.hpp | 12 ++++++- .../utils/calculation.hpp | 20 +++++++++++- .../utils/data_structs.hpp | 2 ++ .../utils/parameters.hpp | 3 +- .../src/interface.cpp | 8 ++--- .../src/manager.cpp | 15 ++++++--- .../src/scene.cpp | 27 +++++++++++++--- .../src/utils/calculation.cpp | 31 ++++++++++++++++--- .../test/test_lane_change_scene.cpp | 4 +-- 13 files changed, 104 insertions(+), 26 deletions(-) rename .github/{CODEOWNERS => _CODEOWNERS} (100%) diff --git a/.github/CODEOWNERS b/.github/_CODEOWNERS similarity index 100% rename from .github/CODEOWNERS rename to .github/_CODEOWNERS 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 499327e3b8483..86d8d1c0c1413 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,7 +886,8 @@ 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 | -| `trajectory.prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `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 | | `trajectory.minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | | `trajectory.lon_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | 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 94a5e7f5d6f39..731bb9059768f 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 @@ -11,7 +11,8 @@ # trajectory generation trajectory: - prepare_duration: 4.0 + max_prepare_duration: 4.0 + min_prepare_duration: 2.0 lateral_jerk: 0.5 min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 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 550925c4a10fc..7202a1c6a9108 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 @@ -54,7 +54,7 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; - void update_transient_data() final; + void update_transient_data(const bool is_approved) final; void update_filtered_objects() final; 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 e5c7fcb6d621e..741b5afb50e08 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 @@ -68,7 +68,7 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; - virtual void update_transient_data() = 0; + virtual void update_transient_data(const bool is_approved) = 0; virtual void update_filtered_objects() = 0; @@ -267,6 +267,15 @@ class LaneChangeBase return turn_signal; } + void set_signal_activation_time(const bool reset = false) const + { + if (reset) { + signal_activation_time_ = std::nullopt; + } else if (!signal_activation_time_) { + signal_activation_time_ = clock_.now(); + } + } + LaneChangeStatus status_{}; PathShifter path_shifter_{}; @@ -292,6 +301,7 @@ class LaneChangeBase mutable StopWatch stop_watch_; mutable lane_change::Debug lane_change_debug_; + mutable std::optional signal_activation_time_{std::nullopt}; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 9e669e87aad0f..20bd04202d4de 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -75,7 +75,7 @@ double calc_dist_to_last_fit_width( * of the maximum lane change preparation duration and the maximum velocity of the ego vehicle. * * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change + * - `lc_param_ptr->maximum_prepare_duration`: The maximum duration allowed for lane change * preparation. * - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle. * @@ -131,6 +131,24 @@ std::vector calc_lon_acceleration_samples( const CommonDataPtr & common_data_ptr, const double max_path_velocity, const double prepare_duration); +/** + * @brief calculates the actual prepare duration that will be used for path generation + * + * @details computes the required prepare duration to be used for candidate path + * generation based on the elapsed time of turn signal activation, the minimum + * and maximum parameterized values for the prepare duration, + * and the minimum lane changing velocity. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which includes + * lane change parameters and bpp common parameters. + * @param current_velocity current ego vehicle velocity. + * @param active_signal_duration elapsed time since turn signal activation. + * @return The calculated prepare duration value in seconds (s) + */ +double calc_actual_prepare_duration( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double active_signal_duration); + std::vector calc_prepare_phase_metrics( const CommonDataPtr & common_data_ptr, const double current_velocity, const double max_path_velocity, const double min_length_threshold = 0.0, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index a83f78f616d4b..56e70d45a59e9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -222,6 +222,8 @@ struct TransientData size_t current_path_seg_idx; // index of nearest segment to ego along current path double current_path_velocity; // velocity of the current path at the ego position along the path + double lane_change_prepare_duration{0.0}; + bool is_ego_near_current_terminal_start{false}; bool is_ego_stuck{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp index 19f5293ef45e2..df458a84f49d3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp @@ -115,7 +115,8 @@ struct SafetyParameters struct TrajectoryParameters { - double prepare_duration{4.0}; + double max_prepare_duration{4.0}; + double min_prepare_duration{1.0}; double lateral_jerk{0.5}; double min_longitudinal_acc{-1.0}; double max_longitudinal_acc{1.0}; 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 817c42cee5440..e042bdb5868d3 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 @@ -77,7 +77,7 @@ void LaneChangeInterface::updateData() module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->update_filtered_objects(); - module_type_->update_transient_data(); + module_type_->update_transient_data(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -145,10 +145,10 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - *prev_approved_path_ = getPreviousModuleOutput().path; - BehaviorModuleOutput out = getPreviousModuleOutput(); + *prev_approved_path_ = 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(); @@ -158,7 +158,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + path_reference_ = std::make_shared(out.reference_path); stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { 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 f597a71e41ed2..1d2cb00fafe2a 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 @@ -44,8 +44,10 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // trajectory generation { - p.trajectory.prepare_duration = - getOrDeclareParameter(*node, parameter("trajectory.prepare_duration")); + p.trajectory.max_prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.max_prepare_duration")); + p.trajectory.min_prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.min_prepare_duration")); p.trajectory.lateral_jerk = getOrDeclareParameter(*node, parameter("trajectory.lateral_jerk")); p.trajectory.min_longitudinal_acc = @@ -66,8 +68,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("trajectory.lat_acc_sampling_num")); const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); - p.trajectory.min_lane_changing_velocity = - std::min(p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.prepare_duration); + p.trajectory.min_lane_changing_velocity = std::min( + p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.max_prepare_duration); // validation of trajectory parameters if (p.trajectory.lon_acc_sampling_num < 1 || p.trajectory.lat_acc_sampling_num < 1) { @@ -310,7 +312,10 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "prepare_duration", p->trajectory.prepare_duration); + updateParam( + parameters, ns + "max_prepare_duration", p->trajectory.max_prepare_duration); + updateParam( + parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration); updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); updateParam( parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); 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 766b2f5725661..933b9356f1c29 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 @@ -128,7 +128,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } -void NormalLaneChange::update_transient_data() +void NormalLaneChange::update_transient_data(const bool is_approved) { if ( !common_data_ptr_ || !common_data_ptr_->is_data_available() || @@ -147,6 +147,13 @@ void NormalLaneChange::update_transient_data() prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps; transient_data.current_path_seg_idx = nearest_seg_idx; + const auto active_signal_duration = + signal_activation_time_ ? (clock_.now() - signal_activation_time_.value()).seconds() : 0.0; + transient_data.lane_change_prepare_duration = + is_approved ? status_.lane_change_path.info.duration.prepare + : calculation::calc_actual_prepare_duration( + common_data_ptr_, common_data_ptr_->get_ego_speed(), active_signal_duration); + std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); @@ -325,6 +332,8 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const return get_terminal_turn_signal_info(); } + set_signal_activation_time(); + return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end); } @@ -353,9 +362,14 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx; - return getTurnSignalDecider().overwrite_turn_signal( + const auto turn_signal_info = getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); + + set_signal_activation_time( + turn_signal_info.turn_signal.command != terminal_turn_signal_info.turn_signal.command); + + return turn_signal_info; } LaneChangePath NormalLaneChange::getLaneChangePath() const @@ -426,8 +440,6 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path = utils::combinePath(output.path, *found_extended_path); } output.reference_path = getReferencePath(); - output.turn_signal_info = - get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); if (isStopState()) { const auto current_velocity = getEgoVelocity(); @@ -443,12 +455,17 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() extendOutputDrivableArea(output); + const auto turn_signal_info = + get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, + turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); + set_signal_activation_time( + output.turn_signal_info.turn_signal.command != turn_signal_info.turn_signal.command); + return output; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index d0f5e4ee2b12a..ad8279cde78ae 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -103,7 +103,7 @@ double calc_dist_to_last_fit_width( double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { - const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.prepare_duration; + const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration; const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel; return max_prepare_duration * ego_max_speed; @@ -195,7 +195,7 @@ std::vector calc_max_lane_change_lengths( const auto & params = common_data_ptr->lc_param_ptr->trajectory; const auto lat_jerk = params.lateral_jerk; - const auto t_prepare = params.prepare_duration; + const auto t_prepare = params.max_prepare_duration; const auto current_velocity = common_data_ptr->get_ego_speed(); const auto path_velocity = common_data_ptr->transient_data.current_path_velocity; @@ -398,18 +398,41 @@ double calc_lane_changing_acceleration( prepare_longitudinal_acc); } +double calc_actual_prepare_duration( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double active_signal_duration) +{ + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto min_lc_velocity = params.min_lane_changing_velocity; + + // need to ensure min prep duration is sufficient to reach minimum lane changing velocity + const auto min_prepare_duration = std::invoke([&]() -> double { + if (current_velocity >= min_lc_velocity) { + return params.min_prepare_duration; + } + const auto max_acc = + std::min(common_data_ptr->bpp_param_ptr->max_acc, params.max_longitudinal_acc); + if (max_acc < eps) { + return params.max_prepare_duration; + } + return (min_lc_velocity - current_velocity) / max_acc; + }); + + return std::max(params.max_prepare_duration - active_signal_duration, min_prepare_duration); +} + std::vector calc_prepare_durations(const CommonDataPtr & common_data_ptr) { const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto threshold = common_data_ptr->bpp_param_ptr->base_link2front + lc_param_ptr->min_length_for_turn_signal_activation; - const auto max_prepare_duration = lc_param_ptr->trajectory.prepare_duration; // TODO(Azu) this check seems to cause scenario failures. if (common_data_ptr->transient_data.dist_to_terminal_start >= threshold) { - return {max_prepare_duration}; + return {common_data_ptr->transient_data.lane_change_prepare_duration}; } + const auto max_prepare_duration = lc_param_ptr->trajectory.max_prepare_duration; std::vector prepare_durations; constexpr double step = 0.5; 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 01b4c451ebf9e..2c0a9c288af0a 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 @@ -202,7 +202,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenInvalid) constexpr auto is_approved = true; normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); - normal_lane_change_->update_transient_data(); + normal_lane_change_->update_transient_data(!is_approved); normal_lane_change_->updateLaneChangeStatus(); const auto & lc_status = normal_lane_change_->getLaneChangeStatus(); @@ -217,7 +217,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenValid) set_previous_approved_path(); normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); - normal_lane_change_->update_transient_data(); + normal_lane_change_->update_transient_data(!is_approved); const auto lane_change_required = normal_lane_change_->isLaneChangeRequired(); ASSERT_TRUE(lane_change_required); From ec210a254b3e03a6ce073d8ea2e94adc1c62502a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 9 Dec 2024 21:26:07 +0900 Subject: [PATCH 27/29] fix(lane_change): check obj predicted path when filtering (#9385) * RT1-8537 check object's predicted path when filtering Signed-off-by: Zulfaqar Azmi * use ranges view in get_line_string_paths Signed-off-by: Zulfaqar Azmi * check only vehicle type predicted path Signed-off-by: Zulfaqar Azmi * Refactor naming Signed-off-by: Zulfaqar Azmi * fix grammatical Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * precommit and grammar fix Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../utils/utils.hpp | 17 ++++++++++ .../src/scene.cpp | 1 + .../src/utils/utils.cpp | 34 +++++++++++-------- .../path_safety_checker/objects_filtering.hpp | 10 ++++++ .../path_safety_checker/objects_filtering.cpp | 14 ++++++++ 5 files changed, 62 insertions(+), 14 deletions(-) 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 24ca63f496938..40be2dee93d76 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 @@ -382,6 +382,23 @@ bool filter_target_lane_objects( const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, const bool before_terminal, TargetLaneLeadingObjects & leading_objects, ExtendedPredictedObjects & trailing_objects); + +/** + * @brief Determines if the object's predicted path overlaps with the given lane polygon. + * + * This function checks whether any of the line string paths derived from the object's predicted + * trajectories intersect or overlap with the specified polygon representing lanes. + * + * @param object The extended predicted object containing predicted trajectories and initial + * polygon. + * @param lanes_polygon A polygon representing the lanes to check for overlaps with the object's + * paths. + * + * @return true if any of the object's predicted paths overlap with the lanes polygon, false + * otherwise. + */ +bool object_path_overlaps_lanes( + const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon); } // 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/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 933b9356f1c29..cc3687438c442 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 @@ -985,6 +985,7 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects( FilteredLanesObjects NormalLaneChange::filter_objects() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->safety.target_object_types); 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 ae66cb704b824..52e8984fd576f 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 @@ -35,6 +35,8 @@ #include #include #include +#include +#include #include #include @@ -1149,11 +1151,8 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & return line_string; }; - const auto paths = object.predicted_paths; - std::vector line_strings; - std::transform(paths.begin(), paths.end(), std::back_inserter(line_strings), to_linestring_2d); - - return line_strings; + return object.predicted_paths | ranges::views::transform(to_linestring_2d) | + ranges::to(); } bool has_overtaking_turn_lane_object( @@ -1164,10 +1163,6 @@ bool has_overtaking_turn_lane_object( return false; } - const auto is_path_overlap_with_target = [&](const LineString2d & path) { - return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target); - }; - const auto is_object_overlap_with_target = [&](const auto & object) { // to compensate for perception issue, or if object is from behind ego, and tries to overtake, // but stop all of sudden @@ -1176,8 +1171,7 @@ bool has_overtaking_turn_lane_object( return true; } - const auto paths = get_line_string_paths(object); - return std::any_of(paths.begin(), paths.end(), is_path_overlap_with_target); + return object_path_overlaps_lanes(object, common_data_ptr->lanes_polygon_ptr->target); }; return std::any_of( @@ -1190,6 +1184,7 @@ bool filter_target_lane_objects( const bool before_terminal, TargetLaneLeadingObjects & leading_objects, ExtendedPredictedObjects & trailing_objects) { + using behavior_path_planner::utils::path_safety_checker::filter::is_vehicle; using behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m; @@ -1206,9 +1201,12 @@ bool filter_target_lane_objects( const auto is_stopped = velocity_filter( object.initial_twist, -std::numeric_limits::epsilon(), stopped_obj_vel_th); if (is_lateral_far && before_terminal) { - const auto in_target_lanes = - !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target); - if (in_target_lanes) { + const auto overlapping_with_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target) || + (!is_stopped && is_vehicle(object.classification) && + object_path_overlaps_lanes(object, lanes_polygon.target)); + + if (overlapping_with_target_lanes) { if (!ahead_of_ego && !is_stopped) { trailing_objects.push_back(object); return true; @@ -1247,4 +1245,12 @@ bool filter_target_lane_objects( return false; } + +bool object_path_overlaps_lanes( + const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) +{ + return ranges::any_of(get_line_string_paths(object), [&](const auto & path) { + return !boost::geometry::disjoint(path, lanes_polygon); + }); +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index a10534ddf60bb..fec2a23bc2652 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -49,6 +49,16 @@ bool position_filter( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); +/** + * @brief Checks if the object classification represents a vehicle (CAR, TRUCK, BUS, TRAILER, + * MOTORCYCLE). + * + * @param classification The object classification to check. + * @return true If the classification is a vehicle type. + * @return false Otherwise. + */ +bool is_vehicle(const ObjectClassification & classification); + } // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index be888bba001b9..94c6c393ad96b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -51,6 +51,20 @@ bool is_within_circle( std::hypot(reference_point.x - object_pos.x, reference_point.y - object_pos.y); return dist < search_radius; } + +bool is_vehicle(const ObjectClassification & classification) +{ + switch (classification.label) { + case ObjectClassification::CAR: + case ObjectClassification::TRUCK: + case ObjectClassification::BUS: + case ObjectClassification::TRAILER: + case ObjectClassification::MOTORCYCLE: + return true; + default: + return false; + } +} } // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter namespace autoware::behavior_path_planner::utils::path_safety_checker From 491c3f50014c53fd32c7805a318d8c2a9a2c75a8 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 12 Dec 2024 16:31:09 +0900 Subject: [PATCH 28/29] fix(lane_change): remove overlapping preceding lanes (#9526) * fix(lane_change): remove overlapping preceding lanes Signed-off-by: Zulfaqar Azmi * fix cpp check Signed-off-by: Zulfaqar Azmi * start searching disconnected lanes directly Signed-off-by: Zulfaqar Azmi * just remove starting from overlapped found Signed-off-by: Zulfaqar Azmi * return non reversed lanes Signed-off-by: Zulfaqar Azmi * fix precommit Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 16 +++++++ .../src/scene.cpp | 5 +-- .../src/utils/utils.cpp | 43 ++++++++++++++++++- 3 files changed, 60 insertions(+), 4 deletions(-) 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 40be2dee93d76..313df7a1d2ae6 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 @@ -383,6 +383,22 @@ bool filter_target_lane_objects( const bool before_terminal, TargetLaneLeadingObjects & leading_objects, ExtendedPredictedObjects & trailing_objects); +/** + * @brief Retrieves the preceding lanes for the target lanes while removing overlapping and + * disconnected lanes. + * + * This function identifies all lanes that precede the target lanes based on the ego vehicle's + * current position and a specified backward search length. The resulting preceding lanes are + * filtered to remove lanes that overlap with the current lanes or are not connected to the route. + * + * @param common_data_ptr Shared pointer to commonly used data in lane change module, which contains + * route handler information, lane details, ego vehicle pose, and behavior parameters. + * + * @return A vector of preceding lanelet groups, with each group containing only the connected and + * non-overlapping preceding lanes. + */ +std::vector get_preceding_lanes(const CommonDataPtr & common_data_ptr); + /** * @brief Determines if the object's predicted path overlaps with the given lane polygon. * 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 cc3687438c442..30df66b2f8542 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 @@ -109,9 +109,8 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(target_lanes.back()); - common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( - *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), - common_data_ptr_->lc_param_ptr->backward_lane_length); + common_data_ptr_->lanes_ptr->preceding_target = + utils::lane_change::get_preceding_lanes(common_data_ptr_); lane_change_debug_.current_lanes = common_data_ptr_->lanes_ptr->current; lane_change_debug_.target_lanes = common_data_ptr_->lanes_ptr->target; 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 52e8984fd576f..035e572026de1 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 @@ -34,9 +34,12 @@ #include #include #include +#include #include +#include +#include #include -#include +#include #include #include @@ -56,6 +59,7 @@ #include #include #include +#include #include namespace autoware::behavior_path_planner::utils::lane_change @@ -1246,6 +1250,43 @@ bool filter_target_lane_objects( return false; } +std::vector get_preceding_lanes(const CommonDataPtr & common_data_ptr) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & ego_pose = common_data_ptr->get_ego_pose(); + const auto backward_lane_length = common_data_ptr->lc_param_ptr->backward_lane_length; + + const auto preceding_lanes_list = + utils::getPrecedingLanelets(*route_handler_ptr, target_lanes, ego_pose, backward_lane_length); + + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + std::unordered_set current_lanes_id; + for (const auto & lane : current_lanes) { + current_lanes_id.insert(lane.id()); + } + const auto is_overlapping = [&](const lanelet::ConstLanelet & lane) { + return current_lanes_id.find(lane.id()) != current_lanes_id.end(); + }; + + std::vector non_overlapping_lanes_vec; + for (const auto & lanes : preceding_lanes_list) { + auto lanes_reversed = lanes | ranges::views::reverse; + auto overlapped_itr = ranges::find_if(lanes_reversed, is_overlapping); + + if (overlapped_itr == lanes_reversed.begin()) { + continue; + } + + // Lanes are not reversed by default. Avoid returning reversed lanes to prevent undefined + // behavior. + lanelet::ConstLanelets non_overlapping_lanes(overlapped_itr.base(), lanes.end()); + non_overlapping_lanes_vec.push_back(non_overlapping_lanes); + } + + return non_overlapping_lanes_vec; +} + bool object_path_overlaps_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) { From 066279a37a3fea2dfe09474fb1fb8c79a9b9047a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 12 Dec 2024 17:39:23 +0900 Subject: [PATCH 29/29] refactor(lane_change): separate structs to different folders (#9625) Signed-off-by: Zulfaqar Azmi --- .../{utils => }/base_class.hpp | 24 +++++++++---------- .../interface.hpp | 6 ++--- .../manager.hpp | 2 +- .../scene.hpp | 4 ++-- .../data_structs.hpp => structs/data.hpp} | 15 ++++++------ .../debug_structs.hpp => structs/debug.hpp} | 10 ++++---- .../{utils => structs}/parameters.hpp | 6 ++--- .../{utils => structs}/path.hpp | 17 +++++++------ .../utils/calculation.hpp | 2 +- .../utils/markers.hpp | 4 ++-- .../utils/utils.hpp | 4 ++-- .../src/scene.cpp | 2 +- .../src/utils/markers.cpp | 1 - .../src/utils/utils.cpp | 4 ++-- .../test/test_lane_change_scene.cpp | 2 +- .../test/test_lane_change_utils.cpp | 2 +- 16 files changed, 52 insertions(+), 53 deletions(-) rename planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/{utils => }/base_class.hpp (93%) rename planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/{utils/data_structs.hpp => structs/data.hpp} (95%) rename planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/{utils/debug_structs.hpp => structs/debug.hpp} (87%) rename planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/{utils => structs}/parameters.hpp (95%) rename planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/{utils => structs}/path.hpp (78%) 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/base_class.hpp similarity index 93% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index 741b5afb50e08..5317b94db2d6b 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/base_class.hpp @@ -11,12 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__BASE_CLASS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__BASE_CLASS_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/debug_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/debug.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" @@ -276,20 +276,20 @@ class LaneChangeBase } } - LaneChangeStatus status_{}; + LaneChangeStatus status_; PathShifter path_shifter_{}; LaneChangeStates current_lane_change_state_{}; - std::shared_ptr lane_change_parameters_{}; - std::shared_ptr abort_path_{}; - std::shared_ptr planner_data_{}; - lane_change::CommonDataPtr common_data_ptr_{}; + std::shared_ptr lane_change_parameters_; + std::shared_ptr abort_path_; + std::shared_ptr planner_data_; + lane_change::CommonDataPtr common_data_ptr_; FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; - PathWithLaneId prev_approved_path_{}; + PathWithLaneId prev_approved_path_; int unsafe_hysteresis_count_{0}; bool is_abort_path_approved_{false}; @@ -311,4 +311,4 @@ class LaneChangeBase friend class ::TestNormalLaneChange; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index eb30adef4eba4..ee6d27e2edd11 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#include "autoware/behavior_path_lane_change_module/base_class.hpp" #include "autoware/behavior_path_lane_change_module/scene.hpp" -#include "autoware/behavior_path_lane_change_module/utils/base_class.hpp" -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp index 2a1862de6a27e..124579735e5d8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "autoware/route_handler/route_handler.hpp" 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 7202a1c6a9108..9bf66722dcec2 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 @@ -14,8 +14,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/base_class.hpp" -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/base_class.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp similarity index 95% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index 56e70d45a59e9..26044924e5fb4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -11,10 +11,10 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/parameters.hpp" +#include "autoware/behavior_path_lane_change_module/structs/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -72,6 +72,7 @@ struct PhaseMetrics double actual_lon_accel{0.0}; double lat_accel{0.0}; + PhaseMetrics() = default; PhaseMetrics( const double _duration, const double _length, const double _velocity, const double _sampled_lon_accel, const double _actual_lon_accel, const double _lat_accel) @@ -103,10 +104,10 @@ struct Info PhaseInfo duration{0.0, 0.0}; PhaseInfo length{0.0, 0.0}; - Pose lane_changing_start{}; - Pose lane_changing_end{}; + Pose lane_changing_start; + Pose lane_changing_end; - ShiftLine shift_line{}; + ShiftLine shift_line; double lateral_acceleration{0.0}; double terminal_lane_changing_velocity{0.0}; @@ -294,4 +295,4 @@ using FilteredLanesObjects = lane_change::FilteredLanesObjects; using LateralAccelerationMap = lane_change::LateralAccelerationMap; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp similarity index 87% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index a28b8523a75c7..ea9807ad1616f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -11,11 +11,11 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include @@ -76,4 +76,4 @@ struct Debug }; } // namespace autoware::behavior_path_planner::lane_change -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp similarity index 95% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index df458a84f49d3..7e0beea10a91e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -177,4 +177,4 @@ struct Parameters } // namespace autoware::behavior_path_planner::lane_change -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp similarity index 78% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp index 5623f03a22eb3..0fa0a9d977a26 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PATH_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PATH_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -29,19 +29,18 @@ using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; struct Path { - PathWithLaneId path{}; - ShiftedPath shifted_path{}; - Info info{}; + PathWithLaneId path; + ShiftedPath shifted_path; + Info info; }; struct Status { - Path lane_change_path{}; + Path lane_change_path; bool is_safe{false}; bool is_valid_path{false}; double start_distance{0.0}; }; - } // namespace autoware::behavior_path_planner::lane_change namespace autoware::behavior_path_planner @@ -50,4 +49,4 @@ using LaneChangePath = lane_change::Path; using LaneChangePaths = std::vector; using LaneChangeStatus = lane_change::Status; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 20bd04202d4de..1312ea06c4435 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index c95b388a2e4a4..4c2712f053b13 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/debug_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/debug.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include 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 313df7a1d2ae6..ce9ab331d90a1 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 @@ -15,8 +15,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" 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 30df66b2f8542..adbf9ea52364e 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 @@ -678,7 +678,7 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; - status_ = {}; + status_ = LaneChangeStatus(); unsafe_hysteresis_count_ = 0; lane_change_debug_.reset(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 9158a9bd38b23..5195d4ba267fa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -14,7 +14,6 @@ #include "autoware/behavior_path_planner_common/marker_utils/colors.hpp" #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include #include 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 035e572026de1..e30e5c4ff2f83 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 @@ -14,8 +14,8 @@ #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" -#include "autoware/behavior_path_lane_change_module/utils/path.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" 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 2c0a9c288af0a..5ca308d0d4abb 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 @@ -14,7 +14,7 @@ #include "autoware/behavior_path_lane_change_module/manager.hpp" #include "autoware/behavior_path_lane_change_module/scene.hpp" -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" #include "autoware_test_utils/mock_data_parser.hpp" diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 68547a491324d..147a53672f30a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include #include