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 1a2e4439d4b7d..25a155c324a55 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 @@ -20,6 +20,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include #include #include #include @@ -280,7 +281,7 @@ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const return std::numeric_limits::infinity(); } - return utils::lane_change::calcMinimumLaneChangeLength( + return utils::lane_change::calculation::calc_minimum_lane_change_length( getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); } 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 b43914ec1cc45..5f1c3debabcf8 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 @@ -11,7 +11,9 @@ The Lane Change module is activated when lane change is needed and can be safely - `allow_lane_change` tags is set as `true` - During lane change request condition - The ego-vehicle isn’t on a `preferred_lane`. - - There is neither intersection nor crosswalk on the path of the lane change + - The ego-vehicle isn't approaching a traffic light. (condition parameterized) + - The ego-vehicle isn't approaching a crosswalk. (condition parameterized) + - The ego-vehicle isn't approaching an intersection. (condition parameterized) - lane change ready condition - Path of the lane change does not collide with other dynamic objects (see the figure below) - Lane change candidate path is approved by an operator. @@ -182,11 +184,9 @@ A candidate path is considered valid if it meets the following criteria: 1. The distance from the ego vehicle's current position to the end of the current lanes is sufficient to perform a single lane change. 2. The distance from the ego vehicle's current position to the goal along the current lanes is adequate to complete multiple lane changes. 3. The distance from the ego vehicle's current position to the end of the target lanes is adequate for completing multiple lane changes. -4. Intersection requirements are met (conditions are parameterized). -5. Crosswalk requirements are satisfied (conditions are parameterized). -6. Traffic light regulations are adhered to (conditions are parameterized). -7. The lane change can be completed after passing a parked vehicle. -8. The lane change is deemed safe to execute. +4. The distance from the ego vehicle's current position to the next regulatory element is adequate to perform a single lane change. +5. The lane change can be completed after passing a parked vehicle. +6. The lane change is deemed safe to execute. The following flow chart illustrates the validity check. @@ -231,43 +231,6 @@ group check for distance #LightYellow endif end group - - -group evaluate on Crosswalk #LightCyan -if (regulate_on_crosswalk and not enough length to crosswalk) then (yes) - if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) - #LightPink:Reject path; - stop - else (no) - :Allow lane change in crosswalk; - endif -else (no) -endif -end group - -group evaluate on Intersection #LightGreen -if (regulate_on_intersection and not enough length to intersection) then (yes) - if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) - #LightPink:Reject path; - stop - else (no) - :Allow lane change in intersection; - endif -else (no) -endif -end group - -group evaluate on Traffic Light #Lavender -if (regulate_on_traffic_light and not enough length to complete lane change before stop line) then (yes) - #LightPink:Reject path; - stop -elseif (stopped at red traffic light within distance) then (yes) - #LightPink:Reject path; - stop -else (no) -endif -end group - if (ego is not stuck but parked vehicle exists in target lane) then (yes) #LightPink:Reject path; stop @@ -517,8 +480,8 @@ The function to stop by keeping a margin against forward obstacle in the previou ### Lane change regulations -If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. -To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`. +If you want to regulate lane change on crosswalks, intersections, or traffic lights, the lane change module is disabled near any of them. +To regulate lane change on crosswalks, intersections, or traffic lights, set `regulation.crosswalk`, `regulation.intersection` or `regulation.traffic_light` to `true`. If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswalk/intersection. If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck. If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping. 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 24257e8855e33..3f4f290663a3f 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 @@ -109,6 +109,8 @@ class NormalLaneChange : public LaneChangeBase bool isStoppedAtRedTrafficLight() const override; + bool is_near_regulatory_element() const final; + TurnSignalInfo get_current_turn_signal_info() const final; protected: 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 51d5dbe0f195c..b237368692312 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 @@ -231,6 +231,8 @@ class LaneChangeBase virtual TurnSignalInfo get_current_turn_signal_info() const = 0; + virtual bool is_near_regulatory_element() const = 0; + protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 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 819cccd9365b6..32b104eb23abb 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 @@ -16,8 +16,12 @@ #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include + namespace autoware::behavior_path_planner::utils::lane_change::calculation { +using autoware::route_handler::Direction; +using autoware::route_handler::RouteHandler; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LCParamPtr; @@ -113,6 +117,21 @@ double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr); 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); + +double calc_maximum_lane_change_length( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, + const double max_acc); } // 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/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 83043aa061f81..84c3f310f0382 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 @@ -60,17 +60,6 @@ using tier4_planning_msgs::msg::PathWithLaneId; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); -double calcMinimumLaneChangeLength( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); - -double calcMinimumLaneChangeLength( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction); - -double calcMaximumLaneChangeLength( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc); - double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); @@ -305,6 +294,10 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co ExtendedPredictedObjects transform_to_extended_objects( const CommonDataPtr & common_data_ptr, const std::vector & objects, const bool check_prepare_phase); + +double get_distance_to_next_regulatory_element( + const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, + const bool ignore_intersection = 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_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 7f30f34a8f633..61daf1183e993 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 @@ -147,6 +147,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() *prev_approved_path_ = getPreviousModuleOutput().path; BehaviorModuleOutput out = getPreviousModuleOutput(); + module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); @@ -249,6 +250,13 @@ bool LaneChangeInterface::canTransitFailureState() } if (isWaitingApproval()) { + if (module_type_->is_near_regulatory_element()) { + log_debug_throttled("Ego is close to regulatory element. Cancel lane change"); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); + return true; + } log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); return false; } 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 01bdbe77f1c99..e59119230c224 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 @@ -42,7 +42,6 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; -using utils::lane_change::calcMinimumLaneChangeLength; using utils::lane_change::create_lanes_polygon; using utils::path_safety_checker::isPolygonOverlapLanelet; using utils::traffic_light::getDistanceToNextTrafficLight; @@ -171,7 +170,46 @@ bool NormalLaneChange::isLaneChangeRequired() 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_); - return ego_dist_to_target_start <= maximum_prepare_length; + if (ego_dist_to_target_start > maximum_prepare_length) { + return false; + } + + if (is_near_regulatory_element()) { + RCLCPP_DEBUG(logger_, "Ego is close to regulatory element, don't run LC module"); + return false; + } + + return true; +} + +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 (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; + + if (dist_to_terminal_start <= max_prepare_length) return false; + + const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + + if (only_tl) { + 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); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -216,7 +254,7 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back()); const double next_lane_change_buffer = - utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); + 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 + @@ -371,7 +409,7 @@ void NormalLaneChange::insertStopPoint( const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); const auto lane_change_buffer = - calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -451,7 +489,7 @@ void NormalLaneChange::insertStopPoint( 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); const double lane_change_buffer_for_blocking_object = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + 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 - @@ -668,7 +706,7 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & route_handler = getRouteHandler(); const auto & current_pose = getEgoPose(); - const auto lane_change_buffer = calcMinimumLaneChangeLength( + const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); const auto distance_to_lane_change_end = std::invoke([&]() { @@ -785,7 +823,7 @@ bool NormalLaneChange::is_near_terminal() const 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 = calcMinimumLaneChangeLength( + 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); @@ -896,16 +934,6 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons return {min_acc, max_acc}; } -double NormalLaneChange::calcMaximumLaneChangeLength( - const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const -{ - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); - return utils::lane_change::calcMaximumLaneChangeLength( - std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()), - *lane_change_parameters_, shift_intervals, max_acc); -} - std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { @@ -928,7 +956,8 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // calculate maximum lane change length - const double max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); + const double max_lane_change_length = + 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)) { RCLCPP_DEBUG( @@ -1295,8 +1324,9 @@ bool NormalLaneChange::hasEnoughLength( 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 = calcMinimumLaneChangeLength( - route_handler, target_lanes.back(), *lane_change_parameters_, direction); + const auto minimum_lane_change_length_to_preferred_lane = + calculation::calc_minimum_lane_change_length( + route_handler, target_lanes.back(), *lane_change_parameters_, direction); const double lane_change_length = path.info.length.sum(); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { @@ -1400,10 +1430,10 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + 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 = utils::lane_change::calcMinimumLaneChangeLength( + const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( *lane_change_parameters_, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); @@ -1433,6 +1463,11 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", prepare_durations.size(), longitudinal_acc_sampling_values.size()); + 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_); + for (const auto & prepare_duration : prepare_durations) { for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { // get path on original lanes @@ -1557,6 +1592,14 @@ bool NormalLaneChange::getLaneChangePaths( 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; + } + // 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; @@ -1631,41 +1674,6 @@ bool NormalLaneChange::getLaneChangePaths( continue; } - if ( - lane_change_parameters_->regulate_on_crosswalk && - !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { - if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print_lat("Reject: including crosswalk!!"); - continue; - } - RCLCPP_INFO_THROTTLE( - logger_, clock_, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); - } - - if ( - lane_change_parameters_->regulate_on_intersection && - !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { - if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print_lat("Reject: including intersection!!"); - continue; - } - RCLCPP_WARN_STREAM( - logger_, "Stop time is over threshold. Allow lane change in intersection."); - } - - if ( - lane_change_parameters_->regulate_on_traffic_light && - !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { - debug_print_lat("Reject: regulate on traffic light!!"); - continue; - } - - if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - get_current_lanes(), candidate_path.value().path, planner_data_, - lane_change_info.length.sum())) { - debug_print_lat("Ego is stopping near traffic light. Do not allow lane change"); - continue; - } candidate_paths->push_back(*candidate_path); if ( @@ -1724,10 +1732,10 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + 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 = utils::lane_change::calcMinimumLaneChangeLength( + const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( *lane_change_parameters_, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); @@ -1863,7 +1871,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + const auto min_lc_length = calculation::calc_minimum_lane_change_length( *lane_change_parameters_, common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); @@ -1985,7 +1993,7 @@ bool NormalLaneChange::calcAbortPath() const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto direction = getDirection(); - const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( + const auto minimum_lane_change_length = calculation::calc_minimum_lane_change_length( route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); const auto & lane_changing_path = selected_path.path; @@ -2305,7 +2313,7 @@ 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 = calcMinimumLaneChangeLength( + const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); 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; @@ -2342,7 +2350,8 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan } const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); + const auto max_lane_change_length = + calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); const auto rss_dist = calcRssDistance( 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); 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 ef1648103ddde..c5424f69f84e0 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 @@ -141,4 +141,94 @@ 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) +{ + 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); +} + +double calc_maximum_lane_change_length( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, + const double max_acc) +{ + 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); +} } // 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 297d04a657cea..6ff69abb85d18 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 @@ -22,6 +22,7 @@ #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" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" @@ -85,82 +86,6 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -double calcMinimumLaneChangeLength( - 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 calcMinimumLaneChangeLength( - 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 utils::lane_change::calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals); -} - -double calcMaximumLaneChangeLength( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc) -{ - 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); -} - double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, const LaneChangeParameters & lane_change_parameters) @@ -626,7 +551,7 @@ bool hasEnoughLengthToLaneChangeAfterAbort( return false; } - const auto minimum_lane_change_length = calcMinimumLaneChangeLength( + const auto minimum_lane_change_length = calculation::calc_minimum_lane_change_length( route_handler, current_lanes.back(), lane_change_parameters, direction); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { @@ -1321,6 +1246,33 @@ ExtendedPredictedObjects transform_to_extended_objects( return extended_objects; } + +double get_distance_to_next_regulatory_element( + const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk, + const bool ignore_intersection) +{ + double distance = std::numeric_limits::max(); + + const auto current_pose = common_data_ptr->get_ego_pose(); + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + + if (!ignore_intersection && common_data_ptr->lc_param_ptr->regulate_on_intersection) { + distance = + std::min(distance, utils::getDistanceToNextIntersection(current_pose, current_lanes)); + } + if (!ignore_crosswalk && common_data_ptr->lc_param_ptr->regulate_on_crosswalk) { + distance = std::min( + distance, utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr)); + } + if (common_data_ptr->lc_param_ptr->regulate_on_traffic_light) { + distance = std::min( + distance, utils::traffic_light::getDistanceToNextTrafficLight(current_pose, current_lanes)); + } + + return distance; +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug