From 0a7b74dc1cf7096c6649eecedc98a7c1006d27eb Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 28 Dec 2022 17:15:36 +0900 Subject: [PATCH] feat(behavior_path_planner): abort lane change function (#2359) * feat(behavior_path_planner): abort lane change function Signed-off-by: Muhammad Zulfaqar Azmi * change Revert -> Cancel Signed-off-by: Muhammad Zulfaqar Azmi * Remove some unwanted functions and and STOP state Signed-off-by: Muhammad Zulfaqar Azmi * update steering factor (accidentally removed) Signed-off-by: Muhammad Zulfaqar Azmi * include is_abort_condition_satisfied_ flag Signed-off-by: Muhammad Zulfaqar Azmi * use only check ego in current lane Signed-off-by: Muhammad Zulfaqar Azmi * Revert "use only check ego in current lane" This reverts commit 4f97408ed41f22d97012ee0bc1918570b8fbdea7. * ci(pre-commit): autofix * use only check ego in current lane Signed-off-by: Muhammad Zulfaqar Azmi * improve isAbortConditionSatisfied by using ego polygon check Signed-off-by: Muhammad Zulfaqar Azmi * add lateral jerk and path doesn't keep on updating anymore Signed-off-by: Muhammad Zulfaqar Azmi * parameterized all abort related values Signed-off-by: Muhammad Zulfaqar Azmi * rename abort_end -> abort_return Signed-off-by: Muhammad Zulfaqar Azmi * fix some parameter issue Signed-off-by: Muhammad Zulfaqar Azmi * check if lane change distance is enough after abort Signed-off-by: Muhammad Zulfaqar Azmi * improve the code flow of isAbortConditionSatisfied Signed-off-by: Muhammad Zulfaqar Azmi * Place warning message in corresponding states. Signed-off-by: Muhammad Zulfaqar Azmi * fix clock and rebase Signed-off-by: Muhammad Zulfaqar Azmi * remove accel and jerk parameters Signed-off-by: Muhammad Zulfaqar Azmi * remove unnecessary parameters Signed-off-by: Muhammad Zulfaqar Azmi * fix param file in config Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/src/scene_module/lane_change/util.cpp Co-authored-by: Takamasa Horibe * remove isStopState and refactoring Signed-off-by: Muhammad Zulfaqar Azmi * Fixed CANCEL when ego is out of lane Signed-off-by: Muhammad Zulfaqar Azmi * fix path reset during abort Signed-off-by: Muhammad Zulfaqar Azmi * fix abort path exceed goal Signed-off-by: Muhammad Zulfaqar Azmi * fix logger to debug Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takamasa Horibe --- .../config/lane_change/lane_change.param.yaml | 19 +- .../lane_change/lane_change_module.hpp | 38 ++- .../lane_change/lane_change_module_data.hpp | 29 +- .../lane_change/lane_change_path.hpp | 3 + .../scene_module/lane_change/util.hpp | 20 +- .../behavior_path_planner/utilities.hpp | 5 +- .../src/behavior_path_planner_node.cpp | 31 ++- .../lane_change/lane_change_module.cpp | 260 +++++++++++------- .../src/scene_module/lane_change/util.cpp | 214 ++++++++++++-- .../behavior_path_planner/src/utilities.cpp | 3 +- 10 files changed, 469 insertions(+), 153 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index bc5623e87ca9d..9ccef914fbfdc 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -17,13 +17,18 @@ maximum_deceleration: 1.0 # [m/s2] lane_change_sampling_num: 10 - abort_lane_change_velocity_thresh: 0.5 - abort_lane_change_angle_thresh: 10.0 # [deg] - abort_lane_change_distance_thresh: 0.3 # [m] + # collision check + enable_collision_check_at_prepare_phase: false prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] - - enable_abort_lane_change: true - enable_collision_check_at_prepare_phase: true use_predicted_path_outside_lanelet: true - use_all_predicted_path: false + use_all_predicted_path: true + + # abort + enable_cancel_lane_change: true + enable_abort_lane_change: false + + abort_delta_time: 3.0 # [s] + abort_max_lateral_jerk: 5.0 # [m/s3] + + # debug publish_debug_marker: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 39de619c4e3eb..0ce55c2796f08 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -54,8 +54,6 @@ class LaneChangeModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters); - BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; bool isExecutionReady() const override; BT::NodeStatus updateState() override; @@ -103,16 +101,24 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; + LaneChangeStates current_lane_change_state_; + std::shared_ptr abort_path_; + PathWithLaneId prev_approved_path_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; + bool is_abort_path_approved_{false}; + bool is_abort_approval_requested_{false}; + bool is_abort_condition_satisfied_{false}; + bool is_activated_ = false; RTCInterface rtc_interface_left_; RTCInterface rtc_interface_right_; UUID uuid_left_; UUID uuid_right_; + UUID candidate_uuid_; - bool is_activated_ = false; + void resetParameters(); void waitApprovalLeft(const double start_distance, const double finish_distance) { @@ -134,12 +140,14 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_left_.updateCooperateStatus( uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); + candidate_uuid_ = uuid_left_; return; } if (candidate.lateral_shift < 0.0) { rtc_interface_right_.updateCooperateStatus( uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); + candidate_uuid_ = uuid_right_; return; } @@ -154,6 +162,21 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_right_.clearCooperateStatus(); } + void removePreviousRTCStatusLeft() + { + if (rtc_interface_left_.isRegistered(uuid_left_)) { + rtc_interface_left_.removeCooperateStatus(uuid_left_); + } + } + + void removePreviousRTCStatusRight() + { + if (rtc_interface_right_.isRegistered(uuid_right_)) { + rtc_interface_right_.removeCooperateStatus(uuid_right_); + } + } + + lanelet::ConstLanelets get_original_lanes() const; PathWithLaneId getReferencePath() const; lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; @@ -165,6 +188,7 @@ class LaneChangeModule : public SceneModuleInterface void generateExtendedDrivableArea(PathWithLaneId & path); void updateOutputTurnSignal(BehaviorModuleOutput & output); void updateSteeringFactorPtr(const BehaviorModuleOutput & output); + bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; @@ -172,20 +196,20 @@ class LaneChangeModule : public SceneModuleInterface bool isValidPath(const PathWithLaneId & path) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; - bool isAbortConditionSatisfied() const; + bool isAbortConditionSatisfied(); bool hasFinishedLaneChange() const; - void resetParameters(); + bool isAbortState() const; // getter Pose getEgoPose() const; Twist getEgoTwist() const; std_msgs::msg::Header getRouteHeader() const; + void resetPathIfAbort(); // debug + void setObjectDebugVisualization() const; mutable std::unordered_map object_debug_; mutable std::vector debug_valid_path_; - - void setObjectDebugVisualization() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index 0c0075655562f..dfcb0f5e24c3b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -25,6 +25,7 @@ namespace behavior_path_planner { struct LaneChangeParameters { + // trajectory generation double lane_change_prepare_duration{2.0}; double lane_changing_safety_check_duration{4.0}; double lane_changing_lateral_jerk{0.5}; @@ -34,18 +35,26 @@ struct LaneChangeParameters double prediction_time_resolution{0.5}; double maximum_deceleration{1.0}; int lane_change_sampling_num{10}; - double abort_lane_change_velocity_thresh{0.5}; - double abort_lane_change_angle_thresh{0.174533}; - double abort_lane_change_distance_thresh{0.3}; - double prepare_phase_ignore_target_speed_thresh{0.1}; - bool enable_abort_lane_change{true}; + + // collision check bool enable_collision_check_at_prepare_phase{true}; + double prepare_phase_ignore_target_speed_thresh{0.1}; bool use_predicted_path_outside_lanelet{false}; bool use_all_predicted_path{false}; - bool publish_debug_marker{false}; + + // abort + bool enable_cancel_lane_change{true}; + bool enable_abort_lane_change{false}; + + double abort_delta_time{3.0}; + double abort_max_lateral_jerk{10.0}; + // drivable area expansion double drivable_area_right_bound_offset{0.0}; double drivable_area_left_bound_offset{0.0}; + + // debug marker + bool publish_debug_marker{false}; }; struct LaneChangeStatus @@ -59,6 +68,14 @@ struct LaneChangeStatus bool is_safe; double start_distance; }; + +enum class LaneChangeStates { + Normal = 0, + Cancel, + Abort, + Stop, +}; + } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp index e346abbf6beeb..83de5e24d5dba 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp @@ -29,12 +29,15 @@ using behavior_path_planner::TurnSignalInfo; struct LaneChangePath { PathWithLaneId path; + lanelet::ConstLanelets reference_lanelets; + lanelet::ConstLanelets target_lanelets; ShiftedPath shifted_path; ShiftLine shift_line; double acceleration{0.0}; double preparation_length{0.0}; double lane_change_length{0.0}; TurnSignalInfo turn_signal_info; + PathWithLaneId prev_path; }; using LaneChangePaths = std::vector; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 15457813c1c32..4c8a7e9fb91a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -97,7 +97,7 @@ bool isLaneChangePathSafe( const size_t current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & lane_change_parameters, - const double front_decel, const double rear_decel, + const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, std::unordered_map & debug_data, const bool use_buffer = true, const double acceleration = 0.0); @@ -133,12 +133,6 @@ PathWithLaneId getLaneChangePathLaneChangingSegment( bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param); -bool isEgoDistanceNearToCenterline( - const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose, - const LaneChangeParameters & lane_change_param); -bool isEgoHeadingAngleLessThanThreshold( - const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose, - const LaneChangeParameters & lane_change_param); TurnSignalInfo calc_turn_signal_info( const PathWithLaneId & prepare_path, const double prepare_velocity, @@ -151,6 +145,18 @@ void get_turn_signal_info( std::vector generateDrivableLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); + +std::optional getAbortPaths( + const std::shared_ptr & planner_data, const LaneChangePath & selected_path, + const Pose & ego_lerp_pose_before_collision, const BehaviorPathPlannerParameters & common_param, + const LaneChangeParameters & lane_change_param); + +double getLateralShift(const LaneChangePath & path); + +bool hasEnoughDistanceToLaneChangeAfterAbort( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const Pose & curent_pose, const double abort_return_dist, + const BehaviorPathPlannerParameters & common_param); } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 42218f54c746d..ca4f924677b2d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -199,6 +199,9 @@ bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_p bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon); +bool calcObjectPolygon( + const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon); + bool calcObjectPolygon( const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon); @@ -478,7 +481,7 @@ bool isSafeInLaneletCollisionCheck( const double check_start_time, const double check_end_time, const double check_time_resolution, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug); + const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug); bool isSafeInFreeSpaceCollisionCheck( const Pose & ego_current_pose, const Twist & ego_current_twist, diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 30760a07f5224..3bdde3fd5cf91 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -381,6 +381,8 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() }; LaneChangeParameters p{}; + + // trajectory generation p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0); p.lane_changing_safety_check_duration = dp("lane_changing_safety_check_duration", 4.0); p.lane_changing_lateral_jerk = dp("lane_changing_lateral_jerk", 0.5); @@ -390,19 +392,27 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.lane_change_sampling_num = dp("lane_change_sampling_num", 10); - p.abort_lane_change_velocity_thresh = dp("abort_lane_change_velocity_thresh", 0.5); - p.abort_lane_change_angle_thresh = - dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0)); - p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3); - p.prepare_phase_ignore_target_speed_thresh = dp("prepare_phase_ignore_target_speed_thresh", 0.1); - p.enable_abort_lane_change = dp("enable_abort_lane_change", true); + + // collision check p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); + p.prepare_phase_ignore_target_speed_thresh = dp("prepare_phase_ignore_target_speed_thresh", 0.1); p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); p.use_all_predicted_path = dp("use_all_predicted_path", true); - p.publish_debug_marker = dp("publish_debug_marker", false); + + // abort + p.enable_cancel_lane_change = dp("enable_cancel_lane_change", true); + p.enable_abort_lane_change = dp("enable_abort_lane_change", false); + + p.abort_delta_time = dp("abort_delta_time", 3.0); + p.abort_max_lateral_jerk = dp("abort_max_lateral_jerk", 10.0); + + // drivable area expansion p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + // debug marker + p.publish_debug_marker = dp("publish_debug_marker", false); + // validation of parameters if (p.lane_change_sampling_num < 1) { RCLCPP_FATAL_STREAM( @@ -419,6 +429,13 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() exit(EXIT_FAILURE); } + if (p.abort_delta_time < 1.0) { + RCLCPP_FATAL_STREAM( + get_logger(), "abort_delta_time: " << p.abort_delta_time << ", is too short.\n" + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + return p; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 507b5dd791ba2..c78da5e8e1769 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -36,6 +36,15 @@ #include #include +std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; + } + return ss.str(); +} + namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::ObjectClassification; @@ -52,27 +61,11 @@ LaneChangeModule::LaneChangeModule( steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); } -BehaviorModuleOutput LaneChangeModule::run() -{ - RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); - current_state_ = BT::NodeStatus::RUNNING; - is_activated_ = isActivated(); - auto output = plan(); - - if (!isSafe()) { - current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop - return output; - } - - updateSteeringFactorPtr(output); - - return output; -} - void LaneChangeModule::onEntry() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onEntry"); current_state_ = BT::NodeStatus::SUCCESS; + current_lane_change_state_ = LaneChangeStates::Normal; updateLaneChangeStatus(); } @@ -123,12 +116,19 @@ BT::NodeStatus LaneChangeModule::updateState() return current_state_; } + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + if (isAbortState() && !is_within_current_lane) { + current_state_ = BT::NodeStatus::RUNNING; + return current_state_; + } + if (isAbortConditionSatisfied()) { - if (isNearEndOfLane() && isCurrentSpeedLow()) { + if ((isNearEndOfLane() && isCurrentSpeedLow()) || !is_within_current_lane) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } - // cancel lane change path + current_state_ = BT::NodeStatus::FAILURE; return current_state_; } @@ -144,51 +144,89 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { resetPathCandidate(); + is_activated_ = isActivated(); - auto path = status_.lane_change_path.path; - + PathWithLaneId path = status_.lane_change_path.path; if (!isValidPath(path)) { status_.is_safe = false; return BehaviorModuleOutput{}; } - generateExtendedDrivableArea(path); - if (isAbortConditionSatisfied()) { - if (isNearEndOfLane() && isCurrentSpeedLow()) { - const auto stop_point = util::insertStopPoint(0.1, &path); + if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow())) { + const auto stop_point = util::insertStopPoint(0.1, &path); + } + + if (isAbortState()) { + resetPathIfAbort(); + if (is_activated_) { + path = abort_path_->path; } } + generateExtendedDrivableArea(path); + + prev_approved_path_ = path; + BehaviorModuleOutput output; output.path = std::make_shared(path); updateOutputTurnSignal(output); + updateSteeringFactorPtr(output); + return output; } +void LaneChangeModule::resetPathIfAbort() +{ + if (!is_abort_approval_requested_) { + const auto lateral_shift = lane_change_utils::getLateralShift(*abort_path_); + if (lateral_shift > 0.0) { + removePreviousRTCStatusRight(); + uuid_right_ = generateUUID(); + } else if (lateral_shift < 0.0) { + removePreviousRTCStatusLeft(); + uuid_left_ = generateUUID(); + } + RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); + is_abort_approval_requested_ = true; + is_abort_path_approved_ = false; + return; + } + + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); + is_abort_path_approved_ = true; + clearWaitingApproval(); + } else { + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); + is_abort_path_approved_ = false; + waitApproval(); + } +} + CandidateOutput LaneChangeModule::planCandidate() const { CandidateOutput output; + LaneChangePath selected_path; // Get lane change lanes const auto current_lanes = util::getCurrentLanes(planner_data_); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - LaneChangePath selected_path; [[maybe_unused]] const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); + if (isAbortState()) { + selected_path = *abort_path_; + } + if (selected_path.path.points.empty()) { return output; } - const auto & start_idx = selected_path.shift_line.start_idx; - const auto & end_idx = selected_path.shift_line.end_idx; - output.path_candidate = selected_path.path; - output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) - - selected_path.shifted_path.shift_length.at(start_idx); + output.lateral_shift = lane_change_utils::getLateralShift(selected_path); output.start_distance_to_path_change = motion_utils::calcSignedArcLength( selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( @@ -200,12 +238,21 @@ CandidateOutput LaneChangeModule::planCandidate() const BehaviorModuleOutput LaneChangeModule::planWaitingApproval() { + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + + if (is_within_current_lane) { + prev_approved_path_ = getReferencePath(); + } + BehaviorModuleOutput out; - out.path = std::make_shared(getReferencePath()); + out.path = std::make_shared(prev_approved_path_); + const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus(candidate); waitApproval(); + is_abort_path_approved_ = false; return out; } @@ -426,18 +473,12 @@ bool LaneChangeModule::isCurrentSpeedLow() const return util::l2Norm(getEgoTwist().linear) < threshold_ms; } -bool LaneChangeModule::isAbortConditionSatisfied() const +bool LaneChangeModule::isAbortConditionSatisfied() { - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & common_parameters = planner_data_->parameters; - - const auto & current_lanes = status_.current_lanes; + is_abort_condition_satisfied_ = false; + current_lane_change_state_ = LaneChangeStates::Normal; - // check abort enable flag - if (!parameters_->enable_abort_lane_change) { + if (!parameters_->enable_cancel_lane_change) { return false; } @@ -445,70 +486,70 @@ bool LaneChangeModule::isAbortConditionSatisfied() const return false; } - // find closest lanelet in original lane - lanelet::ConstLanelet closest_lanelet{}; - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - if (!lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet)) { - RCLCPP_ERROR_THROTTLE( - getLogger(), clock, 1000, - "Failed to find closest lane! Lane change aborting function is not working!"); - return false; - } - - // check if lane change path is still safe - const bool is_path_safe = std::invoke([this, &route_handler, &dynamic_objects, ¤t_lanes, - ¤t_pose, ¤t_twist, &common_parameters]() { - constexpr double check_distance = 100.0; - // get lanes used for detection - const auto & path = status_.lane_change_path; - const double check_distance_with_path = - check_distance + path.preparation_length + path.lane_change_length; - const auto check_lanes = route_handler->getCheckTargetLanesFromPath( - path.path, status_.lane_change_lanes, check_distance_with_path); - - std::unordered_map debug_data; - - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); - return lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, - current_twist, common_parameters, *parameters_, - common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, debug_data, false, - status_.lane_change_path.acceleration); - }); + Pose ego_pose_before_collision; + const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { - // check vehicle velocity thresh - const bool is_velocity_low = - util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; - const bool is_within_original_lane = - lane_change_utils::isEgoWithinOriginalLane(current_lanes, current_pose, common_parameters); - if (is_velocity_low && is_within_original_lane) { + const auto & common_parameters = planner_data_->parameters; + const bool is_within_original_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), common_parameters); + + if (is_within_original_lane) { + current_lane_change_state_ = LaneChangeStates::Cancel; return true; } - const bool is_distance_small = - lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, *parameters_); + // check abort enable flag + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), *clock_, 1000, + "DANGER!!! Path is not safe anymore, but it is too late to CANCEL! Please be cautious"); + + if (!parameters_->enable_abort_lane_change) { + current_lane_change_state_ = LaneChangeStates::Stop; + return false; + } - // check angle thresh from original lane - const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( - closest_lanelet, current_pose, *parameters_); + const auto found_abort_path = lane_change_utils::getAbortPaths( + planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, + *parameters_); - if (is_distance_small && is_angle_diff_small) { + if (!found_abort_path && !is_abort_path_approved_) { + current_lane_change_state_ = LaneChangeStates::Stop; return true; } - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), clock, 1000, - "DANGER!!! Path is not safe anymore, but it is too late to abort! Please be cautious"); + + current_lane_change_state_ = LaneChangeStates::Abort; + + if (!is_abort_path_approved_) { + abort_path_ = std::make_shared(*found_abort_path); + } + + return true; } return false; } +bool LaneChangeModule::isAbortState() const +{ + if (!parameters_->enable_abort_lane_change) { + return false; + } + + if (current_lane_change_state_ != LaneChangeStates::Abort) { + return false; + } + + if (!abort_path_) { + return false; + } + + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), *clock_, 1000, + "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); + return true; +} + bool LaneChangeModule::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -624,6 +665,36 @@ void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) util::generateDrivableArea(path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } +bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const +{ + const auto current_pose = getEgoPose(); + const auto current_twist = getEgoTwist(); + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & current_lanes = status_.current_lanes; + const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; + const auto path = status_.lane_change_path; + + constexpr double check_distance = 100.0; + // get lanes used for detection + const double check_distance_with_path = + check_distance + path.preparation_length + path.lane_change_length; + const auto check_lanes = route_handler->getCheckTargetLanesFromPath( + path.path, status_.lane_change_lanes, check_distance_with_path); + + std::unordered_map debug_data; + + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + return lane_change_utils::isLaneChangePathSafe( + path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, + current_twist, common_parameters, *parameters_, + common_parameters.expected_front_deceleration_for_abort, + common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, + false, status_.lane_change_path.acceleration); +} + void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) { const auto turn_signal_info = util::getPathTurnSignal( @@ -637,6 +708,11 @@ void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) void LaneChangeModule::resetParameters() { + is_abort_path_approved_ = false; + is_abort_approval_requested_ = false; + current_lane_change_state_ = LaneChangeStates::Normal; + abort_path_ = nullptr; + clearWaitingApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 4452c36522546..35d89fc91322e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -132,6 +132,8 @@ std::optional constructCandidatePath( candidate_path.preparation_length = prepare_distance; candidate_path.lane_change_length = lane_change_distance; candidate_path.shift_line = shift_line; + candidate_path.reference_lanelets = original_lanelets; + candidate_path.target_lanelets = target_lanelets; const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); const PathPointWithLaneId & lane_changing_end_point = lane_changing_segment.points.front(); @@ -321,11 +323,13 @@ bool selectSafePath( const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); + Pose ego_pose_before_collision; if (isLaneChangePathSafe( path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, common_parameters, ros_parameters, common_parameters.expected_front_deceleration, - common_parameters.expected_rear_deceleration, debug_data, true, path.acceleration)) { + common_parameters.expected_rear_deceleration, ego_pose_before_collision, debug_data, true, + path.acceleration)) { *selected_path = path; return true; } @@ -384,8 +388,9 @@ bool isLaneChangePathSafe( const size_t current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters, const double front_decel, - const double rear_decel, std::unordered_map & debug_data, - const bool use_buffer, const double acceleration) + const double rear_decel, Pose & ego_pose_before_collision, + std::unordered_map & debug_data, const bool use_buffer, + const double acceleration) { if (dynamic_objects == nullptr) { return true; @@ -463,7 +468,7 @@ bool isLaneChangePathSafe( if (!util::isSafeInLaneletCollisionCheck( current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, - rear_decel, current_debug_data.second)) { + rear_decel, ego_pose_before_collision, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } @@ -500,7 +505,7 @@ bool isLaneChangePathSafe( if (!util::isSafeInLaneletCollisionCheck( current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, - rear_decel, current_debug_data.second)) { + rear_decel, ego_pose_before_collision, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } @@ -660,26 +665,6 @@ bool isEgoWithinOriginalLane( lanelet::utils::to2D(lane_poly).basicPolygon()); } -bool isEgoDistanceNearToCenterline( - const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose, - const LaneChangeParameters & lane_change_param) -{ - const auto centerline2d = lanelet::utils::to2D(closest_lanelet.centerline()).basicLineString(); - lanelet::BasicPoint2d vehicle_pose2d(current_pose.position.x, current_pose.position.y); - const double distance = lanelet::geometry::distance2d(centerline2d, vehicle_pose2d); - return distance < lane_change_param.abort_lane_change_distance_thresh; -} - -bool isEgoHeadingAngleLessThanThreshold( - const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose, - const LaneChangeParameters & lane_change_param) -{ - const double lane_angle = lanelet::utils::getLaneletAngle(closest_lanelet, current_pose.position); - const double vehicle_yaw = tf2::getYaw(current_pose.orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); - return std::abs(yaw_diff) < lane_change_param.abort_lane_change_angle_thresh; -} - TurnSignalInfo calc_turn_signal_info( const PathWithLaneId & prepare_path, const double prepare_velocity, const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line, @@ -764,4 +749,183 @@ std::vector generateDrivableLanes( return drivable_lanes; } +std::optional getAbortPaths( + const std::shared_ptr & planner_data, const LaneChangePath & selected_path, + [[maybe_unused]] const Pose & ego_pose_before_collision, + const BehaviorPathPlannerParameters & common_param, + [[maybe_unused]] const LaneChangeParameters & lane_change_param) +{ + const auto & route_handler = planner_data->route_handler; + const auto current_speed = util::l2Norm(planner_data->self_odometry->twist.twist.linear); + const auto current_pose = planner_data->self_pose->pose; + const auto reference_lanelets = selected_path.reference_lanelets; + + const auto ego_nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; + const auto ego_nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; + const double minimum_lane_change_length = util::calcLaneChangeBuffer(common_param, 1, 0.0); + + const auto & lane_changing_path = selected_path.path; + const auto lane_changing_end_pose_idx = std::invoke([&]() { + constexpr double s_start = 0.0; + const double s_end = + lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length; + + const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); + return motion_utils::findFirstNearestIndexWithSoftConstraints( + lane_changing_path.points, ref.points.back().point.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + }); + + const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + lane_changing_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + + const auto get_abort_idx_and_distance = [&](const double param_time) { + double turning_point_dist{0.0}; + if (ego_pose_idx > lane_changing_end_pose_idx) { + return std::make_pair(ego_pose_idx, turning_point_dist); + } + + constexpr auto min_speed{2.77}; + const auto desired_distance = std::max(min_speed, current_speed) * param_time; + const auto & points = lane_changing_path.points; + size_t idx{0}; + for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { + const auto dist_to_ego = + util::getSignedDistance(current_pose, points.at(idx).point.pose, reference_lanelets); + turning_point_dist = dist_to_ego; + if (dist_to_ego > desired_distance) { + break; + } + } + return std::make_pair(idx, turning_point_dist); + }; + + const auto abort_delta_time = lane_change_param.abort_delta_time; + const auto [abort_start_idx, abort_start_dist] = get_abort_idx_and_distance(abort_delta_time); + const auto [abort_return_idx, abort_return_dist] = + get_abort_idx_and_distance(abort_delta_time * 2); + + if (abort_start_idx >= abort_return_idx) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "abort start idx and return idx is equal. can't compute abort path."); + return std::nullopt; + } + + if (!hasEnoughDistanceToLaneChangeAfterAbort( + *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "insufficient distance to abort."); + return std::nullopt; + } + + const auto abort_start_pose = lane_changing_path.points.at(abort_start_idx).point.pose; + const auto abort_return_pose = lane_changing_path.points.at(abort_return_idx).point.pose; + const auto arc_position = + lanelet::utils::getArcCoordinates(reference_lanelets, abort_return_pose); + const PathWithLaneId reference_lane_segment = std::invoke([&]() { + const double minimum_lane_change_length = + common_param.backward_length_buffer_for_end_of_lane + common_param.minimum_lane_change_length; + + const double s_start = arc_position.length; + double s_end = + lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length; + + if (route_handler->isInGoalRouteSection(selected_path.target_lanelets.back())) { + const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates( + selected_path.target_lanelets, route_handler->getGoalPose()); + s_end = std::min(s_end, goal_arc_coordinates.length) - minimum_lane_change_length; + } + PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); + ref.points.back().point.longitudinal_velocity_mps = std::min( + ref.points.back().point.longitudinal_velocity_mps, + static_cast(lane_change_param.minimum_lane_change_velocity)); + return ref; + }); + + ShiftLine shift_line; + shift_line.start = abort_start_pose; + shift_line.end = abort_return_pose; + shift_line.end_shift_length = -arc_position.distance; + shift_line.start_idx = abort_start_idx; + shift_line.end_idx = abort_return_idx; + + PathShifter path_shifter; + path_shifter.setPath(lane_changing_path); + path_shifter.addShiftLine(shift_line); + const auto lateral_jerk = behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( + shift_line.end_shift_length, abort_start_dist, current_speed); + + if (lateral_jerk > lane_change_param.abort_max_lateral_jerk) { + return std::nullopt; + } + + ShiftedPath shifted_path; + // offset front side + // bool offset_back = false; + if (!path_shifter.generate(&shifted_path)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "failed to generate abort shifted path."); + } + + PathWithLaneId start_to_abort_return_pose; + start_to_abort_return_pose.points.insert( + start_to_abort_return_pose.points.end(), shifted_path.path.points.begin(), + shifted_path.path.points.begin() + abort_return_idx); + start_to_abort_return_pose.points.insert( + start_to_abort_return_pose.points.end(), reference_lane_segment.points.begin(), + reference_lane_segment.points.end()); + + auto abort_path = selected_path; + abort_path.shifted_path = shifted_path; + abort_path.path = start_to_abort_return_pose; + abort_path.shift_line = shift_line; + return std::optional{abort_path}; +} + +double getLateralShift(const LaneChangePath & path) +{ + const auto start_idx = path.shift_line.start_idx; + const auto end_idx = path.shift_line.end_idx; + + return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); +} + +bool hasEnoughDistanceToLaneChangeAfterAbort( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const Pose & current_pose, const double abort_return_dist, + const BehaviorPathPlannerParameters & common_param) +{ + const auto minimum_lane_change_distance = common_param.minimum_lane_change_prepare_distance + + common_param.minimum_lane_change_length + + common_param.backward_length_buffer_for_end_of_lane; + const auto abort_plus_lane_change_distance = abort_return_dist + minimum_lane_change_distance; + if (abort_plus_lane_change_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) { + return false; + } + + if ( + abort_plus_lane_change_distance > + util::getDistanceToNextIntersection(current_pose, current_lanes)) { + return false; + } + + if ( + route_handler.isInGoalRouteSection(current_lanes.back()) && + abort_plus_lane_change_distance > + util::getSignedDistance(current_pose, route_handler.getGoalPose(), current_lanes)) { + return false; + } + + if ( + abort_plus_lane_change_distance > + util::getDistanceToCrosswalk( + current_pose, current_lanes, *route_handler.getOverallGraphPtr())) { + return false; + } + + return true; +} } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 9ef2139e9210c..f4842edf7edd5 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2462,7 +2462,7 @@ bool isSafeInLaneletCollisionCheck( const double check_start_time, const double check_end_time, const double check_time_resolution, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug) + const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug) { const auto lerp_path_reserve = (check_end_time - check_start_time) / check_time_resolution; if (lerp_path_reserve > 1e-3) { @@ -2502,6 +2502,7 @@ bool isSafeInLaneletCollisionCheck( debug.failed_reason = "not_enough_longitudinal"; return false; } + ego_pose_before_collision = expected_ego_pose; } return true; }