From d9903a080b52422e1898cd3d655b4a9ea8f0398e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 16 Aug 2024 14:58:48 +0900 Subject: [PATCH 01/12] refactor(lane_change): update filtered objects only once (#8489) Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_lane_change_module/scene.hpp | 2 ++ .../utils/base_class.hpp | 3 +++ .../src/interface.cpp | 1 + .../src/scene.cpp | 15 +++++++++------ 4 files changed, 15 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 3a11b0ed903dc..ac854f6e957ab 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 @@ -53,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; + void update_filtered_objects() final; + void updateLaneChangeStatus() override; std::pair getSafePath(LaneChangePath & safe_path) const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 65f57fa583817..3203745367c36 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -67,6 +67,8 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; + virtual void update_filtered_objects() = 0; + virtual void updateLaneChangeStatus() = 0; virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; @@ -258,6 +260,7 @@ class LaneChangeBase std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; lane_change::CommonDataPtr common_data_ptr_{}; + FilteredByLanesExtendedObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 3f42eeb23f8f6..fc81e59f2872d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -76,6 +76,7 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); + module_type_->update_filtered_objects(); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 0beec0909c034..c299f12b09101 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 @@ -99,6 +99,11 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } +void NormalLaneChange::update_filtered_objects() +{ + filtered_objects_ = filterObjects(); +} + void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1430,8 +1435,7 @@ bool NormalLaneChange::getLaneChangePaths( return false; } - const auto filtered_objects = filterObjects(); - const auto target_objects = getTargetObjects(filtered_objects, current_lanes); + const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1679,7 +1683,7 @@ bool NormalLaneChange::getLaneChangePaths( 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 " @@ -1860,8 +1864,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {true, true}; } - const auto filtered_objects = filterObjects(); - const auto target_objects = getTargetObjects(filtered_objects, current_lanes); + const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); CollisionCheckDebugMap debug_data; @@ -1870,7 +1873,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects.target_lane_leading, min_lc_length, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, min_lc_length, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); From 05bba63707503955720d695efffc97cf5df4c71e Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Mon, 26 Aug 2024 10:37:36 +0900 Subject: [PATCH 02/12] fix(lane_change): activate turn signal as soon as we have the intention to change lanes (#8571) * modify lane change requested condition Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: mohammad alqudah * modify lane change requested condition Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> * style(pre-commit): autofix * fix docstring Signed-off-by: Muhammad Zulfaqar Azmi * modify LC turn signal logic Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * minor change Signed-off-by: mohammad alqudah --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: mohammad alqudah Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scene.hpp | 4 +- .../utils/base_class.hpp | 29 +++++- .../src/scene.cpp | 98 ++++++++----------- 3 files changed, 71 insertions(+), 60 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 ac854f6e957ab..e5bd2abb8a07c 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 @@ -108,7 +108,7 @@ class NormalLaneChange : public LaneChangeBase bool isStoppedAtRedTrafficLight() const override; - TurnSignalInfo get_current_turn_signal_info() override; + TurnSignalInfo get_current_turn_signal_info() const final; protected: lanelet::ConstLanelets getLaneChangeLanes( @@ -116,6 +116,8 @@ class NormalLaneChange : public LaneChangeBase int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + TurnSignalInfo get_terminal_turn_signal_info() const final; + std::vector sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; 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 3203745367c36..3c3ba8607e021 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 @@ -213,7 +213,7 @@ class LaneChangeBase LaneChangeModuleType getModuleType() const { return type_; } - TurnSignalDecider getTurnSignalDecider() { return planner_data_->turn_signal_decider; } + TurnSignalDecider getTurnSignalDecider() const { return planner_data_->turn_signal_decider; } Direction getDirection() const { @@ -229,7 +229,7 @@ class LaneChangeBase void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } - virtual TurnSignalInfo get_current_turn_signal_info() = 0; + virtual TurnSignalInfo get_current_turn_signal_info() const = 0; protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; @@ -251,6 +251,31 @@ class LaneChangeBase virtual lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; + virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0; + + TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const + { + TurnSignalInfo turn_signal; + switch (direction_) { + case Direction::LEFT: + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + break; + case Direction::RIGHT: + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + break; + default: + turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + break; + } + + turn_signal.desired_start_point = start; + turn_signal.desired_end_point = end; + turn_signal.required_start_point = turn_signal.desired_start_point; + turn_signal.required_end_point = turn_signal.desired_end_point; + + return turn_signal; + } + LaneChangeStatus status_{}; PathShifter path_shifter_{}; 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 c299f12b09101..6e920f9cd288e 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 @@ -187,81 +187,64 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const status_.lane_change_path.info.length.sum()); } -TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() +TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const { const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto & current_lanes = get_current_lanes(); - const auto is_valid = getLaneChangeStatus().is_valid_path; - const auto & lane_change_path = getLaneChangeStatus().lane_change_path; - const auto & lane_change_param = getLaneChangeParam(); - - if (getModuleType() != LaneChangeModuleType::NORMAL || current_lanes.empty() || !is_valid) { + if (getModuleType() != LaneChangeModuleType::NORMAL || get_current_lanes().empty()) { return original_turn_signal_info; } - // check direction - TurnSignalInfo current_turn_signal_info; - const auto & current_pose = getEgoPose(); - const auto direction = getDirection(); - if (direction == Direction::LEFT) { - current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else if (direction == Direction::RIGHT) { - current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + if (direction_ != Direction::LEFT && direction_ != Direction::RIGHT) { + return original_turn_signal_info; } const auto & path = prev_module_output_.path; - if (path.points.empty()) { - current_turn_signal_info.desired_start_point = current_pose; - current_turn_signal_info.required_start_point = current_pose; - current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; - current_turn_signal_info.required_end_point = lane_change_path.info.lane_changing_end; - return current_turn_signal_info; + const auto & original_command = original_turn_signal_info.turn_signal.command; + if ( + !path.points.empty() && original_command != TurnIndicatorsCommand::DISABLE && + original_command != TurnIndicatorsCommand::NO_COMMAND) { + return get_terminal_turn_signal_info(); } - const auto min_length_for_turn_signal_activation = - lane_change_param.min_length_for_turn_signal_activation; - const auto & route_handler = getRouteHandler(); - const auto & common_parameter = getCommonParam(); + return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end); +} + +TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const +{ + const auto & lane_change_param = getLaneChangeParam(); + const auto & common_param = getCommonParam(); + const auto & current_pose = getEgoPose(); + const auto & path = prev_module_output_.path; + + const auto original_turn_signal_info = prev_module_output_.turn_signal_info; + const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back()); const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); - const double nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; - const double base_to_front = common_parameter.base_link2front; - const double buffer = - next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; + const double buffer = next_lane_change_buffer + + lane_change_param.min_length_for_turn_signal_activation + + common_param.base_link2front; const double path_length = autoware::motion_utils::calcArcLength(path.points); - const size_t current_nearest_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path.points, 0, std::max(path_length - buffer, 0.0)); - if (dist_to_terminal - base_to_front < buffer && start_pose) { - // modify turn signal - current_turn_signal_info.desired_start_point = *start_pose; - current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; - current_turn_signal_info.required_start_point = current_turn_signal_info.desired_start_point; - current_turn_signal_info.required_end_point = current_turn_signal_info.desired_end_point; - - const auto & original_command = original_turn_signal_info.turn_signal.command; - if ( - original_command == TurnIndicatorsCommand::DISABLE || - original_command == TurnIndicatorsCommand::NO_COMMAND) { - return current_turn_signal_info; - } - // check the priority of turn signals - return getTurnSignalDecider().overwrite_turn_signal( - path, current_pose, current_nearest_seg_idx, original_turn_signal_info, - current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); - } + if (!start_pose) return original_turn_signal_info; + + const auto terminal_turn_signal_info = + get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end); + + const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; + const size_t current_nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - // not in the vicinity of the end of the path. return original - return original_turn_signal_info; + return getTurnSignalDecider().overwrite_turn_signal( + path, current_pose, current_nearest_seg_idx, original_turn_signal_info, + terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); } LaneChangePath NormalLaneChange::getLaneChangePath() const @@ -325,14 +308,15 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path = abort_path_->path; insertStopPoint(get_current_lanes(), output.path); } else { - output.path = getLaneChangePath().path; + output.path = status_.lane_change_path.path; const auto found_extended_path = extendPath(); if (found_extended_path) { output.path = utils::combinePath(output.path, *found_extended_path); } output.reference_path = getReferencePath(); - output.turn_signal_info = updateOutputTurnSignal(); + output.turn_signal_info = + get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); if (isStopState()) { const auto current_velocity = getEgoVelocity(); From 0b61b6717ff4ea77bb61ea064bd8922041bfbba6 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 18 Sep 2024 17:15:28 +0900 Subject: [PATCH 03/12] feat(lane_change): improve execution condition of lane change module (#8648) * refactor lane change utility funcions Signed-off-by: mohammad alqudah * LC utility function to get distance to next regulatory element Signed-off-by: mohammad alqudah * don't activate LC module when close to regulatory element Signed-off-by: mohammad alqudah * modify threshold distance calculation Signed-off-by: mohammad alqudah * move regulatory element check to canTransitFailureState() function Signed-off-by: mohammad alqudah * always run LC module if approaching terminal point Signed-off-by: mohammad alqudah * use max possible LC length as threshold Signed-off-by: mohammad alqudah * update LC readme Signed-off-by: mohammad alqudah * refactor implementation Signed-off-by: mohammad alqudah * update readme Signed-off-by: mohammad alqudah * check distance to reg element for candidate path only if not near terminal start Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../src/scene.cpp | 3 +- .../README.md | 53 +------ .../scene.hpp | 2 + .../utils/base_class.hpp | 2 + .../utils/calculation.hpp | 19 +++ .../utils/utils.hpp | 15 +- .../src/interface.cpp | 8 + .../src/scene.cpp | 137 ++++++++++-------- .../src/utils/calculation.cpp | 90 ++++++++++++ .../src/utils/utils.cpp | 104 ++++--------- 10 files changed, 237 insertions(+), 196 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 2c3e716151df8..86810789c20ae 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 @@ -281,7 +282,7 @@ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const return std::numeric_limits::infinity(); } - return utils::lane_change::calcMinimumLaneChangeLength( + return utils::lane_change::calculation::calc_minimum_lane_change_length( getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index e42e6e2d47738..e1b5eb9250863 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -11,7 +11,9 @@ The Lane Change module is activated when lane change is needed and can be safely - `allow_lane_change` tags is set as `true` - During lane change request condition - The ego-vehicle isn’t on a `preferred_lane`. - - There is neither intersection nor crosswalk on the path of the lane change + - The ego-vehicle isn't approaching a traffic light. (condition parameterized) + - The ego-vehicle isn't approaching a crosswalk. (condition parameterized) + - The ego-vehicle isn't approaching an intersection. (condition parameterized) - lane change ready condition - Path of the lane change does not collide with other dynamic objects (see the figure below) - Lane change candidate path is approved by an operator. @@ -182,11 +184,9 @@ A candidate path is considered valid if it meets the following criteria: 1. The distance from the ego vehicle's current position to the end of the current lanes is sufficient to perform a single lane change. 2. The distance from the ego vehicle's current position to the goal along the current lanes is adequate to complete multiple lane changes. 3. The distance from the ego vehicle's current position to the end of the target lanes is adequate for completing multiple lane changes. -4. Intersection requirements are met (conditions are parameterized). -5. Crosswalk requirements are satisfied (conditions are parameterized). -6. Traffic light regulations are adhered to (conditions are parameterized). -7. The lane change can be completed after passing a parked vehicle. -8. The lane change is deemed safe to execute. +4. The distance from the ego vehicle's current position to the next regulatory element is adequate to perform a single lane change. +5. The lane change can be completed after passing a parked vehicle. +6. The lane change is deemed safe to execute. The following flow chart illustrates the validity check. @@ -231,43 +231,6 @@ group check for distance #LightYellow endif end group - - -group evaluate on Crosswalk #LightCyan -if (regulate_on_crosswalk and not enough length to crosswalk) then (yes) - if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) - #LightPink:Reject path; - stop - else (no) - :Allow lane change in crosswalk; - endif -else (no) -endif -end group - -group evaluate on Intersection #LightGreen -if (regulate_on_intersection and not enough length to intersection) then (yes) - if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) - #LightPink:Reject path; - stop - else (no) - :Allow lane change in intersection; - endif -else (no) -endif -end group - -group evaluate on Traffic Light #Lavender -if (regulate_on_traffic_light and not enough length to complete lane change before stop line) then (yes) - #LightPink:Reject path; - stop -elseif (stopped at red traffic light within distance) then (yes) - #LightPink:Reject path; - stop -else (no) -endif -end group - if (ego is not stuck but parked vehicle exists in target lane) then (yes) #LightPink:Reject path; stop @@ -517,8 +480,8 @@ The function to stop by keeping a margin against forward obstacle in the previou ### Lane change regulations -If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. -To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`. +If you want to regulate lane change on crosswalks, intersections, or traffic lights, the lane change module is disabled near any of them. +To regulate lane change on crosswalks, intersections, or traffic lights, set `regulation.crosswalk`, `regulation.intersection` or `regulation.traffic_light` to `true`. If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswalk/intersection. If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck. If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index e5bd2abb8a07c..266734750e2c3 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 @@ -108,6 +108,8 @@ class NormalLaneChange : public LaneChangeBase bool isStoppedAtRedTrafficLight() const override; + bool is_near_regulatory_element() const final; + TurnSignalInfo get_current_turn_signal_info() const final; protected: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 3c3ba8607e021..24b24729a42ff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -231,6 +231,8 @@ class LaneChangeBase virtual TurnSignalInfo get_current_turn_signal_info() const = 0; + virtual bool is_near_regulatory_element() const = 0; + protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index ae0a094e038a4..f395902dcc476 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; @@ -96,6 +100,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 717e8d322f05a..38b4cfd43341a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -60,17 +60,6 @@ using tier4_planning_msgs::msg::PathWithLaneId; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); -double calcMinimumLaneChangeLength( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); - -double calcMinimumLaneChangeLength( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction); - -double calcMaximumLaneChangeLength( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc); - double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); @@ -305,6 +294,10 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co ExtendedPredictedObjects transform_to_extended_objects( const CommonDataPtr & common_data_ptr, const std::vector & objects, const bool check_prepare_phase); + +double get_distance_to_next_regulatory_element( + const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, + const bool ignore_intersection = false); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index fc81e59f2872d..4ee5156f0ab03 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -147,6 +147,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() *prev_approved_path_ = getPreviousModuleOutput().path; BehaviorModuleOutput out = getPreviousModuleOutput(); + module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); @@ -252,6 +253,13 @@ bool LaneChangeInterface::canTransitFailureState() } if (isWaitingApproval()) { + if (module_type_->is_near_regulatory_element()) { + log_debug_throttled("Ego is close to regulatory element. Cancel lane change"); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); + return true; + } log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); return false; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 6e920f9cd288e..f52deefacbd09 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; @@ -177,7 +176,46 @@ bool NormalLaneChange::isLaneChangeRequired() calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); const auto maximum_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - return ego_dist_to_target_start <= maximum_prepare_length; + if (ego_dist_to_target_start > maximum_prepare_length) { + return false; + } + + if (is_near_regulatory_element()) { + RCLCPP_DEBUG(logger_, "Ego is close to regulatory element, don't run LC module"); + return false; + } + + return true; +} + +bool NormalLaneChange::is_near_regulatory_element() const +{ + const auto & current_lanes = get_current_lanes(); + + if (current_lanes.empty()) return false; + + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back()); + + if (shift_intervals.empty()) return false; + + const auto & lc_params = *common_data_ptr_->lc_param_ptr; + const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + const auto min_lc_length = + calculation::calc_minimum_lane_change_length(lc_params, shift_intervals); + const auto dist_to_terminal_start = + calculation::calc_ego_dist_to_terminal_end(common_data_ptr_) - min_lc_length; + + if (dist_to_terminal_start <= max_prepare_length) return false; + + const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + + if (only_tl) { + RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); + } + + return max_prepare_length > utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -222,7 +260,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 + @@ -377,7 +415,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); @@ -449,7 +487,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 - @@ -666,7 +704,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([&]() { @@ -782,7 +820,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); @@ -892,16 +930,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 { @@ -924,7 +952,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( @@ -1290,8 +1319,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)) { @@ -1397,10 +1427,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())); @@ -1430,6 +1460,11 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", prepare_durations.size(), longitudinal_acc_sampling_values.size()); + const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; + const auto dist_to_next_regulatory_element = + utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); + const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + for (const auto & prepare_duration : prepare_durations) { for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { // get path on original lanes @@ -1554,6 +1589,14 @@ bool NormalLaneChange::getLaneChangePaths( continue; } + if ( + ego_dist_to_terminal_start > max_prepare_length && + lane_changing_length + prepare_length > dist_to_next_regulatory_element) { + debug_print_lat( + "Reject: length of lane changing path is longer than length to regulatory element!!"); + continue; + } + // if multiple lane change is necessary, does the remaining distance is sufficient const auto remaining_dist_in_target = std::invoke([&]() { const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; @@ -1628,41 +1671,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 ( @@ -1713,10 +1721,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())); @@ -1852,7 +1860,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())); @@ -1973,7 +1981,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; @@ -2024,7 +2032,7 @@ bool NormalLaneChange::calcAbortPath() const auto dist_to_terminal_start = std::invoke([&]() { const auto dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end( common_data_ptr_, get_current_lanes(), common_data_ptr_->get_ego_pose()); - const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + const auto min_lc_length = calculation::calc_minimum_lane_change_length( route_handler, get_current_lanes().back(), *common_data_ptr_->lc_param_ptr, common_data_ptr_->direction); return dist_to_terminal_end - min_lc_length; @@ -2248,7 +2256,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; @@ -2285,7 +2293,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 2dc65a8b78045..1d656a5ee698f 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 @@ -94,4 +94,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 8bcd6a97d19f5..09ff29d777a40 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -14,6 +14,7 @@ #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" #include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" @@ -21,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" @@ -84,81 +86,6 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -double calcMinimumLaneChangeLength( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals) -{ - if (shift_intervals.empty()) { - return 0.0; - } - - const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; - const auto [min_lat_acc, max_lat_acc] = - lane_change_parameters.lane_change_lat_acc_map.find(min_vel); - 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) @@ -1314,6 +1241,33 @@ ExtendedPredictedObjects transform_to_extended_objects( return extended_objects; } + +double get_distance_to_next_regulatory_element( + const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk, + const bool ignore_intersection) +{ + double distance = std::numeric_limits::max(); + + const auto current_pose = common_data_ptr->get_ego_pose(); + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + + if (!ignore_intersection && common_data_ptr->lc_param_ptr->regulate_on_intersection) { + distance = + std::min(distance, utils::getDistanceToNextIntersection(current_pose, current_lanes)); + } + if (!ignore_crosswalk && common_data_ptr->lc_param_ptr->regulate_on_crosswalk) { + distance = std::min( + distance, utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr)); + } + if (common_data_ptr->lc_param_ptr->regulate_on_traffic_light) { + distance = std::min( + distance, utils::traffic_light::getDistanceToNextTrafficLight(current_pose, current_lanes)); + } + + return distance; +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug From eb093ab7136cd777e024ddd0be7790ed3ef539ea Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 11 Nov 2024 20:58:08 +0900 Subject: [PATCH 04/12] fix(behavior_path_planner_common): use boost intersects instead of overlaps (#9289) * fix(behavior_path_planner_common): use boost intersects instead of overlaps Signed-off-by: kosuke55 * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp Co-authored-by: Go Sakayori --------- Signed-off-by: kosuke55 Co-authored-by: Go Sakayori --- .../src/utils/path_safety_checker/safety_check.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 8d2dd2fb86e29..ad2c295defa92 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -547,7 +547,7 @@ bool checkSafetyWithIntegralPredictedPolygon( CollisionCheckDebugPair debug_pair = createObjectDebug(object); for (const auto & path : object.predicted_paths) { for (const auto & pose_with_poly : path.path) { - if (boost::geometry::overlaps(ego_integral_polygon, pose_with_poly.poly)) { + if (boost::geometry::intersects(ego_integral_polygon, pose_with_poly.poly)) { debug_pair.second.ego_predicted_path = ego_predicted_path; // raw path debug_pair.second.obj_predicted_path = path.path; // raw path debug_pair.second.extended_obj_polygon = pose_with_poly.poly; @@ -621,8 +621,8 @@ std::vector getCollidedPolygons( const double yaw_difference = autoware::universe_utils::normalizeRadian(ego_yaw - object_yaw); if (std::abs(yaw_difference) > yaw_difference_th) continue; - // check overlap - if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + // check intersects + if (boost::geometry::intersects(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; collided_polygons.push_back(obj_polygon); @@ -675,8 +675,8 @@ std::vector getCollidedPolygons( : createExtendedPolygon( obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug); - // check overlap with extended polygon - if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { + // check intersects with extended polygon + if (boost::geometry::intersects(extended_ego_polygon, extended_obj_polygon)) { debug.unsafe_reason = "overlap_extended_polygon"; collided_polygons.push_back(obj_polygon); From ba408830967300c493dd7f8ad824e34205b5852e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 15 Nov 2024 18:29:12 +0900 Subject: [PATCH 05/12] fix(bpp): update collided polygon pose only once (#9338) * fix(bpp): update collided polygon pose only once Signed-off-by: Zulfaqar Azmi * add expected pose Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../path_safety_checker/safety_check.cpp | 29 +++++++++++-------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index ad2c295defa92..c00803ff3f2d8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -623,13 +623,15 @@ std::vector getCollidedPolygons( // check intersects if (boost::geometry::intersects(ego_polygon, obj_polygon)) { - debug.unsafe_reason = "overlap_polygon"; + if (collided_polygons.empty()) { + debug.unsafe_reason = "overlap_polygon"; + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + debug.extended_ego_polygon = ego_polygon; + debug.extended_obj_polygon = obj_polygon; + } collided_polygons.push_back(obj_polygon); - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = obj_pose; - debug.extended_ego_polygon = ego_polygon; - debug.extended_obj_polygon = obj_polygon; continue; } @@ -677,14 +679,17 @@ std::vector getCollidedPolygons( // check intersects with extended polygon if (boost::geometry::intersects(extended_ego_polygon, extended_obj_polygon)) { - debug.unsafe_reason = "overlap_extended_polygon"; + if (collided_polygons.empty()) { + debug.unsafe_reason = "overlap_extended_polygon"; + debug.rss_longitudinal = rss_dist; + debug.inter_vehicle_distance = min_lon_length; + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + debug.extended_ego_polygon = extended_ego_polygon; + debug.extended_obj_polygon = extended_obj_polygon; + debug.is_front = is_object_front; + } collided_polygons.push_back(obj_polygon); - - debug.rss_longitudinal = rss_dist; - debug.inter_vehicle_distance = min_lon_length; - debug.extended_ego_polygon = extended_ego_polygon; - debug.extended_obj_polygon = extended_obj_polygon; - debug.is_front = is_object_front; } } From a15ff9e87bac4fe04e308f7c3566c7fd91aa8af4 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 14 Aug 2024 22:15:03 +0900 Subject: [PATCH 06/12] fix(lane_change): unify stuck detection to avoid unnecessary computation (#8383) unify stuck detection in getLaneChangePaths Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene.hpp | 4 +-- .../utils/base_class.hpp | 6 ---- .../src/scene.cpp | 33 +++++++++---------- 3 files changed, 17 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 266734750e2c3..0661d072cc06b 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 @@ -163,9 +163,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, LaneChangePaths * candidate_paths, - const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, - const bool check_safety = true) const override; + Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 24b24729a42ff..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 @@ -240,12 +240,6 @@ class LaneChangeBase const lanelet::ConstLanelets & current_lanes, const double backward_path_length, const double prepare_length) const = 0; - virtual bool getLaneChangePaths( - const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, - const bool is_stuck, const bool check_safety) const = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 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 f52deefacbd09..c686763036c17 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -129,15 +129,9 @@ 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_, &valid_paths, lane_change_parameters_->rss_params, - is_stuck); + bool found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction_, is_stuck, &valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin - if (!found_safe_path && is_stuck) { - found_safe_path = getLaneChangePaths( - current_lanes, target_lanes, direction_, &valid_paths, - lane_change_parameters_->rss_params_for_stuck, is_stuck); - } lane_change_debug_.valid_paths = valid_paths; @@ -1400,9 +1394,7 @@ bool NormalLaneChange::hasEnoughLengthToTrafficLight( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, LaneChangePaths * candidate_paths, - const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, - const bool check_safety) const + Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); if (current_lanes.empty() || target_lanes.empty()) { @@ -1683,13 +1675,20 @@ bool NormalLaneChange::getLaneChangePaths( return false; } - if (!check_safety) { - debug_print_lat("ACCEPT!!!: it is valid (and safety check is skipped)."); - return false; - } + const auto is_safe = std::invoke([&]() { + const auto safety_check_with_normal_rss = isLaneChangePathSafe( + *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, + lane_change_debug_.collision_check_objects); - const auto [is_safe, is_trailing_object] = isLaneChangePathSafe( - *candidate_path, target_objects, rss_params, 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, + 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!"); From b522ef231ad4bd9934c558bce151e8b617e1476f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 21 Aug 2024 19:50:38 +0900 Subject: [PATCH 07/12] refactor(lane_change): rename prepare_segment_ignore_object_velocity_thresh (#8532) change parameter name for more expressive name Signed-off-by: Zulfaqar Azmi --- .../config/lane_change.param.yaml | 2 +- .../utils/data_structs.hpp | 2 +- .../src/manager.cpp | 4 ++-- .../src/scene.cpp | 16 ++++++++-------- .../src/utils/utils.cpp | 3 +-- 5 files changed, 13 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index a7128a0dc546f..bba3ef52338ea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -106,7 +106,7 @@ general_lanes: false intersection: true turns: true - prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] + stopped_object_velocity_threshold: 1.0 # [m/s] check_objects_on_current_lanes: true check_objects_on_other_lanes: true use_all_predicted_path: true 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 9ddeca87c12a8..2de7027100c13 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 @@ -131,7 +131,7 @@ struct Parameters bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; bool enable_collision_check_for_prepare_phase_in_intersection{true}; bool enable_collision_check_for_prepare_phase_in_turns{true}; - double prepare_segment_ignore_object_velocity_thresh{0.1}; + double stopped_object_velocity_threshold{0.1}; bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index c197d0d63aa37..ec0fa2394b8cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -74,8 +74,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) *node, parameter("enable_collision_check_for_prepare_phase.intersection")); p.enable_collision_check_for_prepare_phase_in_turns = getOrDeclareParameter(*node, parameter("enable_collision_check_for_prepare_phase.turns")); - p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter( - *node, parameter("prepare_segment_ignore_object_velocity_thresh")); + p.stopped_object_velocity_threshold = + getOrDeclareParameter(*node, parameter("stopped_object_velocity_threshold")); p.check_objects_on_current_lanes = getOrDeclareParameter(*node, parameter("check_objects_on_current_lanes")); p.check_objects_on_other_lanes = 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 c686763036c17..e62aa1be3d0fa 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 @@ -446,7 +446,7 @@ void NormalLaneChange::insertStopPoint( for (const auto & object : target_objects.current_lane) { // check if stationary const auto obj_v = std::abs(object.initial_twist.twist.linear.x); - if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { continue; } @@ -500,7 +500,7 @@ void NormalLaneChange::insertStopPoint( target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), [&](const auto & o) { const auto v = std::abs(o.initial_twist.twist.linear.x); - if (v > lane_change_parameters_->stop_velocity_threshold) { + if (v > lane_change_parameters_->stopped_object_velocity_threshold) { return false; } @@ -1229,7 +1229,8 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( is_before_terminal()) { const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); - constexpr double stopped_obj_vel_th = 1.0; + const auto stopped_obj_vel_th = + common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; if (object.kinematics.initial_twist_with_covariance.twist.linear.x < stopped_obj_vel_th) { if (ahead_of_ego) { target_lane_leading_objects.push_back(object); @@ -2161,11 +2162,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); auto is_safe = true; - const auto selected_rss_param = - (obj.initial_twist.twist.linear.x <= - lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh) - ? lane_change_parameters_->rss_params_for_parked - : rss_params; + const auto selected_rss_param = (obj.initial_twist.twist.linear.x <= + lane_change_parameters_->stopped_object_velocity_threshold) + ? lane_change_parameters_->rss_params_for_parked + : rss_params; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0, 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 09ff29d777a40..75950e9161e3d 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 @@ -977,8 +977,7 @@ ExtendedPredictedObject transform( const auto & time_resolution = lane_change_parameters.prediction_time_resolution; const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const auto & velocity_threshold = - lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; + const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; const double obj_vel_norm = std::hypot( extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); From d93a517acb29b057e18cb3f6932bca6dea0b4e88 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 21 Aug 2024 21:16:53 +0900 Subject: [PATCH 08/12] feat(lane_change): consider deceleration in safety check for cancel (#7943) * feat(lane_change): consider deceleration in safety check for cancel Signed-off-by: Fumiya Watanabe * docs(lane_change): fix document Signed-off-by: Fumiya Watanabe * fix conflicts and refactor Signed-off-by: Muhammad Zulfaqar Azmi * fix conflict Signed-off-by: Muhammad Zulfaqar Azmi * style(pre-commit): autofix --------- Signed-off-by: Fumiya Watanabe Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 5 +- .../config/lane_change.param.yaml | 1 + .../scene.hpp | 12 ++ .../utils/data_structs.hpp | 2 + .../utils/utils.hpp | 2 +- .../src/manager.cpp | 2 + .../src/scene.cpp | 188 ++++++++++++------ .../src/utils/utils.cpp | 9 +- 8 files changed, 150 insertions(+), 71 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 e1b5eb9250863..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 @@ -536,6 +536,8 @@ detach @enduml ``` +During a lane change, a safety check is made in consideration of the deceleration of the ego vehicle, and a safety check is made for `cancel.deceleration_sampling_num` deceleration patterns, and the lane change will be canceled if the abort condition is satisfied for all deceleration patterns. + To preventive measure for lane change path oscillations caused by alternating safe and unsafe conditions, an additional hysteresis count check is implemented before executing an abort or cancel maneuver. If unsafe, the `unsafe_hysteresis_count_` is incremented and compared against `unsafe_hysteresis_threshold`; exceeding it prompts an abort condition check, ensuring decisions are made with consideration to recent safety assessments as shown in flow chart above. This mechanism stabilizes decision-making, preventing abrupt changes due to transient unsafe conditions. ```plantuml @@ -786,7 +788,8 @@ The following parameters are configurable in `lane_change.param.yaml`. | `cancel.duration` | [s] | double | The time taken to complete returning to the center line. | 3.0 | | `cancel.max_lateral_jerk` | [m/sss] | double | The maximum lateral jerk for abort path | 1000.0 | | `cancel.overhang_tolerance` | [m] | double | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0 | -| `unsafe_hysteresis_threshold` | [-] | int | threshold that helps prevent frequent switching between safe and unsafe decisions | 10 | +| `cancel.unsafe_hysteresis_threshold` | [-] | int | threshold that helps prevent frequent switching between safe and unsafe decisions | 10 | +| `cancel.deceleration_sampling_num` | [-] | int | Number of deceleration patterns to check safety to cancel lane change | 5 | ### Debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index bba3ef52338ea..9fe9dfd62eece 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -120,6 +120,7 @@ max_lateral_jerk: 1000.0 # [m/s3] overhang_tolerance: 0.0 # [m] unsafe_hysteresis_threshold: 10 # [/] + deceleration_sampling_num: 5 # [/] lane_change_finish_judge_buffer: 2.0 # [m] finish_judge_lateral_threshold: 0.1 # [m] 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 0661d072cc06b..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 @@ -37,6 +37,7 @@ using geometry_msgs::msg::Twist; using lane_change::LanesPolygon; using tier4_planning_msgs::msg::PathWithLaneId; using utils::path_safety_checker::ExtendedPredictedObjects; +using utils::path_safety_checker::RSSparams; class NormalLaneChange : public LaneChangeBase { @@ -175,8 +176,18 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & lane_change_path, const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, + const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const; + + bool has_collision_with_decel_patterns( + const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, + const size_t deceleration_sampling_num, const RSSparams & rss_param, CollisionCheckDebugMap & debug_data) const; + bool is_collided( + const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj, + 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. @@ -224,6 +235,7 @@ class NormalLaneChange : public LaneChangeBase } double stop_time_{0.0}; + static constexpr double floating_err_th{1e-3}; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_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 2de7027100c13..194412dfe01b7 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 @@ -94,6 +94,8 @@ struct CancelParameters // number of unsafe exceeds unsafe_hysteresis_threshold, the lane change will be cancelled or // aborted. int unsafe_hysteresis_threshold{2}; + + int deceleration_sampling_num{5}; }; struct Parameters 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 38b4cfd43341a..39b44dba063cd 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 @@ -138,7 +138,7 @@ std::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameters, + const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters, const double resolution); bool isParkedObject( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index ec0fa2394b8cc..69df2fe14317a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -238,6 +238,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); p.cancel.unsafe_hysteresis_threshold = getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); + p.cancel.deceleration_sampling_num = + getOrDeclareParameter(*node, parameter("cancel.deceleration_sampling_num")); // finish judge parameters p.lane_change_finish_judge_buffer = 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 e62aa1be3d0fa..4ffb817fdef1e 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 @@ -1677,14 +1677,15 @@ 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, - lane_change_debug_.collision_check_objects); + 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, - lane_change_debug_.collision_check_objects); + decel_sampling_num, lane_change_debug_.collision_check_objects); return safety_check_with_stuck_rss.is_safe; } @@ -1873,7 +1874,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const } const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, + static_cast(lane_change_parameters_->cancel.deceleration_sampling_num), debug_data); { // only for debug purpose lane_change_debug_.collision_check_objects.clear(); @@ -2123,7 +2125,7 @@ bool NormalLaneChange::calcAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const lane_change::TargetObjects & collision_check_objects, - const utils::path_safety_checker::RSSparams & rss_params, + const utils::path_safety_checker::RSSparams & rss_params, const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2135,82 +2137,140 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return {is_safe, !is_object_behind_ego}; } + const auto all_decel_pattern_has_collision = + [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool { + return has_collision_with_decel_patterns( + lane_change_path, objects, deceleration_sampling_num, rss_params, debug_data); + }; + + if (all_decel_pattern_has_collision(collision_check_objects.trailing)) { + return {!is_safe, is_object_behind_ego}; + } + + if (all_decel_pattern_has_collision(collision_check_objects.leading)) { + return {!is_safe, !is_object_behind_ego}; + } + + return {is_safe, !is_object_behind_ego}; +} + +bool NormalLaneChange::has_collision_with_decel_patterns( + const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, + const size_t deceleration_sampling_num, const RSSparams & rss_param, + CollisionCheckDebugMap & debug_data) const +{ + if (objects.empty()) { + return false; + } + const auto & path = lane_change_path.path; - const auto & common_parameters = planner_data_->parameters; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); if (path.points.empty()) { - return {!is_safe, !is_object_behind_ego}; + return false; } + const auto current_pose = common_data_ptr_->get_ego_pose(); + const auto current_twist = common_data_ptr_->get_ego_twist(); + const auto bpp_param = *common_data_ptr_->bpp_param_ptr; + const auto global_min_acc = bpp_param.min_acc; + const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; + + const auto min_acc = std::min(lane_changing_acc, global_min_acc); + const auto sampling_num = + std::abs(min_acc - lane_changing_acc) > floating_err_th ? deceleration_sampling_num : 1; + const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); + + std::vector acceleration_values(sampling_num); + std::iota(acceleration_values.begin(), acceleration_values.end(), 0); + + std::transform( + acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), + [&](double n) { return lane_changing_acc + n * acc_resolution; }); + const auto time_resolution = lane_change_parameters_->prediction_time_resolution; - const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( - lane_change_path, current_twist, current_pose, common_parameters, *lane_change_parameters_, - time_resolution); - const auto debug_predicted_path = - utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); - - const auto & current_lanes_polygon = common_data_ptr_->lanes_polygon_ptr->current; - const auto & expanded_target_polygon = common_data_ptr_->lanes_polygon_ptr->expanded_target; - - constexpr double collision_check_yaw_diff_threshold{M_PI}; - - const auto check_collision = [&](const ExtendedPredictedObject & obj) { - auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); - const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - obj, lane_change_parameters_->use_all_predicted_path); - auto is_safe = true; - const auto selected_rss_param = (obj.initial_twist.twist.linear.x <= - lane_change_parameters_->stopped_object_velocity_threshold) - ? lane_change_parameters_->rss_params_for_parked - : rss_params; - for (const auto & obj_path : obj_predicted_paths) { - const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( - path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0, - get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold, - current_debug_data.second); - - if (collided_polygons.empty()) { - utils::path_safety_checker::updateCollisionCheckDebugMap( - debug_data, current_debug_data, is_safe); - continue; - } + const auto all_collided = std::all_of( + acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { + const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( + lane_change_path, current_twist, current_pose, acceleration, bpp_param, + *lane_change_parameters_, time_resolution); + const auto debug_predicted_path = + utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); + + return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { + const auto selected_rss_param = (obj.initial_twist.twist.linear.x <= + lane_change_parameters_->stopped_object_velocity_threshold) + ? lane_change_parameters_->rss_params_for_parked + : rss_param; + return is_collided( + lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data); + }); + }); - const auto collision_in_current_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_lanes_polygon); - const auto collision_in_target_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); + return all_collided; +} - if (!collision_in_current_lanes && !collision_in_target_lanes) { - utils::path_safety_checker::updateCollisionCheckDebugMap( - debug_data, current_debug_data, is_safe); - continue; - } +bool NormalLaneChange::is_collided( + const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj, + const std::vector & ego_predicted_path, + const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const +{ + constexpr auto is_collided{true}; + + if (lane_change_path.points.empty()) { + return !is_collided; + } + + if (ego_predicted_path.empty()) { + return !is_collided; + } + + const auto & lanes_polygon_ptr = common_data_ptr_->lanes_polygon_ptr; + const auto & current_polygon = lanes_polygon_ptr->current; + const auto & expanded_target_polygon = lanes_polygon_ptr->target; + + if (!current_polygon.has_value() || !expanded_target_polygon.has_value()) { + return !is_collided; + } + + constexpr auto is_safe{true}; + auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); + constexpr auto collision_check_yaw_diff_threshold{M_PI}; + constexpr auto hysteresis_factor{1.0}; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + obj, lane_change_parameters_->use_all_predicted_path); + const auto safety_check_max_vel = get_max_velocity_for_safety_check(); + const auto & bpp_param = *common_data_ptr_->bpp_param_ptr; + for (const auto & obj_path : obj_predicted_paths) { + const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( + lane_change_path, ego_predicted_path, obj, obj_path, bpp_param, selected_rss_param, + hysteresis_factor, safety_check_max_vel, collision_check_yaw_diff_threshold, + current_debug_data.second); + + if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( - debug_data, current_debug_data, !is_safe); - return !is_safe; + debug_data, current_debug_data, is_safe); + continue; } - utils::path_safety_checker::updateCollisionCheckDebugMap( - debug_data, current_debug_data, is_safe); - return is_safe; - }; - for (const auto & obj : collision_check_objects.trailing) { - if (!check_collision(obj)) { - return {!is_safe, is_object_behind_ego}; - } - } + const auto collision_in_current_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_polygon); + const auto collision_in_target_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); - for (const auto & obj : collision_check_objects.leading) { - if (!check_collision(obj)) { - return {!is_safe, !is_object_behind_ego}; + if (!collision_in_current_lanes && !collision_in_target_lanes) { + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_data, current_debug_data, is_safe); + continue; } - } - return {is_safe, !is_object_behind_ego}; + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_data, current_debug_data, !is_safe); + return is_collided; + } + utils::path_safety_checker::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); + return !is_collided; } // Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes 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 75950e9161e3d..f922f65176247 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 @@ -663,7 +663,7 @@ std::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const BehaviorPathPlannerParameters & common_parameters, + const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters, const double resolution) { if (lane_change_path.path.points.empty()) { @@ -672,7 +672,6 @@ std::vector convertToPredictedPath( const auto & path = lane_change_path.path; const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare; - const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; const auto duration = lane_change_path.info.duration.sum(); const auto prepare_time = lane_change_path.info.duration.prepare; const auto & minimum_lane_changing_velocity = @@ -705,9 +704,9 @@ std::vector convertToPredictedPath( initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; for (double t = prepare_time; t < duration; t += resolution) { const double delta_t = t - prepare_time; - const double velocity = lane_changing_velocity + lane_changing_acc * delta_t; - const double length = - lane_changing_velocity * delta_t + 0.5 * lane_changing_acc * delta_t * delta_t + offset; + const double velocity = lane_changing_velocity + lane_changing_acceleration * delta_t; + const double length = lane_changing_velocity * delta_t + + 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); From d1a3e699e01332bef122af1fbc7370e42a77c112 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 22 Aug 2024 23:27:21 +0900 Subject: [PATCH 09/12] feat(lane_change): fix delay logic that caused timing to be late (#8549) * RT1-5067 fix delay logic that caused timing to be late Signed-off-by: Muhammad Zulfaqar Azmi * remove autoware namespace Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../src/utils/utils.cpp | 47 ++++++++++++------- 1 file changed, 29 insertions(+), 18 deletions(-) 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 f922f65176247..32a5f180c75e1 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 @@ -873,30 +873,41 @@ bool passed_parked_objects( } const auto & current_path_end = current_lane_path.points.back().point.pose.position; - double min_dist_to_end_of_current_lane = std::numeric_limits::max(); - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); - const double dist = autoware::motion_utils::calcSignedArcLength( - current_lane_path.points, obj_p, current_path_end); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - if (is_goal_in_route) { + const auto dist_to_path_end = [&](const auto & src_point) { + if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { const auto goal_pose = route_handler.getGoalPose(); - const double dist_to_goal = autoware::motion_utils::calcSignedArcLength( - current_lane_path.points, obj_p, goal_pose.position); - min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); + return motion_utils::calcSignedArcLength( + current_lane_path.points, src_point, goal_pose.position); } - } + return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); + }; + + const auto min_dist_to_end_of_current_lane = std::invoke([&]() { + auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); + for (const auto & point : leading_obj_poly.outer()) { + const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); + const auto dist = dist_to_path_end(obj_p); + min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); + } + return min_dist_to_end_of_current_lane; + }); // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { - debug.second.unsafe_reason = "delay lane change"; - utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return false; + if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { + return true; } - return true; + const auto current_pose = common_data_ptr->get_ego_pose(); + const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( + current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position); + + if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { + return true; + } + + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return false; } std::optional getLeadingStaticObjectIdx( From 3d19e521da61416b13119472daa1f2b20418d031 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 30 Sep 2024 15:47:33 +0900 Subject: [PATCH 10/12] refactor(bpp): simplify ExtendedPredictedObject and add new member variables (#8889) * simplify ExtendedPredictedObject and add new member variables Signed-off-by: Zulfaqar Azmi * replace self polygon to initial polygon Signed-off-by: Zulfaqar Azmi * comment Signed-off-by: Zulfaqar Azmi * add comments to dist of ego Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 22 +++++------ .../src/utils/utils.cpp | 19 +++++----- .../path_safety_checker_parameters.hpp | 37 ++++++++++++------- .../src/marker_utils/utils.cpp | 2 +- .../path_safety_checker/objects_filtering.cpp | 2 +- .../path_safety_checker/safety_check.cpp | 9 ++--- .../src/start_planner_module.cpp | 2 +- .../src/scene.cpp | 10 ++--- 8 files changed, 56 insertions(+), 47 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 4ffb817fdef1e..e2f3729fd80c4 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 @@ -445,14 +445,14 @@ void NormalLaneChange::insertStopPoint( for (const auto & object : target_objects.current_lane) { // check if stationary - const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + const auto obj_v = std::abs(object.initial_twist.linear.x); if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { continue; } // calculate distance from path front to the stationary object polygon on the ego lane. const auto polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer(); for (const auto & polygon_p : polygon) { const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); @@ -499,21 +499,21 @@ void NormalLaneChange::insertStopPoint( const bool has_blocking_target_lane_obj = std::any_of( target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), [&](const auto & o) { - const auto v = std::abs(o.initial_twist.twist.linear.x); + const auto v = std::abs(o.initial_twist.linear.x); if (v > lane_change_parameters_->stopped_object_velocity_threshold) { return false; } // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( - autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(), lanelet::utils::combineLaneletsShape(get_target_lanes()) .polygon2d() .basicPolygon())) { return false; } - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose); return stopping_distance_for_obj < distance_to_target_lane_obj && distance_to_target_lane_obj < distance_to_ego_lane_obj; }); @@ -2198,10 +2198,10 @@ bool NormalLaneChange::has_collision_with_decel_patterns( utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = (obj.initial_twist.twist.linear.x <= - lane_change_parameters_->stopped_object_velocity_threshold) - ? lane_change_parameters_->rss_params_for_parked - : rss_param; + const auto selected_rss_param = + (obj.initial_twist.linear.x <= lane_change_parameters_->stopped_object_velocity_threshold) + ? lane_change_parameters_->rss_params_for_parked + : rss_param; return is_collided( lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data); }); @@ -2295,10 +2295,10 @@ bool NormalLaneChange::isVehicleStuck( const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point + const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point // Note: it needs chattering prevention. - if (std::abs(object.initial_twist.twist.linear.x) > 0.3) { // check if stationary + if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary 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 32a5f180c75e1..2ac7ca6938535 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 @@ -734,12 +734,12 @@ bool isParkedObject( using lanelet::geometry::toArcCoordinates; const double object_vel_norm = - std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y); + std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { return false; } - const auto & object_pose = object.initial_pose.pose; + const auto & object_pose = object.initial_pose; const auto object_closest_index = autoware::motion_utils::findNearestIndex(path.points, object_pose.position); const auto object_closest_pose = path.points.at(object_closest_index).point.pose; @@ -806,7 +806,7 @@ bool isParkedObject( { using lanelet::geometry::distance2d; - const auto & obj_pose = object.initial_pose.pose; + const auto & obj_pose = object.initial_pose; const auto & obj_shape = object.shape; const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); const auto obj_point = obj_pose.position; @@ -867,7 +867,7 @@ bool passed_parked_objects( const auto & leading_obj = objects.at(*leading_obj_idx); auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); + autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { return true; } @@ -899,7 +899,7 @@ bool passed_parked_objects( const auto current_pose = common_data_ptr->get_ego_pose(); const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position); + current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { return true; @@ -928,12 +928,11 @@ std::optional getLeadingStaticObjectIdx( std::optional leading_obj_idx = std::nullopt; for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose.pose; + const auto & obj_pose = obj.initial_pose; // ignore non-static object // TODO(shimizu): parametrize threshold - const double obj_vel_norm = - std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y); + const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); if (obj_vel_norm > 1.0) { continue; } @@ -989,8 +988,8 @@ ExtendedPredictedObject transform( const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; - const double obj_vel_norm = std::hypot( - extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); + const double obj_vel_norm = + std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 0e7d1cee65f02..795dc6d145190 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #include +#include #include #include @@ -23,7 +24,7 @@ #include -#include +#include #include #include #include @@ -33,7 +34,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -60,8 +63,8 @@ struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped Polygon2d poly; PoseWithVelocityAndPolygonStamped( - const double time, const Pose & pose, const double velocity, const Polygon2d & poly) - : PoseWithVelocityStamped(time, pose, velocity), poly(poly) + const double time, const Pose & pose, const double velocity, Polygon2d poly) + : PoseWithVelocityStamped(time, pose, velocity), poly(std::move(poly)) { } }; @@ -75,22 +78,30 @@ struct PredictedPathWithPolygon struct ExtendedPredictedObject { unique_identifier_msgs::msg::UUID uuid; - geometry_msgs::msg::PoseWithCovariance initial_pose; - geometry_msgs::msg::TwistWithCovariance initial_twist; - geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_perception_msgs::msg::Shape shape; - std::vector classification; + Pose initial_pose; + Twist initial_twist; + Shape shape; + ObjectClassification classification; + Polygon2d initial_polygon; std::vector predicted_paths; + double dist_from_ego{0.0}; ///< Distance from ego to obj, can be arc length or euclidean. ExtendedPredictedObject() = default; explicit ExtendedPredictedObject(const PredictedObject & object) : uuid(object.object_id), - initial_pose(object.kinematics.initial_pose_with_covariance), - initial_twist(object.kinematics.initial_twist_with_covariance), - initial_acceleration(object.kinematics.initial_acceleration_with_covariance), - shape(object.shape), - classification(object.classification) + initial_pose(object.kinematics.initial_pose_with_covariance.pose), + initial_twist(object.kinematics.initial_twist_with_covariance.twist), + shape(object.shape) { + classification.label = std::invoke([&]() { + auto max_elem = std::max_element( + object.classification.begin(), object.classification.end(), + [](const auto & a, const auto & b) { return a.probability < b.probability; }); + + return (max_elem != object.classification.end()) ? max_elem->label + : ObjectClassification::UNKNOWN; + }); + initial_polygon = autoware::universe_utils::toPolygon2d(initial_pose, shape); } }; using ExtendedPredictedObjects = std::vector; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index 8c9b4ef35d5af..044457a149a7d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -608,7 +608,7 @@ MarkerArray showFilteredObjects( cube_marker = default_cube_marker(1.0, 1.0, color); cube_marker.pose = pose; }; - insert_cube_marker(obj.initial_pose.pose); + insert_cube_marker(obj.initial_pose); }); return marker_array; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 1de55dca29347..25b307ab2cc4d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -327,7 +327,7 @@ ExtendedPredictedObject transform( { ExtendedPredictedObject extended_object(object); - const auto obj_velocity = extended_object.initial_twist.twist.linear.x; + const auto obj_velocity = extended_object.initial_twist.linear.x; extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index c00803ff3f2d8..368e0501b149d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -590,7 +590,7 @@ std::vector getCollidedPolygons( { debug.ego_predicted_path = predicted_ego_path; debug.obj_predicted_path = target_object_path.path; - debug.current_obj_pose = target_object.initial_pose.pose; + debug.current_obj_pose = target_object.initial_pose; } std::vector collided_polygons{}; @@ -712,11 +712,10 @@ bool checkPolygonsIntersects( CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_obj_pose = obj.initial_pose.pose; - debug.extended_obj_polygon = - autoware::universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + debug.current_obj_pose = obj.initial_pose; + debug.extended_obj_polygon = autoware::universe_utils::toPolygon2d(obj.initial_pose, obj.shape); debug.obj_shape = obj.shape; - debug.current_twist = obj.initial_twist.twist; + debug.current_twist = obj.initial_twist; return {autoware::universe_utils::toBoostUUID(obj.uuid), debug}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index dd9129216fbba..e30b9c66d99c2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -537,7 +537,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { const auto arc_length = autoware::motion_utils::calcSignedArcLength( - centerline_path.points, ego_pose.position, o.initial_pose.pose.position); + centerline_path.points, ego_pose.position, o.initial_pose.position); if (arc_length > 0.0) return; if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; arc_length_to_closet_object = arc_length; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 8b2e0a6bd1077..69db33cdc755d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -838,20 +838,20 @@ bool StaticObstacleAvoidanceModule::isSafePath( auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); const auto obj_polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape); + autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); - const auto & object_twist = object.initial_twist.twist; + const auto & object_twist = object.initial_twist; const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); - const auto object_type = utils::getHighestProbLabel(object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto object_type = object.classification; + const auto object_parameter = parameters_->object_parameters.at(object_type.label); const auto is_object_moving = v_norm > object_parameter.moving_speed_threshold; const auto is_object_oncoming = is_object_moving && - utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose.pose); + utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); From a676acd1354992425b627b8727823bb8163433c8 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Mon, 18 Nov 2024 09:39:46 +0900 Subject: [PATCH 11/12] refactor(lane_change): refactor extended object safety check (#9322) * refactor LC extended object collision check code Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --------- Signed-off-by: mohammad alqudah Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../scene.hpp | 7 +-- .../utils/utils.hpp | 5 +- .../src/scene.cpp | 46 +++++++++++++------ .../src/utils/utils.cpp | 15 ++---- 4 files changed, 42 insertions(+), 31 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..1bf8407d64ffb 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 @@ -181,12 +181,13 @@ class NormalLaneChange : public LaneChangeBase bool has_collision_with_decel_patterns( const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, const size_t deceleration_sampling_num, const RSSparams & rss_param, - CollisionCheckDebugMap & debug_data) const; + const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const; bool is_collided( - const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj, + const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, - const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const; + const RSSparams & selected_rss_param, const bool check_prepare_phase, + CollisionCheckDebugMap & debug_data) const; //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. //! @param obstacle_check_distance Distance to check ahead for any objects that might be 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 39b44dba063cd..73bbef1453459 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 @@ -167,7 +167,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); + const LaneChangeParameters & lane_change_parameters); bool isCollidedPolygonsInLanelet( const std::vector & collided_polygons, @@ -292,8 +292,7 @@ bool is_before_terminal( double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose); ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects, - const bool check_prepare_phase); + const CommonDataPtr & common_data_ptr, const std::vector & objects); double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, 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 e2f3729fd80c4..39cc4327be07b 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 @@ -1106,17 +1106,16 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return is_within_vel_th(object) && ahead_of_ego; }); - const auto is_check_prepare_phase = check_prepare_phase(); const auto target_lane_leading_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_leading, is_check_prepare_phase); + common_data_ptr_, filtered_by_lanes_objects.target_lane_leading); const auto target_lane_trailing_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing, is_check_prepare_phase); + common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing); const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.current_lane, is_check_prepare_phase); + common_data_ptr_, filtered_by_lanes_objects.current_lane); const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.other_lane, is_check_prepare_phase); + common_data_ptr_, filtered_by_lanes_objects.other_lane); FilteredByLanesExtendedObjects lane_change_target_objects( current_lane_extended_objects, target_lane_leading_extended_objects, @@ -2137,10 +2136,13 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return {is_safe, !is_object_behind_ego}; } + const auto is_check_prepare_phase = check_prepare_phase(); + const auto all_decel_pattern_has_collision = [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool { return has_collision_with_decel_patterns( - lane_change_path, objects, deceleration_sampling_num, rss_params, debug_data); + lane_change_path, objects, deceleration_sampling_num, rss_params, is_check_prepare_phase, + debug_data); }; if (all_decel_pattern_has_collision(collision_check_objects.trailing)) { @@ -2157,7 +2159,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( bool NormalLaneChange::has_collision_with_decel_patterns( const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, const size_t deceleration_sampling_num, const RSSparams & rss_param, - CollisionCheckDebugMap & debug_data) const + const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const { if (objects.empty()) { return false; @@ -2203,7 +2205,8 @@ bool NormalLaneChange::has_collision_with_decel_patterns( ? lane_change_parameters_->rss_params_for_parked : rss_param; return is_collided( - lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data); + lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase, + debug_data); }); }); @@ -2211,13 +2214,14 @@ bool NormalLaneChange::has_collision_with_decel_patterns( } bool NormalLaneChange::is_collided( - const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj, + const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, - const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const + const RSSparams & selected_rss_param, const bool check_prepare_phase, + CollisionCheckDebugMap & debug_data) const { constexpr auto is_collided{true}; - if (lane_change_path.points.empty()) { + if (lane_change_path.path.points.empty()) { return !is_collided; } @@ -2242,11 +2246,25 @@ bool NormalLaneChange::is_collided( const auto safety_check_max_vel = get_max_velocity_for_safety_check(); const auto & bpp_param = *common_data_ptr_->bpp_param_ptr; + const double velocity_threshold = lane_change_parameters_->stopped_object_velocity_threshold; + const double prepare_duration = lane_change_path.info.duration.prepare; + const double start_time = check_prepare_phase ? 0.0 : prepare_duration; + for (const auto & obj_path : obj_predicted_paths) { + utils::path_safety_checker::PredictedPathWithPolygon predicted_obj_path; + predicted_obj_path.confidence = obj_path.confidence; + std::copy_if( + obj_path.path.begin(), obj_path.path.end(), std::back_inserter(predicted_obj_path.path), + [&](const auto & entry) { + return !( + entry.time < start_time || + (entry.time < prepare_duration && entry.velocity < velocity_threshold)); + }); + const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( - lane_change_path, ego_predicted_path, obj, obj_path, bpp_param, selected_rss_param, - hysteresis_factor, safety_check_max_vel, collision_check_yaw_diff_threshold, - current_debug_data.second); + lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param, + selected_rss_param, hysteresis_factor, safety_check_max_vel, + collision_check_yaw_diff_threshold, current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( 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 2ac7ca6938535..950547c383c68 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 @@ -980,14 +980,11 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase) + const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object(object); const auto & time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold; - const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; const double obj_vel_norm = std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); @@ -999,11 +996,8 @@ ExtendedPredictedObject transform( extended_object.predicted_paths.at(i).confidence = path.confidence; // create path - for (double t = start_time; t < end_time + std::numeric_limits::epsilon(); + for (double t = 0.0; t < end_time + std::numeric_limits::epsilon(); t += time_resolution) { - if (t < prepare_duration && obj_vel_norm < velocity_threshold) { - continue; - } const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); @@ -1234,8 +1228,7 @@ 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) + const CommonDataPtr & common_data_ptr, const std::vector & objects) { ExtendedPredictedObjects extended_objects; extended_objects.reserve(objects.size()); @@ -1244,7 +1237,7 @@ ExtendedPredictedObjects transform_to_extended_objects( const auto & lc_param = *common_data_ptr->lc_param_ptr; std::transform( objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) { - return utils::lane_change::transform(object, bpp_param, lc_param, check_prepare_phase); + return utils::lane_change::transform(object, bpp_param, lc_param); }); return extended_objects; From b9d569e36ab77e00422587023ac00dd82a001568 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 29 Nov 2024 15:57:53 +0900 Subject: [PATCH 12/12] fix(lane_change): cap ego's predicted path velocity (RT1-8505) (#9341) * fix(lane_change): cap ego's predicted path velocity (RT1-8505) Signed-off-by: Zulfaqar Azmi * properly cap based on 0.0 instead of min lc vel Signed-off-by: Zulfaqar Azmi * fix build error Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 7 ++- .../src/scene.cpp | 11 +--- .../src/utils/utils.cpp | 51 ++++++++++--------- 3 files changed, 32 insertions(+), 37 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 73bbef1453459..a19508dd11b7f 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 @@ -136,10 +136,9 @@ std::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction); -std::vector convertToPredictedPath( - const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, - const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const double resolution); +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const double lane_changing_acceleration); bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, 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 39cc4327be07b..28e95580a4bc7 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 @@ -2171,8 +2171,6 @@ bool NormalLaneChange::has_collision_with_decel_patterns( return false; } - const auto current_pose = common_data_ptr_->get_ego_pose(); - const auto current_twist = common_data_ptr_->get_ego_twist(); const auto bpp_param = *common_data_ptr_->bpp_param_ptr; const auto global_min_acc = bpp_param.min_acc; const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; @@ -2189,15 +2187,10 @@ bool NormalLaneChange::has_collision_with_decel_patterns( acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), [&](double n) { return lane_changing_acc + n * acc_resolution; }); - const auto time_resolution = lane_change_parameters_->prediction_time_resolution; - const auto all_collided = std::all_of( acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { - const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( - lane_change_path, current_twist, current_pose, acceleration, bpp_param, - *lane_change_parameters_, time_resolution); - const auto debug_predicted_path = - utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); + const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path( + common_data_ptr_, lane_change_path, acceleration); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { const auto selected_rss_param = 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 950547c383c68..a063919a1db5a 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 @@ -661,52 +661,55 @@ std::optional getLaneChangeTargetLane( return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } -std::vector convertToPredictedPath( - const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const double resolution) +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const double lane_changing_acceleration) { if (lane_change_path.path.points.empty()) { return {}; } const auto & path = lane_change_path.path; - const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare; - const auto duration = lane_change_path.info.duration.sum(); - const auto prepare_time = lane_change_path.info.duration.prepare; - const auto & minimum_lane_changing_velocity = - lane_change_parameters.minimum_lane_changing_velocity; - + const auto & vehicle_pose = common_data_ptr->get_ego_pose(); + const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr; const auto nearest_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); + path.points, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold, + bpp_param_ptr->ego_nearest_yaw_threshold); - std::vector predicted_path; const auto vehicle_pose_frenet = convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx); - const double initial_velocity = std::abs(vehicle_twist.linear.x); + + const auto initial_velocity = common_data_ptr->get_ego_speed(); + const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare; + const auto duration = lane_change_path.info.duration.sum(); + const auto prepare_time = lane_change_path.info.duration.prepare; + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto resolution = lc_param_ptr->prediction_time_resolution; + std::vector predicted_path; // prepare segment for (double t = 0.0; t < prepare_time; t += resolution) { - const double velocity = - std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity); - const double length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto velocity = + std::clamp(initial_velocity + prepare_acc * t, 0.0, lane_change_path.info.velocity.prepare); + const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } // lane changing segment - const double lane_changing_velocity = - std::max(initial_velocity + prepare_acc * prepare_time, minimum_lane_changing_velocity); - const double offset = + const auto lane_changing_velocity = std::clamp( + initial_velocity + prepare_acc * prepare_time, 0.0, lane_change_path.info.velocity.prepare); + const auto offset = initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; for (double t = prepare_time; t < duration; t += resolution) { - const double delta_t = t - prepare_time; - const double velocity = lane_changing_velocity + lane_changing_acceleration * delta_t; - const double length = lane_changing_velocity * delta_t + - 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; + const auto delta_t = t - prepare_time; + const auto velocity = std::clamp( + lane_changing_velocity + lane_changing_acceleration * delta_t, 0.0, + lane_change_path.info.velocity.lane_changing); + const auto length = lane_changing_velocity * delta_t + + 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity);