From 0acc3582be477d7ed15d19cd50ca27bd9a80b133 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 1 Oct 2024 11:30:45 +0900 Subject: [PATCH 01/12] RT1-8004 replace hasEnoughLength Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 4 -- .../src/scene.cpp | 43 ++----------------- 2 files changed, 4 insertions(+), 43 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 68ff887e6552..42cd87a18c89 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 @@ -150,10 +150,6 @@ class NormalLaneChange : public LaneChangeBase const double target_lane_length, const double lane_changing_length, const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - bool hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; - std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, 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 2bfde9d400ae..6542db0a4cd7 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 @@ -1360,43 +1360,6 @@ PathWithLaneId NormalLaneChange::getTargetSegment( return target_segment; } -bool NormalLaneChange::hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, [[maybe_unused]] const Direction direction) const -{ - if (target_lanes.empty()) { - return false; - } - - const auto current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); - const auto minimum_lane_change_length_to_preferred_lane = - common_data_ptr_->transient_data.next_dist_buffer.min; - - const double lane_change_length = path.info.length.sum(); - if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } - - const auto goal_pose = route_handler->getGoalPose(); - if ( - route_handler->isInGoalRouteSection(current_lanes.back()) && - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { - return false; - } - - // return if there are no target lanes - if ( - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - return false; - } - - return true; -} - std::vector NormalLaneChange::get_prepare_metrics() const { const auto & current_lanes = common_data_ptr_->lanes_ptr->current; @@ -1578,7 +1541,6 @@ LaneChangePath NormalLaneChange::get_candidate_path( const Pose & lc_start_pose, const double shift_length) const { const auto & route_handler = *getRouteHandler(); - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto resample_interval = @@ -1609,7 +1571,10 @@ LaneChangePath NormalLaneChange::get_candidate_path( throw std::logic_error("failed to generate candidate path!"); } - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + if ( + candidate_path.value().info.length.sum() + + common_data_ptr_->transient_data.next_dist_buffer.min > + common_data_ptr_->transient_data.dist_to_terminal_end) { throw std::logic_error("invalid candidate path length!"); } From e894feffc8d7a97ef0cf534ab95c3175f0f99767 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 1 Oct 2024 12:00:13 +0900 Subject: [PATCH 02/12] RT1-8004 Removed isNearEndOfCurrentLanes Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 8 +-- .../utils/base_class.hpp | 4 -- .../src/scene.cpp | 59 +++++-------------- 3 files changed, 16 insertions(+), 55 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 42cd87a18c89..f27aa223a9bb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -89,10 +89,6 @@ class NormalLaneChange : public LaneChangeBase bool isRequiredStop(const bool is_trailing_object) override; - bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const override; - bool hasFinishedLaneChange() const override; bool isAbleToReturnCurrentLane() const override; @@ -127,9 +123,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - std::vector calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + std::vector calcPrepareDuration() const; lane_change::TargetObjects getTargetObjects( const FilteredByLanesExtendedObjects & predicted_objects, 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 580c5709cb5c..4816a2f6c4ea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -112,10 +112,6 @@ class LaneChangeBase virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( PathSafetyStatus approve_path_safety_status) = 0; - virtual bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const = 0; - virtual bool isStoppedAtRedTrafficLight() const = 0; virtual bool calcAbortPath() = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 6542db0a4cd7..be1e9b42bdc1 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 @@ -731,35 +731,6 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( lane_change_lane.value(), getEgoPose(), backward_length, forward_length); } -bool NormalLaneChange::isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const -{ - if (current_lanes.empty()) { - return false; - } - - const auto & route_handler = getRouteHandler(); - const auto & current_pose = getEgoPose(); - - // TODO(Azu) fully change to transient data - const auto distance_to_lane_change_end = std::invoke([&]() { - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - - if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { - distance_to_end = std::min( - distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); - } - - return std::max(0.0, distance_to_end) - - common_data_ptr_->transient_data.current_dist_buffer.min; - }); - - lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; - return distance_to_lane_change_end < threshold; -} - bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -986,7 +957,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( const auto current_max_dist_buffer = calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - if (current_max_dist_buffer > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { RCLCPP_DEBUG( logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); @@ -1023,22 +994,23 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -std::vector NormalLaneChange::calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +std::vector NormalLaneChange::calcPrepareDuration() const { - const auto base_link2front = planner_data_->parameters.base_link2front; - const auto threshold = - lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front + + lc_param_ptr->min_length_for_turn_signal_activation; + const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; + + // TODO(Azu) this check seems to cause scenario failures. + if (common_data_ptr_->transient_data.dist_to_terminal_start >= threshold) { + return {max_prepare_duration}; + } std::vector prepare_durations; constexpr double step = 0.5; - for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; - duration -= step) { + for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { prepare_durations.push_back(duration); - if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { - break; - } } return prepare_durations; @@ -1370,7 +1342,7 @@ std::vector NormalLaneChange::get_prepare_metrics() cons const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_durations = calcPrepareDuration(); RCLCPP_DEBUG( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", @@ -1852,10 +1824,9 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && - isAbleToStopSafely() && is_trailing_object) { + common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && + is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } From fe059e14aa6d7e61bdbab7a280108172df7c0b99 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 1 Oct 2024 15:29:01 +0900 Subject: [PATCH 03/12] RT1-8004 refactor sample longitudinal acc values Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 3 +- .../src/scene.cpp | 62 ++++++------------- 2 files changed, 19 insertions(+), 46 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 f27aa223a9bb..fc04cf0a5e9a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -120,8 +120,7 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_terminal_turn_signal_info() const final; std::vector sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + const lanelet::ConstLanelets & current_lanes) const; std::vector calcPrepareDuration() 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 be1e9b42bdc1..5be7c90288db 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 @@ -932,66 +932,41 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons } std::vector NormalLaneChange::sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const + const lanelet::ConstLanelets & current_lanes) const { if (prev_module_output_.path.points.empty()) { return {}; } - const auto & route_handler = *getRouteHandler(); - const auto current_pose = getEgoPose(); - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; + const auto & transient_data = common_data_ptr_->transient_data; + const auto max_acc = transient_data.acc.max; + const auto sample_acc_values = [&](std::string && reason_for_sampling) { + const auto min_acc = transient_data.acc.min; + const auto lon_acc_sampling_num = common_data_ptr_->lc_param_ptr->longitudinal_acc_sampling_num; - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); + RCLCPP_DEBUG( + logger_, "%sSample all possible acc: [%f ~ %f]", reason_for_sampling.data(), min_acc, + max_acc); + return utils::lane_change::getAccelerationValues(min_acc, max_acc, lon_acc_sampling_num); + }; // if max acc is not positive, then we do the normal sampling if (max_acc <= 0.0) { - RCLCPP_DEBUG( - logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); - } - - // calculate maximum lane change length - // TODO(Azu) Double check why it's failing with transient data - const auto current_max_dist_buffer = - calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - - if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { - RCLCPP_DEBUG( - logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, - max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); + return sample_acc_values("Maximum acceleration <= 0. "); } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. if (isVehicleStuck(current_lanes)) { - auto clock = rclcpp::Clock(RCL_ROS_TIME); - RCLCPP_INFO_THROTTLE( - logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, - max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); + return sample_acc_values("Ego is stuck. "); } - // if maximum lane change length is less than length to goal or the end of target lanes, only - // sample max acc - if (route_handler.isInGoalRouteSection(target_lanes.back())) { - const auto goal_pose = route_handler.getGoalPose(); - if (current_max_dist_buffer < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { - RCLCPP_DEBUG( - logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); - return {max_acc}; - } - } else if (current_max_dist_buffer < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - RCLCPP_DEBUG( - logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); + const auto max_lc_length = + transient_data.max_prepare_length + transient_data.current_dist_buffer.max; + if (max_lc_length <= transient_data.dist_to_terminal_end) { return {max_acc}; } - RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); - return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); + return sample_acc_values(""); } std::vector NormalLaneChange::calcPrepareDuration() const @@ -1339,8 +1314,7 @@ std::vector NormalLaneChange::get_prepare_metrics() cons const auto current_velocity = getEgoVelocity(); // get sampling acceleration values - const auto longitudinal_acc_sampling_values = - sampleLongitudinalAccValues(current_lanes, target_lanes); + const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes); const auto prepare_durations = calcPrepareDuration(); From bb661b56d9dd427756a9ef915d6b2ab33d249445 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 1 Oct 2024 17:42:04 +0900 Subject: [PATCH 04/12] remove calc maximum lane change length Signed-off-by: Zulfaqar Azmi --- .../utils/calculation.hpp | 8 --- .../src/utils/calculation.cpp | 52 ------------------- 2 files changed, 60 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 021fa16fa6ec..25244e1d3044 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 @@ -103,14 +103,6 @@ double calc_ego_dist_to_lanes_start( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -double calc_maximum_lane_change_length( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc); - -double calc_maximum_lane_change_length( - const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, - const double max_acc); - double calc_lane_changing_acceleration( const double initial_lane_changing_velocity, const double max_path_velocity, const double lane_changing_time, const double prepare_longitudinal_acc); 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 1c4aede51074..c4e2e95e812f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -140,58 +140,6 @@ double calc_ego_dist_to_lanes_start( return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); } -double calc_maximum_lane_change_length( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc) -{ - 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); -} - std::vector calc_all_min_lc_lengths( const LCParamPtr & lc_param_ptr, const std::vector & shift_intervals) { From 4a316842910c7b25af4121c72dc4cf2cb6115ab4 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 1 Oct 2024 18:34:30 +0900 Subject: [PATCH 05/12] Revert "remove calc maximum lane change length" This reverts commit e9cc386e1c21321c59f518d2acbe78a3c668471f. --- .../utils/calculation.hpp | 8 +++ .../src/utils/calculation.cpp | 52 +++++++++++++++++++ 2 files changed, 60 insertions(+) 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 25244e1d3044..021fa16fa6ec 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 @@ -103,6 +103,14 @@ double calc_ego_dist_to_lanes_start( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +double calc_maximum_lane_change_length( + const double current_velocity, const LaneChangeParameters & lane_change_parameters, + const std::vector & shift_intervals, const double max_acc); + +double calc_maximum_lane_change_length( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, + const double max_acc); + double calc_lane_changing_acceleration( const double initial_lane_changing_velocity, const double max_path_velocity, const double lane_changing_time, const double prepare_longitudinal_acc); 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 c4e2e95e812f..1c4aede51074 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -140,6 +140,58 @@ double calc_ego_dist_to_lanes_start( return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); } +double calc_maximum_lane_change_length( + const double current_velocity, const LaneChangeParameters & lane_change_parameters, + const std::vector & shift_intervals, const double max_acc) +{ + 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); +} + std::vector calc_all_min_lc_lengths( const LCParamPtr & lc_param_ptr, const std::vector & shift_intervals) { From 3b7e4aacfe95ebb22602f7c8989c8648ab018d7e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 1 Oct 2024 18:34:40 +0900 Subject: [PATCH 06/12] Revert "RT1-8004 refactor sample longitudinal acc values" This reverts commit 775bcdb8fa1817511741776861f9edb7e22fd744. --- .../scene.hpp | 3 +- .../src/scene.cpp | 62 +++++++++++++------ 2 files changed, 46 insertions(+), 19 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 fc04cf0a5e9a..f27aa223a9bb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -120,7 +120,8 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_terminal_turn_signal_info() const final; std::vector sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes) const; + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; std::vector calcPrepareDuration() 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 5be7c90288db..be1e9b42bdc1 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 @@ -932,41 +932,66 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons } std::vector NormalLaneChange::sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes) const + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { if (prev_module_output_.path.points.empty()) { return {}; } - const auto & transient_data = common_data_ptr_->transient_data; - const auto max_acc = transient_data.acc.max; - const auto sample_acc_values = [&](std::string && reason_for_sampling) { - const auto min_acc = transient_data.acc.min; - const auto lon_acc_sampling_num = common_data_ptr_->lc_param_ptr->longitudinal_acc_sampling_num; + const auto & route_handler = *getRouteHandler(); + const auto current_pose = getEgoPose(); + const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; - RCLCPP_DEBUG( - logger_, "%sSample all possible acc: [%f ~ %f]", reason_for_sampling.data(), min_acc, - max_acc); - return utils::lane_change::getAccelerationValues(min_acc, max_acc, lon_acc_sampling_num); - }; + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); // if max acc is not positive, then we do the normal sampling if (max_acc <= 0.0) { - return sample_acc_values("Maximum acceleration <= 0. "); + RCLCPP_DEBUG( + logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + + // calculate maximum lane change length + // TODO(Azu) Double check why it's failing with transient data + const auto current_max_dist_buffer = + calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); + + if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { + RCLCPP_DEBUG( + logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, + max_acc); + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. if (isVehicleStuck(current_lanes)) { - return sample_acc_values("Ego is stuck. "); + auto clock = rclcpp::Clock(RCL_ROS_TIME); + RCLCPP_INFO_THROTTLE( + logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, + max_acc); + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); } - const auto max_lc_length = - transient_data.max_prepare_length + transient_data.current_dist_buffer.max; - if (max_lc_length <= transient_data.dist_to_terminal_end) { + // if maximum lane change length is less than length to goal or the end of target lanes, only + // sample max acc + if (route_handler.isInGoalRouteSection(target_lanes.back())) { + const auto goal_pose = route_handler.getGoalPose(); + if (current_max_dist_buffer < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); + return {max_acc}; + } + } else if (current_max_dist_buffer < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } - return sample_acc_values(""); + RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); + return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } std::vector NormalLaneChange::calcPrepareDuration() const @@ -1314,7 +1339,8 @@ std::vector NormalLaneChange::get_prepare_metrics() cons const auto current_velocity = getEgoVelocity(); // get sampling acceleration values - const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes); + const auto longitudinal_acc_sampling_values = + sampleLongitudinalAccValues(current_lanes, target_lanes); const auto prepare_durations = calcPrepareDuration(); From 487fcdc6b85e067953b643d5ade6772435499420 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 1 Oct 2024 17:43:54 +0900 Subject: [PATCH 07/12] replace generateCenterLinePath Signed-off-by: Zulfaqar Azmi --- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 3 +-- 1 file changed, 1 insertion(+), 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 be1e9b42bdc1..c1c417a79034 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 @@ -1105,8 +1105,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto path = - route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr_->current_lanes_path; auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); From 53790d92b860c6d9022678c731400a377fbf198f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 1 Oct 2024 18:01:12 +0900 Subject: [PATCH 08/12] RT1-8004 simplify stuck detection Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 8 +- .../utils/data_structs.hpp | 1 + .../src/scene.cpp | 124 +++++++----------- 3 files changed, 47 insertions(+), 86 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 f27aa223a9bb..144983959a56 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 @@ -182,15 +182,9 @@ class NormalLaneChange : public LaneChangeBase const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const; - //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. - //! @param obstacle_check_distance Distance to check ahead for any objects that might be - //! obstructing ego path. It makes sense to use values like the maximum lane change distance. - bool isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; - double get_max_velocity_for_safety_check() const; - bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool isVehicleStuck() const; /** * @brief Checks if the given pose is a valid starting point for a lane change. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/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 4cdd3d1cbad3..e4f70f54a843 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -344,6 +344,7 @@ struct TransientData lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes bool is_ego_near_current_terminal_start{false}; + bool is_ego_stuck{false}; }; using RouteHandlerPtr = std::shared_ptr; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index c1c417a79034..8ae8c5dd0b2b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -141,6 +141,9 @@ void NormalLaneChange::update_transient_data() transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + updateStopTime(); + transient_data.is_ego_stuck = isVehicleStuck(); + RCLCPP_DEBUG( logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); RCLCPP_DEBUG( @@ -168,7 +171,6 @@ void NormalLaneChange::update_filtered_objects() void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -934,6 +936,7 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { + // TODO(Azu): sampler should work even when we're not approaching terminal if (prev_module_output_.path.points.empty()) { return {}; } @@ -966,7 +969,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuck(current_lanes)) { + if (common_data_ptr_->transient_data.is_ego_stuck) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, @@ -1055,12 +1058,11 @@ bool NormalLaneChange::get_prepare_segment( lane_change::TargetObjects NormalLaneChange::getTargetObjects( const FilteredByLanesExtendedObjects & filtered_objects, - const lanelet::ConstLanelets & current_lanes) const + [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; - const auto is_stuck = isVehicleStuck(current_lanes); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; - if (chk_obj_in_curr_lanes || is_stuck) { + if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { leading_objects.insert( leading_objects.end(), filtered_objects.current_lane.begin(), filtered_objects.current_lane.end()); @@ -2126,63 +2128,6 @@ bool NormalLaneChange::is_collided( return !is_collided; } -// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes -bool NormalLaneChange::isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // Ego is still moving, not in stuck - if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { - RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); - return false; - } - - // Ego is just stopped, not sure it is in stuck yet. - if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); - return false; - } - - // Check if any stationary object exist in obstacle_check_distance - const auto base_distance = common_data_ptr_->transient_data.current_lanes_ego_arc.length; - - for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point - - // Note: it needs chattering prevention. - if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary - continue; - } - - const auto ego_to_obj_dist = - lanelet::utils::getArcCoordinates(current_lanes, p).length - base_distance; - if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { - RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); - return true; // Stationary object is in front of ego. - } - } - - // Check if Ego is in terminal of current lanes - const auto & route_handler = getRouteHandler(); - const double distance_to_terminal = - route_handler->isInGoalRouteSection(current_lanes.back()) - ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) - : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = current_min_dist_buffer + stop_point_buffer + 1.0; - if (distance_to_terminal < terminal_judge_buffer) { - return true; - } - - // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current - RCLCPP_DEBUG( - logger_, - "No stationary objects found in obstacle_check_distance and Ego is not in " - "terminal of current lanes"); - return false; -} - double NormalLaneChange::get_max_velocity_for_safety_check() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2195,35 +2140,56 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const return getCommonParam().max_vel; } -bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::isVehicleStuck() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (current_lanes.empty()) { - lane_change_debug_.is_stuck = false; - return false; // can not check + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + + if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); + return false; } - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto current_max_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.max; - const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lc_param_ptr->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + const auto & current_lanes_path = common_data_ptr_->current_lanes_path; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto rss_dist = + calcRssDistance(0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be // determined, even though you are stuck by an obstacle. If it is too long, // the ego will be judged to be stuck by a distant vehicle, even though the ego is only // stopped at a traffic light. Essentially, the calculation should be based on the information of // the stop reason, but this is outside the scope of one module. I keep it as a TODO. - constexpr double DETECTION_DISTANCE_MARGIN = 10.0; - const auto detection_distance = current_max_dist_buffer + rss_dist + - getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_DEBUG( - logger_, "current_max_dist_buffer: %f, max_acc: %f", current_max_dist_buffer, max_acc); + constexpr auto detection_distance_margin = 10.0; + const auto obstacle_check_distance = common_data_ptr_->transient_data.lane_changing_length.max + + rss_dist + common_data_ptr_->bpp_param_ptr->base_link2front + + detection_distance_margin; + const auto has_object_blocking = std::any_of( + filtered_objects_.current_lane.begin(), filtered_objects_.current_lane.end(), + [&](const auto & object) { + // Note: it needs chattering prevention. + if ( + std::abs(object.initial_twist.linear.x) > + lc_param_ptr->stopped_object_velocity_threshold) { // check if stationary + return false; + } - auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + const auto ego_to_obj_dist = + calcSignedArcLength( + current_lanes_path.points, ego_pose.position, object.initial_pose.position) - + obstacle_check_distance; + return ego_to_obj_dist < 0.0; + }); - lane_change_debug_.is_stuck = is_vehicle_stuck; - return is_vehicle_stuck; + lane_change_debug_.is_stuck = has_object_blocking; + return has_object_blocking; } bool NormalLaneChange::is_valid_start_point( From 9e59029df243721de6a56a425f04e31ec8d0976e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Thu, 3 Oct 2024 16:24:48 +0900 Subject: [PATCH 09/12] swap call to update filtered_objects and update transient data Signed-off-by: Zulfaqar Azmi --- .../autoware_behavior_path_lane_change_module/src/interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0b647acd4426..09550a135ba3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -76,8 +76,8 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); - module_type_->update_transient_data(); module_type_->update_filtered_objects(); + module_type_->update_transient_data(); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { From 67114ccbb7e4134e339074ba7f3aeb4e762331b4 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 4 Oct 2024 14:36:43 +0900 Subject: [PATCH 10/12] RT1-8004 fix conflict Signed-off-by: Zulfaqar Azmi --- .../autoware/behavior_path_lane_change_module/scene.hpp | 3 +-- .../src/scene.cpp | 7 +++---- 2 files changed, 4 insertions(+), 6 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 144983959a56..e96c150f4831 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 @@ -157,8 +157,7 @@ class NormalLaneChange : public LaneChangeBase const Pose & lc_start_pose, const double shift_length) const; bool check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const bool is_stuck) const; + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, 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 8ae8c5dd0b2b..42337aca22cb 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 @@ -1404,7 +1404,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto & current_lanes = get_current_lanes(); const auto & target_lanes = get_target_lanes(); - const auto is_stuck = isVehicleStuck(current_lanes); const auto current_velocity = getEgoVelocity(); const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); @@ -1491,7 +1490,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) candidate_paths.push_back(candidate_path); try { - if (check_candidate_path_safety(candidate_path, target_objects, is_stuck)) { + if (check_candidate_path_safety(candidate_path, target_objects)) { debug_print_lat("ACCEPT!!!: it is valid and safe!"); return true; } @@ -1555,9 +1554,9 @@ LaneChangePath NormalLaneChange::get_candidate_path( } bool NormalLaneChange::check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const bool is_stuck) const + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { + const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck; if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, From a6d0e980ba997e6d1fa76f9dfbeb87e4227e0527 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 4 Oct 2024 14:40:50 +0900 Subject: [PATCH 11/12] RT1-8004 Rename isVehicleStuck to is_ego_stuck() Signed-off-by: Zulfaqar Azmi --- .../autoware_behavior_path_lane_change_module/README.md | 2 +- .../autoware/behavior_path_lane_change_module/scene.hpp | 2 +- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 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 18a797976161..02280f7ffa69 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -195,7 +195,7 @@ if (max_lane_change_length > ego's distance to the end of the current lanes.) t stop endif -if (isVehicleStuck(current_lanes)) then (yes) +if ego is stuck in the current lanes then (yes) :Return **sampled acceleration values**; stop else (no) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index e96c150f4831..810b832e36a0 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 @@ -183,7 +183,7 @@ class NormalLaneChange : public LaneChangeBase double get_max_velocity_for_safety_check() const; - bool isVehicleStuck() const; + bool is_ego_stuck() const; /** * @brief Checks if the given pose is a valid starting point for a lane change. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 42337aca22cb..feac64907fd2 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 @@ -142,7 +142,7 @@ void NormalLaneChange::update_transient_data() transient_data.dist_to_terminal_start < transient_data.max_prepare_length; updateStopTime(); - transient_data.is_ego_stuck = isVehicleStuck(); + transient_data.is_ego_stuck = is_ego_stuck(); RCLCPP_DEBUG( logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); @@ -2139,7 +2139,7 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const return getCommonParam().max_vel; } -bool NormalLaneChange::isVehicleStuck() const +bool NormalLaneChange::is_ego_stuck() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; From 24548b8f1b544bcea6a781a1fc480a0aced0ab27 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 4 Oct 2024 14:47:21 +0900 Subject: [PATCH 12/12] RT1-8004 change calcPrepareDuration to snake case Signed-off-by: Zulfaqar Azmi --- .../autoware/behavior_path_lane_change_module/scene.hpp | 2 +- .../autoware_behavior_path_lane_change_module/src/scene.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 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 810b832e36a0..311063a8bbf4 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 @@ -123,7 +123,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - std::vector calcPrepareDuration() const; + std::vector calc_prepare_durations() const; lane_change::TargetObjects getTargetObjects( const FilteredByLanesExtendedObjects & predicted_objects, 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 feac64907fd2..4360cc3f8180 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 @@ -997,7 +997,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -std::vector NormalLaneChange::calcPrepareDuration() const +std::vector NormalLaneChange::calc_prepare_durations() const { const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front + @@ -1343,7 +1343,7 @@ std::vector NormalLaneChange::get_prepare_metrics() cons const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto prepare_durations = calcPrepareDuration(); + const auto prepare_durations = calc_prepare_durations(); RCLCPP_DEBUG( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu",