From 66cd111d4a542a31efdc9293e9460ee888f3aa8b Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 26 Aug 2024 16:21:11 +0900 Subject: [PATCH 01/26] refactor lane change utility funcions Signed-off-by: mohammad alqudah --- .../src/scene.cpp | 3 +- .../utils/calculation.hpp | 19 ++++ .../utils/utils.hpp | 11 --- .../src/scene.cpp | 46 ++++------ .../src/utils/calculation.cpp | 90 +++++++++++++++++++ .../src/utils/utils.cpp | 78 +--------------- 6 files changed, 131 insertions(+), 116 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 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/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 ca13318aeeb81..878ea571e3dd9 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); 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..4d5546be48b69 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; @@ -216,7 +215,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 +370,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 +450,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 +667,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 +784,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 +895,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 +917,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 +1285,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 +1391,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())); @@ -1724,10 +1715,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 +1854,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 +1976,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 +2296,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 +2333,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 e4a482def7b3d..f470b74b8abe9 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 @@ -85,82 +85,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) @@ -655,7 +579,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)) { From 4f9bc8b55d0ba7a0b2642b31680886f7a9ccf3f5 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Tue, 27 Aug 2024 09:00:00 +0900 Subject: [PATCH 02/26] LC utility function to get distance to next regulatory element Signed-off-by: mohammad alqudah --- .../utils/utils.hpp | 2 ++ .../src/utils/utils.cpp | 26 +++++++++++++++++++ 2 files changed, 28 insertions(+) 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 878ea571e3dd9..2f76fc56fb8aa 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 @@ -305,6 +305,8 @@ 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); } // 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/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index f470b74b8abe9..6c4c329e841b9 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" @@ -1290,6 +1291,31 @@ ExtendedPredictedObjects transform_to_extended_objects( return extended_objects; } + +double get_distance_to_next_regulatory_element(const CommonDataPtr & common_data_ptr) +{ + 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 (common_data_ptr->lc_param_ptr->regulate_on_intersection) { + distance = + std::min(distance, utils::getDistanceToNextIntersection(current_pose, current_lanes)); + } + if (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 From bf2344527fb7cc8da6ba2fac867c1c4c6d275c06 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Tue, 27 Aug 2024 09:06:38 +0900 Subject: [PATCH 03/26] don't activate LC module when close to regulatory element Signed-off-by: mohammad alqudah --- .../scene.hpp | 2 ++ .../utils/base_class.hpp | 2 ++ .../src/interface.cpp | 10 +++++++++ .../src/scene.cpp | 21 ++++++++++++++++++- 4 files changed, 34 insertions(+), 1 deletion(-) 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..fe59ba9a76d4b 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_too_close_to_regulatory_element() const override; + 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..887c8745d1756 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_too_close_to_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/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 979004d439bdc..2d7fcc42355c6 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,16 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() *prev_approved_path_ = getPreviousModuleOutput().path; BehaviorModuleOutput out = getPreviousModuleOutput(); + + if (module_type_->is_too_close_to_regulatory_element()) { + module_type_->toCancelState(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); + path_candidate_ = std::make_shared(); + return out; + } + module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); 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 4d5546be48b69..458c476e55254 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 @@ -170,7 +170,26 @@ 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; + } + + return !is_too_close_to_regulatory_element(); +} + +bool NormalLaneChange::is_too_close_to_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()); + const double min_lc_length = + calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); + + return min_lc_length > + utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const From 06db58dd2b541a07cdb99bc6167f236f652dfb72 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Tue, 27 Aug 2024 17:02:21 +0900 Subject: [PATCH 04/26] modify threshold distance calculation Signed-off-by: mohammad alqudah --- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 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 458c476e55254..0a405160ba81e 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 @@ -185,10 +185,11 @@ bool NormalLaneChange::is_too_close_to_regulatory_element() const const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double min_lc_length = + double threshold_distance = calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); + threshold_distance += calculation::calc_maximum_prepare_length(common_data_ptr_); - return min_lc_length > + return threshold_distance > utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_); } From c05c05008796d86a06161d4380892d98add385b2 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 28 Aug 2024 10:10:10 +0900 Subject: [PATCH 05/26] move regulatory element check to canTransitFailureState() function Signed-off-by: mohammad alqudah --- .../src/interface.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) 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 2d7fcc42355c6..0069801a5e8ff 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 @@ -148,15 +148,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() BehaviorModuleOutput out = getPreviousModuleOutput(); - if (module_type_->is_too_close_to_regulatory_element()) { - module_type_->toCancelState(); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - path_candidate_ = std::make_shared(); - return out; - } - module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); @@ -262,6 +253,12 @@ bool LaneChangeInterface::canTransitFailureState() } if (isWaitingApproval()) { + if (module_type_->is_too_close_to_regulatory_element()) { + 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; } From ac2311130791de779ddb7c4a7d574baecf37a5b4 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 2 Sep 2024 15:19:05 +0900 Subject: [PATCH 06/26] always run LC module if approaching terminal point Signed-off-by: mohammad alqudah --- .../utils/utils.hpp | 4 +++- .../src/scene.cpp | 13 +++++++++++-- .../src/utils/utils.cpp | 8 +++++--- 3 files changed, 19 insertions(+), 6 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 2f76fc56fb8aa..f45f0123ebd51 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 @@ -306,7 +306,9 @@ 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); +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/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 0a405160ba81e..bcd25da3755ac 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 @@ -189,8 +189,17 @@ bool NormalLaneChange::is_too_close_to_regulatory_element() const calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); threshold_distance += calculation::calc_maximum_prepare_length(common_data_ptr_); - return threshold_distance > - utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_); + const auto dist_from_ego_to_terminal_end = + calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + + if (dist_from_ego_to_terminal_end <= threshold_distance) { + return false; + } + + const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + + return threshold_distance > utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const 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 6c4c329e841b9..fe09f01c98db0 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 @@ -1292,7 +1292,9 @@ ExtendedPredictedObjects transform_to_extended_objects( return extended_objects; } -double get_distance_to_next_regulatory_element(const CommonDataPtr & common_data_ptr) +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(); @@ -1301,11 +1303,11 @@ double get_distance_to_next_regulatory_element(const CommonDataPtr & common_data const auto & route_handler = *common_data_ptr->route_handler_ptr; const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - if (common_data_ptr->lc_param_ptr->regulate_on_intersection) { + if (!ignore_intersection && common_data_ptr->lc_param_ptr->regulate_on_intersection) { distance = std::min(distance, utils::getDistanceToNextIntersection(current_pose, current_lanes)); } - if (common_data_ptr->lc_param_ptr->regulate_on_crosswalk) { + 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)); } From 6eef49e5916ac373fa4f72ff29c88d86114c6bfe Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 9 Sep 2024 09:14:41 +0900 Subject: [PATCH 07/26] use max possible LC length as threshold Signed-off-by: mohammad alqudah --- .../scene.hpp | 2 +- .../utils/base_class.hpp | 2 +- .../utils/calculation.hpp | 4 ++ .../src/interface.cpp | 3 +- .../src/scene.cpp | 57 ++++++------------- .../src/utils/calculation.cpp | 14 +++++ 6 files changed, 40 insertions(+), 42 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 fe59ba9a76d4b..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,7 +109,7 @@ class NormalLaneChange : public LaneChangeBase bool isStoppedAtRedTrafficLight() const override; - bool is_too_close_to_regulatory_element() const override; + bool is_near_regulatory_element() const final; TurnSignalInfo get_current_turn_signal_info() const 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 887c8745d1756..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,7 +231,7 @@ class LaneChangeBase virtual TurnSignalInfo get_current_turn_signal_info() const = 0; - virtual bool is_too_close_to_regulatory_element() 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 32b104eb23abb..da8d39f7a1b0b 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 @@ -132,6 +132,10 @@ 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); + +double calc_lane_change_length( + const LaneChangeParameters & lane_change_parameters, const double velocity, + const double acceleration, const double shift_distance); } // 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/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 4797d9b036539..d8de2eb882448 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 @@ -253,7 +253,8 @@ bool LaneChangeInterface::canTransitFailureState() } if (isWaitingApproval()) { - if (module_type_->is_too_close_to_regulatory_element()) { + 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); 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 bcd25da3755ac..e779d46e6aadf 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 @@ -174,10 +174,15 @@ bool NormalLaneChange::isLaneChangeRequired() return false; } - return !is_too_close_to_regulatory_element(); + 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_too_close_to_regulatory_element() const +bool NormalLaneChange::is_near_regulatory_element() const { const auto & current_lanes = get_current_lanes(); @@ -185,8 +190,13 @@ bool NormalLaneChange::is_too_close_to_regulatory_element() const const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back()); - double threshold_distance = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); + + if (shift_intervals.empty()) return false; + + const double max_vel = getCommonParam().max_vel; + const auto & lc_params = *common_data_ptr_->lc_param_ptr; + auto threshold_distance = + calculation::calc_lane_change_length(lc_params, max_vel, 0.0, shift_intervals.front()); threshold_distance += calculation::calc_maximum_prepare_length(common_data_ptr_); const auto dist_from_ego_to_terminal_end = @@ -198,6 +208,10 @@ bool NormalLaneChange::is_too_close_to_regulatory_element() const 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 threshold_distance > utils::lane_change::get_distance_to_next_regulatory_element( common_data_ptr_, only_tl, only_tl); } @@ -1651,41 +1665,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 ( 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 c5424f69f84e0..7eaf10db44aae 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 @@ -231,4 +231,18 @@ 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_lane_change_length( + const LaneChangeParameters & lane_change_parameters, const double velocity, + const double acceleration, const double shift_distance) +{ + const auto lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const auto [min_lat_acc, max_lat_acc] = + lane_change_parameters.lane_change_lat_acc_map.find(velocity); + const auto t_lane_changing = + PathShifter::calcShiftTimeFromJerk(shift_distance, lateral_jerk, max_lat_acc); + const auto lane_changing_length = + (velocity * t_lane_changing) + (0.5 * acceleration * t_lane_changing * t_lane_changing); + return lane_changing_length; +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation From 8c22547ddd317ac5cc6a98f8d630ab61b5760f39 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 9 Sep 2024 09:44:40 +0900 Subject: [PATCH 08/26] update LC readme Signed-off-by: mohammad alqudah --- .../README.md | 52 +++---------------- 1 file changed, 7 insertions(+), 45 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 b43914ec1cc45..56a9061441602 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,8 @@ 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 lane change can be completed after passing a parked vehicle. +5. The lane change is deemed safe to execute. The following flow chart illustrates the validity check. @@ -231,43 +230,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 +479,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. From d2f0459cae2a4d9e0e469e3ef63c039846ac9706 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 9 Sep 2024 15:00:06 +0900 Subject: [PATCH 09/26] refactor implementation Signed-off-by: mohammad alqudah --- .../utils/calculation.hpp | 4 --- .../src/scene.cpp | 28 +++++++++++-------- .../src/utils/calculation.cpp | 14 ---------- 3 files changed, 17 insertions(+), 29 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 da8d39f7a1b0b..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 @@ -132,10 +132,6 @@ 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); - -double calc_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const double velocity, - const double acceleration, const double shift_distance); } // 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/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index e779d46e6aadf..5fd58ebb5a1ef 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 @@ -193,18 +193,14 @@ bool NormalLaneChange::is_near_regulatory_element() const if (shift_intervals.empty()) return false; - const double max_vel = getCommonParam().max_vel; const auto & lc_params = *common_data_ptr_->lc_param_ptr; - auto threshold_distance = - calculation::calc_lane_change_length(lc_params, max_vel, 0.0, shift_intervals.front()); - threshold_distance += calculation::calc_maximum_prepare_length(common_data_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_from_ego_to_terminal_end = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); - - if (dist_from_ego_to_terminal_end <= threshold_distance) { - return false; - } + if (dist_to_terminal_start <= max_prepare_length) return false; const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; @@ -212,7 +208,7 @@ bool NormalLaneChange::is_near_regulatory_element() const RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); } - return threshold_distance > utils::lane_change::get_distance_to_next_regulatory_element( + return max_prepare_length > utils::lane_change::get_distance_to_next_regulatory_element( common_data_ptr_, only_tl, only_tl); } @@ -1467,6 +1463,10 @@ 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); + for (const auto & prepare_duration : prepare_durations) { for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { // get path on original lanes @@ -1591,6 +1591,12 @@ bool NormalLaneChange::getLaneChangePaths( continue; } + if (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; 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 7eaf10db44aae..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 @@ -231,18 +231,4 @@ 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_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const double velocity, - const double acceleration, const double shift_distance) -{ - const auto lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto [min_lat_acc, max_lat_acc] = - lane_change_parameters.lane_change_lat_acc_map.find(velocity); - const auto t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_distance, lateral_jerk, max_lat_acc); - const auto lane_changing_length = - (velocity * t_lane_changing) + (0.5 * acceleration * t_lane_changing * t_lane_changing); - return lane_changing_length; -} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation From 2d22773020839ee69b166ffb611ca5e24a31dbd4 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 9 Sep 2024 16:15:51 +0900 Subject: [PATCH 10/26] update readme Signed-off-by: mohammad alqudah --- .../autoware_behavior_path_lane_change_module/README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 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 56a9061441602..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 @@ -184,8 +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. The lane change can be completed after passing a parked vehicle. -5. 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. From 7fa64f96ba39e94c694619b34963307c0fd78d5c Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Tue, 10 Sep 2024 10:59:51 +0900 Subject: [PATCH 11/26] refactor checking data validity Signed-off-by: mohammad alqudah --- .../src/scene.cpp | 54 +++++++++---------- 1 file changed, 26 insertions(+), 28 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 01bdbe77f1c99..ff5f5d98497f6 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 @@ -1378,10 +1378,21 @@ bool NormalLaneChange::getLaneChangePaths( 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."); + + 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; @@ -1415,13 +1426,6 @@ bool NormalLaneChange::getLaneChangePaths( 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); @@ -1711,10 +1715,20 @@ bool NormalLaneChange::getLaneChangePaths( 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; @@ -1736,13 +1750,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()); @@ -1789,15 +1796,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}; @@ -1809,7 +1807,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 " From b08abaff2e7b4e5cd3ac3f44aa42bfa6699bde71 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 11 Sep 2024 09:54:58 +0900 Subject: [PATCH 12/26] refactor sampling of prepare phase metrics and lane changing phase metrics Signed-off-by: mohammad alqudah --- .../utils/calculation.hpp | 33 ++ .../utils/data_structs.hpp | 9 + .../utils/utils.hpp | 21 -- .../src/scene.cpp | 353 +++++++----------- .../src/utils/calculation.cpp | 143 +++++++ .../src/utils/utils.cpp | 23 -- 6 files changed, 325 insertions(+), 257 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 32b104eb23abb..1c98fc6092a31 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 194412dfe01b7..c68ff3556f304 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 @@ -192,6 +192,15 @@ struct PhaseInfo } }; +struct PhaseMetrics +{ + double duration; + double length; + double velocity; + double lon_accel; + double lat_accel; +}; + struct Lanes { bool current_lane_in_goal_section{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 84c3f310f0382..268293d594c02 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 @@ -68,10 +68,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); @@ -254,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 ccdacb70785cb..f81cd4e58c85b 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 @@ -1428,8 +1428,6 @@ bool NormalLaneChange::getLaneChangePaths( 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 @@ -1448,8 +1446,10 @@ 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); @@ -1471,245 +1471,172 @@ bool NormalLaneChange::getLaneChangePaths( const auto dist_to_next_regulatory_element = utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - 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); + 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.lon_accel, prep_metric.length); + }; - const auto prepare_length = utils::lane_change::calcPhaseLength( - current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); + auto prepare_segment = + getPrepareSegment(current_lanes, backward_path_length, prep_metric.length); - 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 (prepare_segment.points.empty()) { + debug_print("prepare segment is empty...? Unexpected."); + continue; + } - 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; - } - } - auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + 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; + } - const auto debug_print = [&](const auto & 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); - }; + // 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); - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); - continue; - } + // 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; + } - 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; - } + debug_print("Prepare path satisfy constraints"); - // 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); + const auto initial_lane_changing_velocity = prep_metric.velocity; + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + const auto max_lane_changing_length = + std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length; - // 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; - } + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, initial_lane_changing_velocity, max_path_velocity, + prep_metric.lon_accel, max_lane_changing_length); - 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 initial_lane_changing_velocity = prepare_velocity; - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + for (const auto & lc_metric : lane_changing_metrics) { + const auto debug_print_lat = [&](const auto & s) { + RCLCPP_DEBUG( + logger_, " - %s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s, + lc_metric.duration, lc_metric.lon_accel, lc_metric.lat_accel, lc_metric.length); + }; - // 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; + // 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 lc_metric.length + finish_judge_buffer + backward_buffer_to_target_lane + + next_lane_change_buffer; + }); - 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 (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_, " - 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 (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; - 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; - } + const auto terminal_lane_changing_velocity = std::min( + initial_lane_changing_velocity + lc_metric.lon_accel * lc_metric.duration, + getCommonParam().max_vel); + utils::lane_change::setPrepareVelocity( + prepare_segment, current_velocity, terminal_lane_changing_velocity); - 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); + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose, target_lane_length, lc_metric.length, + initial_lane_changing_velocity, next_lane_change_buffer); - 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); + if (target_segment.points.empty()) { + debug_print_lat("Reject: target segment is empty!! something wrong..."); + continue; + } - if (target_segment.points.empty()) { - debug_print_lat("Reject: target segment is empty!! something wrong..."); - continue; - } + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = + LaneChangePhaseInfo{prep_metric.lon_accel, lc_metric.lon_accel}; + lane_change_info.duration = LaneChangePhaseInfo{prep_metric.duration, lc_metric.duration}; + lane_change_info.velocity = + LaneChangePhaseInfo{prep_metric.velocity, initial_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{prep_metric.length, lc_metric.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 = lc_metric.lat_accel; + lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; + + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lc_metric.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, lc_metric.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; + } - 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; - } + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + prepare_segment, target_segment, target_lane_reference_path, shift_length); - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, 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); - 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); + if (!candidate_path) { + debug_print_lat("Reject: failed to generate candidate path!!"); + continue; + } - if (!candidate_path) { - debug_print_lat("Reject: failed to generate candidate path!!"); - continue; - } + if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { + debug_print_lat("Reject: invalid candidate path!!"); + continue; + } - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print_lat("Reject: invalid candidate path!!"); - continue; - } + candidate_paths->push_back(*candidate_path); - candidate_paths->push_back(*candidate_path); + 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; + } - 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; - } + 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); - 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, + 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); - - 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; + return safety_check_with_stuck_rss.is_safe; } - debug_print_lat("Reject: sampled path is not safe."); + return safety_check_with_normal_rss.is_safe; + }); + + if (is_safe) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; } + + debug_print_lat("Reject: sampled path is not safe."); } } 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 c5424f69f84e0..83a8f6bc385cb 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,13 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation { + +rclcpp::Logger get_logger() +{ + constexpr const char * name{"lane_change.calculation"}; + return rclcpp::get_logger(name); +} + double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) { const auto & lanes_ptr = common_data_ptr->lanes_ptr; @@ -231,4 +238,140 @@ 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; + } + + if (metrics.empty()) return false; + + const auto length_diff = std::abs(metrics.back().length - prepare_length); + if (length_diff < common_data_ptr->lc_param_ptr->skip_process_lon_diff_th_prepare) { + RCLCPP_DEBUG(get_logger(), "Skip: Change in prepare length is less than 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.push_back({prepare_duration, prepare_length, prepare_velocity, 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_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; + } + + if (metrics.empty()) return false; + + const auto length_diff = std::abs(metrics.back().length - lane_changing_length); + if (length_diff < common_data_ptr->lc_param_ptr->skip_process_lon_diff_th_prepare) { + RCLCPP_DEBUG(get_logger(), "Skip: Change in lane changing length is less than 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_velocity, lane_changing_duration, lon_accel); + + const auto lane_changing_length = calculation::calc_phase_length( + initial_velocity, common_data_ptr->bpp_param_ptr->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.push_back( + {lane_changing_duration, lane_changing_length, lane_changing_velocity, 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 6ff69abb85d18..11bb1b8c2ef33 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 @@ -105,19 +105,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) { @@ -1075,16 +1062,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 bf9b3e35afa3ff14ada858b1231eb109bcc931b1 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 13 Sep 2024 14:23:57 +0900 Subject: [PATCH 13/26] add route handler function to get pose from 2d arc length Signed-off-by: mohammad alqudah --- .../autoware/route_handler/route_handler.hpp | 3 +++ .../src/route_handler.cpp | 27 +++++++++++++++++++ 2 files changed, 30 insertions(+) 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 13e13b642af1f..8b8fca4818b52 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 @@ -326,6 +326,9 @@ class RouteHandler */ lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; + Pose getPoseFrom2DArcLength( + 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 0c0d14f95c768..0a38149b386f8 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -47,6 +47,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; @@ -2035,4 +2037,29 @@ std::optional RouteHandler::findDrivableLanePath( if (route) return route->shortestPath(); return {}; } + +Pose RouteHandler::getPoseFrom2DArcLength( + 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(); it < centerline.end() - 1; ++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 From 06c01fed41c72fae00cf86fd62c00f00659aa4e3 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 13 Sep 2024 14:36:51 +0900 Subject: [PATCH 14/26] refactor candidate path generation Signed-off-by: mohammad alqudah --- .../scene.hpp | 10 +- .../utils/data_structs.hpp | 27 ++- .../utils/utils.hpp | 7 +- .../src/scene.cpp | 183 +++++++++--------- .../src/utils/calculation.cpp | 7 +- .../src/utils/utils.cpp | 18 +- 6 files changed, 124 insertions(+), 128 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 3f4f290663a3f..2721680416475 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 @@ -162,9 +162,13 @@ class NormalLaneChange : public LaneChangeBase 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 getLaneChangePaths(LaneChangePaths * candidate_paths) const; + + LaneChangePath getCandidatePath( + 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; 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/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 c68ff3556f304..566288dd7bffc 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 @@ -194,11 +194,11 @@ struct PhaseInfo struct PhaseMetrics { - double duration; - double length; - double velocity; - double lon_accel; - double lat_accel; + double duration{0.0}; + double length{0.0}; + double velocity{0.0}; + double lon_accel{0.0}; + double lat_accel{0.0}; }; struct Lanes @@ -224,6 +224,22 @@ 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.lon_accel, _lc_metrics.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 @@ -325,6 +341,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 268293d594c02..578392fdc3612 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 @@ -92,14 +92,9 @@ bool isPathInLanelets( std::optional constructCandidatePath( 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( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); 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 f81cd4e58c85b..5439efe991f78 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 = getLaneChangePaths(&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; @@ -1403,12 +1401,14 @@ bool NormalLaneChange::hasEnoughLengthToTrafficLight( 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 +bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) 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); @@ -1426,10 +1426,6 @@ bool NormalLaneChange::getLaneChangePaths( 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 lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; - // get velocity const auto current_velocity = getEgoVelocity(); @@ -1461,7 +1457,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); candidate_paths->reserve( - longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + 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", @@ -1483,7 +1480,7 @@ bool NormalLaneChange::getLaneChangePaths( }; auto prepare_segment = - getPrepareSegment(current_lanes, backward_path_length, prep_metric.length); + getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); if (prepare_segment.points.empty()) { debug_print("prepare segment is empty...? Unexpected."); @@ -1513,101 +1510,50 @@ bool NormalLaneChange::getLaneChangePaths( const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - const auto max_lane_changing_length = - std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length; - const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( - common_data_ptr_, shift_length, initial_lane_changing_velocity, max_path_velocity, - prep_metric.lon_accel, max_lane_changing_length); + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); 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 = + std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - 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; + } + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_length; + }); + + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, initial_lane_changing_velocity, max_path_velocity, + prep_metric.lon_accel, max_lane_changing_length); + for (const auto & lc_metric : lane_changing_metrics) { - const auto debug_print_lat = [&](const auto & s) { + const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, " - %s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s, + logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), lc_metric.duration, lc_metric.lon_accel, lc_metric.lat_accel, lc_metric.length); }; - // 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 lc_metric.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; - } - - const auto terminal_lane_changing_velocity = std::min( - initial_lane_changing_velocity + lc_metric.lon_accel * lc_metric.duration, - getCommonParam().max_vel); - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); - - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose, target_lane_length, lc_metric.length, - initial_lane_changing_velocity, next_lane_change_buffer); - - if (target_segment.points.empty()) { - debug_print_lat("Reject: target segment is empty!! something wrong..."); - continue; - } - - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{prep_metric.lon_accel, lc_metric.lon_accel}; - lane_change_info.duration = LaneChangePhaseInfo{prep_metric.duration, lc_metric.duration}; - lane_change_info.velocity = - LaneChangePhaseInfo{prep_metric.velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prep_metric.length, lc_metric.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 = lc_metric.lat_accel; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lc_metric.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, lc_metric.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; - } - - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, 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); - - if (!candidate_path) { - debug_print_lat("Reject: failed to generate candidate path!!"); - continue; - } - - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print_lat("Reject: invalid candidate path!!"); + LaneChangePath candidate_path; + try { + candidate_path = getCandidatePath( + 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; } - candidate_paths->push_back(*candidate_path); + candidate_paths->push_back(candidate_path); 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, 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 " @@ -1618,12 +1564,12 @@ bool NormalLaneChange::getLaneChangePaths( 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, + 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, + 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; } @@ -1644,6 +1590,53 @@ bool NormalLaneChange::getLaneChangePaths( return false; } +LaneChangePath NormalLaneChange::getCandidatePath( + 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 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_lane_reference_path.points.empty()) { + throw std::logic_error("target_lane_reference_path is empty!"); + } + + 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.getPoseFrom2DArcLength(target_lanes, dist_to_lc_end); + }); + + const auto shift_line = utils::lane_change::getLaneChangingShiftLine( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); + + LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; + + const auto candidate_path = utils::lane_change::constructCandidatePath( + common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); + + if (!candidate_path) { + throw std::logic_error("failed to generate candidate path!"); + } + + if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + throw std::logic_error("invalid candidate path length!"); + } + + return *candidate_path; +} + std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { @@ -1773,8 +1766,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( 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); + 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 83a8f6bc385cb..cedb575ccad2a 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 @@ -317,7 +317,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_length_threshold) + 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; @@ -356,11 +356,10 @@ std::vector calc_shift_phase_metrics( 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_velocity, lane_changing_duration, lon_accel); + initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); const auto lane_changing_length = calculation::calc_phase_length( - initial_velocity, common_data_ptr->bpp_param_ptr->max_vel, lane_changing_accel, - lane_changing_duration); + initial_velocity, max_vel, lane_changing_accel, lane_changing_duration); if (is_skip(lane_changing_length)) continue; 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 11bb1b8c2ef33..19de7636d79a0 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 @@ -201,8 +201,7 @@ bool isPathInLanelets( std::optional constructCandidatePath( 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; @@ -254,8 +253,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 @@ -325,17 +324,6 @@ 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( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length) From 066fb7c29464c29e52bc116f9c0f030ef7159f58 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 13 Sep 2024 17:36:53 +0900 Subject: [PATCH 15/26] refactor candidate path safety check Signed-off-by: mohammad alqudah --- .../scene.hpp | 4 ++ .../src/scene.cpp | 63 +++++++++++-------- 2 files changed, 41 insertions(+), 26 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 2721680416475..44c4d994fbd93 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 @@ -170,6 +170,10 @@ class NormalLaneChange : public LaneChangeBase 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 checkCandidatePathSafety( + 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, const lanelet::ConstLanelets & target_lanes) const; 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 5439efe991f78..551bfd62ecb63 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 @@ -1487,6 +1487,8 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con 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"); @@ -1506,8 +1508,6 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con debug_print("Prepare path satisfy constraints"); - const auto initial_lane_changing_velocity = prep_metric.velocity; - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); @@ -1528,8 +1528,9 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con 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, initial_lane_changing_velocity, max_path_velocity, + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, prep_metric.lon_accel, max_lane_changing_length); for (const auto & lc_metric : lane_changing_metrics) { @@ -1551,32 +1552,15 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con candidate_paths->push_back(candidate_path); - 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."); + bool is_safe = false; + try { + is_safe = + checkCandidatePathSafety(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 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; @@ -1637,6 +1621,33 @@ LaneChangePath NormalLaneChange::getCandidatePath( return *candidate_path; } +bool NormalLaneChange::checkCandidatePathSafety( + 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."); + } + + 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; +} + std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { From 5274a1ede4d15d88883e4a1ae2d1ff94dc75922f Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Thu, 19 Sep 2024 13:44:24 +0900 Subject: [PATCH 16/26] fix variable name Signed-off-by: mohammad alqudah --- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 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 88145ac87aef0..99e9cf4baae1b 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 @@ -1519,7 +1519,7 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con const auto max_lane_changing_length = std::invoke([&]() { double max_length = - ego_dist_to_terminal_start > max_prepare_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 = From d0b83cfe80acae66f51fbda4a5b4fab847b5ade9 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Thu, 19 Sep 2024 13:47:52 +0900 Subject: [PATCH 17/26] Update planning/autoware_route_handler/src/route_handler.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- planning/autoware_route_handler/src/route_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 0a38149b386f8..075710c061687 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -2044,7 +2044,7 @@ Pose RouteHandler::getPoseFrom2DArcLength( double accumulated_distance2d = 0; for (const auto & llt : lanelet_sequence) { const auto & centerline = llt.centerline(); - for (auto it = centerline.begin(); it < centerline.end() - 1; ++it) { + 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)); From a510fcd6a2f689a23ba41c4c98aeff1111c7bd71 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 20 Sep 2024 11:28:14 +0900 Subject: [PATCH 18/26] correct parameter name Signed-off-by: mohammad alqudah --- .../src/utils/calculation.cpp | 2 +- 1 file changed, 1 insertion(+), 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 cedb575ccad2a..75f4268cff72d 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 @@ -342,7 +342,7 @@ std::vector calc_shift_phase_metrics( if (metrics.empty()) return false; const auto length_diff = std::abs(metrics.back().length - lane_changing_length); - if (length_diff < common_data_ptr->lc_param_ptr->skip_process_lon_diff_th_prepare) { + if (length_diff < common_data_ptr->lc_param_ptr->skip_process_lon_diff_th_lane_changing) { RCLCPP_DEBUG(get_logger(), "Skip: Change in lane changing length is less than threshold."); return true; } From b65477f0af4637d5c71d7064331389ecbfb0c3b7 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 20 Sep 2024 11:49:09 +0900 Subject: [PATCH 19/26] set prepare segment velocity after taking max path velocity value Signed-off-by: mohammad alqudah --- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 99e9cf4baae1b..6a8ec70aa96d4 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 @@ -1512,8 +1512,6 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); - 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); @@ -1536,6 +1534,8 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, prep_metric.lon_accel, max_lane_changing_length); + 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( From da1ac8f611d123f0fa125625d2feb3682e0e4145 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 20 Sep 2024 16:04:55 +0900 Subject: [PATCH 20/26] update LC README Signed-off-by: mohammad alqudah --- .../README.md | 102 ++++++++++++++++++ 1 file changed, 102 insertions(+) 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..d52faec37bee7 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) + :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) + :Return first valid path; + stop + else (no) + :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) + :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) + :Return first valid path; + stop +else (no) +endif + +: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) + :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) + :Return valid candidate path; + stop +else (no) + :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. From 766a439e7015fd30600679f92c3bfd51aa524854 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 20 Sep 2024 16:48:58 +0900 Subject: [PATCH 21/26] minor changes Signed-off-by: mohammad alqudah --- .../behavior_path_lane_change_module/scene.hpp | 2 +- .../utils/data_structs.hpp | 11 +++++++++++ .../src/scene.cpp | 8 ++++---- .../src/utils/calculation.cpp | 11 ++++++----- .../src/utils/utils.cpp | 3 ++- 5 files changed, 24 insertions(+), 11 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 44c4d994fbd93..6325fae529929 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 @@ -162,7 +162,7 @@ class NormalLaneChange : public LaneChangeBase bool hasEnoughLengthToTrafficLight( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; - bool getLaneChangePaths(LaneChangePaths * candidate_paths) const; + bool getLaneChangePaths(LaneChangePaths & candidate_paths) const; LaneChangePath getCandidatePath( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_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 566288dd7bffc..a2b38fb27ab7a 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 @@ -199,6 +199,17 @@ struct PhaseMetrics double velocity{0.0}; double lon_accel{0.0}; double lat_accel{0.0}; + + PhaseMetrics( + const double _duration, const double _length, const double _velocity, const double _lon_accel, + const double _lat_accel) + : duration(_duration), + length(_length), + velocity(_velocity), + lon_accel(_lon_accel), + lat_accel(_lat_accel) + { + } }; struct Lanes 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 6a8ec70aa96d4..fdc5838910194 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 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) } LaneChangePaths valid_paths{}; - bool found_safe_path = getLaneChangePaths(&valid_paths); + bool found_safe_path = getLaneChangePaths(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; @@ -1401,7 +1401,7 @@ bool NormalLaneChange::hasEnoughLengthToTrafficLight( dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; } -bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) const +bool NormalLaneChange::getLaneChangePaths(LaneChangePaths & candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); @@ -1456,7 +1456,7 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - candidate_paths->reserve( + candidate_paths.reserve( longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * prepare_durations.size()); @@ -1553,7 +1553,7 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths * candidate_paths) con continue; } - candidate_paths->push_back(candidate_path); + candidate_paths.push_back(candidate_path); bool is_safe = false; try { 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 75f4268cff72d..8d9ef27bbe0c6 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 @@ -24,7 +24,8 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation rclcpp::Logger get_logger() { constexpr const char * name{"lane_change.calculation"}; - return rclcpp::get_logger(name); + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; } double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) @@ -308,7 +309,7 @@ std::vector calc_prepare_phase_metrics( if (is_skip(prepare_length)) continue; - metrics.push_back({prepare_duration, prepare_length, prepare_velocity, prepare_accel, 0.0}); + metrics.emplace_back(prepare_duration, prepare_length, prepare_velocity, prepare_accel, 0.0); } } @@ -366,9 +367,9 @@ std::vector calc_shift_phase_metrics( const auto lane_changing_velocity = std::clamp( initial_velocity + lane_changing_accel * lane_changing_duration, min_lc_vel, max_vel); - metrics.push_back( - {lane_changing_duration, lane_changing_length, lane_changing_velocity, lane_changing_accel, - lat_acc}); + metrics.emplace_back( + lane_changing_duration, lane_changing_length, lane_changing_velocity, lane_changing_accel, + lat_acc); } return metrics; 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 19de7636d79a0..5db545735973a 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( From 55f330da6c955df97231f7f877d0caeb7f9d787b Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 25 Sep 2024 09:01:57 +0900 Subject: [PATCH 22/26] check phase length difference with previos valid candidate path Signed-off-by: mohammad alqudah --- .../utils/data_structs.hpp | 13 +++++--- .../src/scene.cpp | 31 +++++++++++++++++-- .../src/utils/calculation.cpp | 25 +++------------ .../src/utils/utils.cpp | 2 +- 4 files changed, 41 insertions(+), 30 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 0854e3dff2e0b..351132d9f8ce5 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 @@ -198,16 +198,18 @@ struct PhaseMetrics double duration{0.0}; double length{0.0}; double velocity{0.0}; - double lon_accel{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 _lon_accel, - const double _lat_accel) + 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), - lon_accel(_lon_accel), + sampled_lon_accel(_sampled_lon_accel), + actual_lon_accel(_actual_lon_accel), lat_accel(_lat_accel) { } @@ -242,7 +244,8 @@ struct 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.lon_accel, _lc_metrics.lon_accel}; + 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}; 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 fdc5838910194..d92e7194b3454 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 @@ -1473,13 +1473,33 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths & candidate_paths) con 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) { + 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 (!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; + }; + 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.lon_accel, prep_metric.length); + 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); @@ -1532,7 +1552,7 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths & candidate_paths) con 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.lon_accel, max_lane_changing_length); + prep_metric.sampled_lon_accel, max_lane_changing_length); utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); @@ -1540,9 +1560,14 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths & candidate_paths) con const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), - lc_metric.duration, lc_metric.lon_accel, lc_metric.lat_accel, lc_metric.length); + lc_metric.duration, lc_metric.actual_lon_accel, lc_metric.lat_accel, lc_metric.length); }; + 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; + } + LaneChangePath candidate_path; try { candidate_path = getCandidatePath( 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 8d9ef27bbe0c6..05ee759b401a1 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 @@ -281,15 +281,6 @@ std::vector calc_prepare_phase_metrics( prepare_length, min_length_threshold, max_length_threshold); return true; } - - if (metrics.empty()) return false; - - const auto length_diff = std::abs(metrics.back().length - prepare_length); - if (length_diff < common_data_ptr->lc_param_ptr->skip_process_lon_diff_th_prepare) { - RCLCPP_DEBUG(get_logger(), "Skip: Change in prepare length is less than threshold."); - return true; - } - return false; }; @@ -309,7 +300,8 @@ std::vector calc_prepare_phase_metrics( if (is_skip(prepare_length)) continue; - metrics.emplace_back(prepare_duration, prepare_length, prepare_velocity, prepare_accel, 0.0); + metrics.emplace_back( + prepare_duration, prepare_length, prepare_velocity, lon_accel, prepare_accel, 0.0); } } @@ -339,15 +331,6 @@ std::vector calc_shift_phase_metrics( lane_changing_length, max_length_threshold); return true; } - - if (metrics.empty()) return false; - - const auto length_diff = std::abs(metrics.back().length - lane_changing_length); - if (length_diff < common_data_ptr->lc_param_ptr->skip_process_lon_diff_th_lane_changing) { - RCLCPP_DEBUG(get_logger(), "Skip: Change in lane changing length is less than threshold."); - return true; - } - return false; }; @@ -368,8 +351,8 @@ std::vector calc_shift_phase_metrics( 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, lane_changing_accel, - lat_acc); + lane_changing_duration, lane_changing_length, lane_changing_velocity, lon_accel, + lane_changing_accel, lat_acc); } return metrics; 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 9209e7303c7c5..920ff4c4d7edb 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 @@ -132,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; From fa5a5fef90a399fdfc648c074b463d67941cd655 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 25 Sep 2024 11:17:09 +0900 Subject: [PATCH 23/26] change logger name Signed-off-by: mohammad alqudah --- .../src/utils/calculation.cpp | 2 +- 1 file changed, 1 insertion(+), 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 05ee759b401a1..bda3fc4e54ea2 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 @@ -23,7 +23,7 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation rclcpp::Logger get_logger() { - constexpr const char * name{"lane_change.calculation"}; + constexpr const char * name{"lane_change.utils"}; static rclcpp::Logger logger = rclcpp::get_logger(name); return logger; } From f64d80a77bd023a229e3bba40a73953df831661b Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 25 Sep 2024 14:42:50 +0900 Subject: [PATCH 24/26] change functions names to snake case Signed-off-by: mohammad alqudah --- .../scene.hpp | 15 +---- .../src/scene.cpp | 64 ++----------------- 2 files changed, 9 insertions(+), 70 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 6325fae529929..9463c4442645c 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,24 +153,15 @@ 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; - - bool hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; - - bool getLaneChangePaths(LaneChangePaths & candidate_paths) const; - - LaneChangePath getCandidatePath( + 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 checkCandidatePathSafety( + 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; 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 d92e7194b3454..3d098807fc1d5 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 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) } LaneChangePaths valid_paths{}; - bool found_safe_path = getLaneChangePaths(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; @@ -1349,59 +1349,7 @@ bool NormalLaneChange::hasEnoughLength( return true; } -bool NormalLaneChange::hasEnoughLengthToCrosswalk( - 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 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; - } - - return true; -} - -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 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 - if ( - dist_to_intersection_from_lane_change_start_pose > 0.0 && - dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { - 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(LaneChangePaths & candidate_paths) const +bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); @@ -1570,7 +1518,7 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths & candidate_paths) con LaneChangePath candidate_path; try { - candidate_path = getCandidatePath( + 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) { @@ -1583,7 +1531,7 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths & candidate_paths) con bool is_safe = false; try { is_safe = - checkCandidatePathSafety(candidate_path, target_objects, lane_change_buffer, is_stuck); + 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; @@ -1602,7 +1550,7 @@ bool NormalLaneChange::getLaneChangePaths(LaneChangePaths & candidate_paths) con return false; } -LaneChangePath NormalLaneChange::getCandidatePath( +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, @@ -1649,7 +1597,7 @@ LaneChangePath NormalLaneChange::getCandidatePath( return *candidate_path; } -bool NormalLaneChange::checkCandidatePathSafety( +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 { From a57b58542ff1466b0849ad0ea9c675ca1cb0ce49 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 25 Sep 2024 15:19:36 +0900 Subject: [PATCH 25/26] use snake case for function names Signed-off-by: mohammad alqudah --- .../include/autoware/route_handler/route_handler.hpp | 2 +- planning/autoware_route_handler/src/route_handler.cpp | 2 +- .../behavior_path_lane_change_module/utils/utils.hpp | 4 ++-- .../src/scene.cpp | 10 +++++----- .../src/utils/utils.cpp | 4 ++-- 5 files changed, 11 insertions(+), 11 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 8b8fca4818b52..6202be28e7e87 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 @@ -326,7 +326,7 @@ class RouteHandler */ lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; - Pose getPoseFrom2DArcLength( + Pose get_pose_from_2d_arc_length( const lanelet::ConstLanelets & lanelet_sequence, const double s) const; private: diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 075710c061687..616b91d1bc5f9 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -2038,7 +2038,7 @@ std::optional RouteHandler::findDrivableLanePath( return {}; } -Pose RouteHandler::getPoseFrom2DArcLength( +Pose RouteHandler::get_pose_from_2d_arc_length( const lanelet::ConstLanelets & lanelet_sequence, const double s) const { double accumulated_distance2d = 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 e7768455490d0..c9990747f57a7 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 @@ -95,12 +95,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_lane_reference_path, const std::vector> & sorted_lane_ids); -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); 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 3d098807fc1d5..e8a63c09580e1 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 @@ -1575,15 +1575,15 @@ LaneChangePath NormalLaneChange::get_candidate_path( 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.getPoseFrom2DArcLength(target_lanes, dist_to_lc_end); + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); }); - const auto shift_line = utils::lane_change::getLaneChangingShiftLine( + const auto shift_line = utils::lane_change::get_lane_changing_shift_line( lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - const auto candidate_path = utils::lane_change::constructCandidatePath( + 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 (!candidate_path) { @@ -1739,7 +1739,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); @@ -1752,7 +1752,7 @@ 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( + 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); 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 920ff4c4d7edb..36aedf5c99a53 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 @@ -230,7 +230,7 @@ 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_lane_reference_path, const std::vector> & sorted_lane_ids) @@ -355,7 +355,7 @@ PathWithLaneId getReferencePathFromTargetLane( lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_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) { From e89ef2332f8bdb46a4e32b34ea96f3b2ab25196d Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Wed, 25 Sep 2024 15:33:39 +0900 Subject: [PATCH 26/26] add colors to flow chart in README Signed-off-by: mohammad alqudah --- .../README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 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 d52faec37bee7..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 @@ -34,7 +34,7 @@ skinparam backgroundColor #WHITE start if (Is current lanes, target lanes OR target neighbor lanes polygon empty?) then (yes) - :Return prev approved path; + #LightPink:Return prev approved path; stop else (no) endif @@ -50,10 +50,10 @@ repeat if (Is LC start point outside target lanes range) then (yes) if (Is found a valid path) then (yes) - :Return first valid path; + #Orange:Return first valid path; stop else (no) - :Return prev approved path; + #LightPink:Return prev approved path; stop endif else (no) @@ -68,7 +68,7 @@ repeat if (Is valid candidate path?) then (yes) :Store candidate path; if (Is candidate path safe?) then (yes) - :Return candidate path; + #LightGreen:Return candidate path; stop else (no) endif @@ -78,12 +78,12 @@ repeat repeat while (Is finished looping prepare phase metrics) is (FALSE) if (Is found a valid path) then (yes) - :Return first valid path; + #Orange:Return first valid path; stop else (no) endif -:Return prev approved path; +#LightPink:Return prev approved path; stop @enduml @@ -103,7 +103,7 @@ start :Get reference path from target lanes; if (Is reference path empty?) then (yes) - :Return; + #LightPink:Return; stop else (no) endif @@ -116,10 +116,10 @@ note left: set information from sampled prepare phase and shift phase metrics\n< :Construct candidate path; if (Candidate path has enough length?) then (yes) - :Return valid candidate path; + #LightGreen:Return valid candidate path; stop else (no) - :Return; + #LightPink:Return; stop endif