From 93447c152a7ab7041b6ef40a52044a9c9bb209ff Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Fri, 18 Nov 2022 23:16:29 +0900 Subject: [PATCH 01/27] feat(behavior_path_planner): abort lane change function Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 90 +++++++- .../lane_change/lane_change_module_data.hpp | 11 +- .../lane_change/lane_change_path.hpp | 3 + .../scene_module/lane_change/util.hpp | 8 +- .../behavior_path_planner/utilities.hpp | 5 +- .../src/behavior_path_planner_node.cpp | 3 +- .../lane_change/lane_change_module.cpp | 201 ++++++++++++------ .../src/scene_module/lane_change/util.cpp | 147 ++++++++++++- .../behavior_path_planner/src/utilities.cpp | 3 +- 9 files changed, 396 insertions(+), 75 deletions(-) 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..9d6fbecbf96cc 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 @@ -47,6 +47,20 @@ using marker_utils::CollisionCheckDebug; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; +inline std::string toStr(const LaneChangeStates state) +{ + if (state == LaneChangeStates::Trying) { + return "Trying"; + } else if (state == LaneChangeStates::Success) { + return "Success"; + } else if (state == LaneChangeStates::Revert) { + return "Revert"; + } else if (state == LaneChangeStates::Abort) { + return "Abort"; + } + return "UNKNOWN"; +} + class LaneChangeModule : public SceneModuleInterface { public: @@ -54,8 +68,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,6 +115,11 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; + LaneDepartureChecker lane_departure_checker_; + mutable LaneChangeStates current_lane_change_state_; + mutable std::shared_ptr abort_path_; + PathWithLaneId prev_approved_path_; + mutable Pose abort_non_collision_pose_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; @@ -111,9 +128,15 @@ class LaneChangeModule : public SceneModuleInterface RTCInterface rtc_interface_right_; UUID uuid_left_; UUID uuid_right_; + UUID candidate_uuid_; + + bool is_abort_path_approved_ = false; + bool is_abort_approval_requested_ = false; bool is_activated_ = false; + void resetParameters(); + void waitApprovalLeft(const double start_distance, const double finish_distance) { rtc_interface_left_.updateCooperateStatus( @@ -128,18 +151,49 @@ class LaneChangeModule : public SceneModuleInterface is_waiting_approval_ = true; } + void updateRegisteredRTCStatus(const LaneChangePath & path) + { + const auto start_distance_to_path_change = motion_utils::calcSignedArcLength( + path.path.points, planner_data_->self_pose->pose.position, path.shift_line.start.position); + + const auto finish_distance_to_path_change = motion_utils::calcSignedArcLength( + path.path.points, planner_data_->self_pose->pose.position, path.shift_line.end.position); + + const auto & start_idx = path.shift_line.start_idx; + const auto & end_idx = path.shift_line.end_idx; + const auto lateral_shift = + path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); + + if (lateral_shift > 0.0) { + rtc_interface_left_.updateCooperateStatus( + uuid_left_, isExecutionReady(), start_distance_to_path_change, + finish_distance_to_path_change, clock_->now()); + candidate_uuid_ = uuid_left_; + } else { + rtc_interface_right_.updateCooperateStatus( + uuid_right_, isExecutionReady(), start_distance_to_path_change, + finish_distance_to_path_change, clock_->now()); + candidate_uuid_ = uuid_right_; + } + + RCLCPP_WARN_STREAM( + getLogger(), "Direction is UNKNOWN, start_distance = " << start_distance_to_path_change); + } + void updateRTCStatus(const CandidateOutput & candidate) { if (candidate.lateral_shift > 0.0) { 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 +208,30 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_right_.clearCooperateStatus(); } + void removeCandidateRTCStatus() + { + if (rtc_interface_left_.isRegistered(candidate_uuid_)) { + rtc_interface_left_.removeCooperateStatus(candidate_uuid_); + } else if (rtc_interface_right_.isRegistered(candidate_uuid_)) { + rtc_interface_right_.removeCooperateStatus(candidate_uuid_); + } + } + + 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; @@ -172,16 +250,18 @@ 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 isCancel() const; + bool isAbort() const; // getter Pose getEgoPose() const; Twist getEgoTwist() const; std_msgs::msg::Header getRouteHeader() const; + void resetPathIfAbort(PathWithLaneId & selected_path); // debug + void append_marker_array(const MarkerArray & marker_array) const; mutable std::unordered_map object_debug_; mutable std::vector debug_valid_path_; 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..1752182ce1db4 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 @@ -38,7 +38,8 @@ struct LaneChangeParameters 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}; + bool enable_cancel_lane_change{true}; + bool enable_abort_lane_change{false}; bool enable_collision_check_at_prepare_phase{true}; bool use_predicted_path_outside_lanelet{false}; bool use_all_predicted_path{false}; @@ -59,6 +60,14 @@ struct LaneChangeStatus bool is_safe; double start_distance; }; + +enum class LaneChangeStates { + Trying = 0, + Success, + Revert, + Abort, +}; + } // 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..472440b06cfa0 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); @@ -151,6 +151,12 @@ 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, ShiftLine & shift); +double getLateralShift(const LaneChangePath & path); + } // 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..dfa60d7443c5a 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -395,7 +395,8 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() 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); + p.enable_cancel_lane_change = dp("enable_cancel_lane_change", true); + p.enable_abort_lane_change = dp("enable_abort_lane_change", false); p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); p.use_all_predicted_path = dp("use_all_predicted_path", true); 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..0d9c75de6fe06 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::Trying; updateLaneChangeStatus(); } @@ -124,11 +117,11 @@ BT::NodeStatus LaneChangeModule::updateState() } if (isAbortConditionSatisfied()) { - if (isNearEndOfLane() && isCurrentSpeedLow()) { + if ((isNearEndOfLane() && isCurrentSpeedLow()) || isAbort()) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } - // cancel lane change path + current_state_ = BT::NodeStatus::FAILURE; return current_state_; } @@ -146,16 +139,28 @@ BehaviorModuleOutput LaneChangeModule::plan() resetPathCandidate(); auto 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); + is_activated_ = isActivated(); + constexpr double resample_interval{1.0}; + + RCLCPP_INFO( + getLogger(), "[plan] current_lane_change_state_ = %s", + toStr(current_lane_change_state_).c_str()); + PathWithLaneId selected_path = status_.lane_change_path.path; + PathWithLaneId path; + if (!isAbort()) { + path = util::resamplePathWithSpline(selected_path, resample_interval); + if (!isValidPath(path)) { + status_.is_safe = false; + return BehaviorModuleOutput{}; + } + generateExtendedDrivableArea(path); + prev_approved_path_ = path; + } else { + resetPathIfAbort(selected_path); + path = util::resamplePathWithSpline(selected_path, resample_interval); + generateExtendedDrivableArea(path); + if (is_abort_path_approved_) { + clearWaitingApproval(); } } @@ -163,32 +168,64 @@ BehaviorModuleOutput LaneChangeModule::plan() output.path = std::make_shared(path); updateOutputTurnSignal(output); + if (!isSafe()) { + current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop + } + return output; } +void LaneChangeModule::resetPathIfAbort(PathWithLaneId & selected_path) +{ + 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(); + } + is_abort_approval_requested_ = true; + is_abort_path_approved_ = false; + RCLCPP_ERROR(getLogger(), "[plan] uuid is reset to request abort approval."); + return; + } + + if (isActivated()) { + RCLCPP_INFO(getLogger(), "[plan] isActivated() is true. set is_abort_path_approved to true."); + selected_path = abort_path_->path; + is_abort_path_approved_ = true; + } else { + RCLCPP_INFO(getLogger(), "[plan] 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 (isAbort()) { + 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( @@ -201,7 +238,14 @@ CandidateOutput LaneChangeModule::planCandidate() const BehaviorModuleOutput LaneChangeModule::planWaitingApproval() { BehaviorModuleOutput out; - out.path = std::make_shared(getReferencePath()); + if (!isAbort()) { + const auto lane_keeping_path = getReferencePath(); + prev_approved_path_ = lane_keeping_path; + out.path = std::make_shared(lane_keeping_path); + } else { + out.path = std::make_shared(prev_approved_path_); + } + const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus(candidate); @@ -426,7 +470,7 @@ 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(); @@ -437,11 +481,13 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto & current_lanes = status_.current_lanes; // check abort enable flag - if (!parameters_->enable_abort_lane_change) { + if (!parameters_->enable_cancel_lane_change) { + current_lane_change_state_ = LaneChangeStates::Success; return false; } if (!is_activated_) { + current_lane_change_state_ = LaneChangeStates::Success; return false; } @@ -455,33 +501,36 @@ bool LaneChangeModule::isAbortConditionSatisfied() const return false; } + const auto path = status_.lane_change_path; // 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 bool is_path_safe = + std::invoke([this, &route_handler, &dynamic_objects, &path, ¤t_lanes, ¤t_pose, + ¤t_twist, &common_parameters, &ego_pose_before_collision]() { + 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); + }); // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { // check vehicle velocity thresh + current_lane_change_state_ = LaneChangeStates::Revert; const bool is_velocity_low = util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; const bool is_within_original_lane = @@ -500,14 +549,41 @@ bool LaneChangeModule::isAbortConditionSatisfied() const if (is_distance_small && is_angle_diff_small) { return true; } + + if (!parameters_->enable_abort_lane_change) { + 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; + + ShiftLine shift_line; + const auto abort_path = + lane_change_utils::getAbortPaths(planner_data_, path, ego_pose_before_collision, shift_line); + abort_non_collision_pose_ = ego_pose_before_collision; + + if (abort_path) { + if (isWaitingApproval()) { + abort_path_ = std::make_shared(*abort_path); + } + } + return true; } return false; } +bool LaneChangeModule::isAbort() const +{ + return (current_lane_change_state_ == LaneChangeStates::Abort); +} + +bool LaneChangeModule::isCancel() const +{ + return current_lane_change_state_ == LaneChangeStates::Revert; +} bool LaneChangeModule::hasFinishedLaneChange() const { @@ -637,6 +713,11 @@ void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) void LaneChangeModule::resetParameters() { + is_abort_path_approved_ = false; + is_abort_approval_requested_ = false; + current_lane_change_state_ = LaneChangeStates::Trying; + abort_path_.reset(); + 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..8fa2c17b9ffd6 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; } @@ -764,4 +769,136 @@ 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, ShiftLine & shift) +{ + 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 abort_point_dist = + [&](const double param_accel, const double param_jerk, const double param_time) { + return std::max(1.0, current_speed) * param_time + + param_accel * std::pow(param_time, 2) / 2. - param_jerk * std::pow(param_jerk, 3) / 6.; + }; + + 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; + + constexpr double resample_path{1.0}; + auto resampled_selected_path = util::resamplePathWithSpline(selected_path.path, resample_path); + + const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + resampled_selected_path.points, current_pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + const auto lane_changing_end_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + resampled_selected_path.points, selected_path.shift_line.end, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + + [[maybe_unused]] const auto pose_idx_min = + [&](const double accel, const double jerk, const double param_time, const double min_dist) { + if (ego_pose_idx > lane_changing_end_pose_idx) { + return ego_pose_idx; + } + const double turning_point_dist = + std::min(std::invoke(abort_point_dist, accel, jerk, param_time), min_dist); + const auto & points = resampled_selected_path.points; + double sum{0.0}; + size_t idx{0}; + for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { + sum += tier4_autoware_utils::calcDistance2d(points.at(idx), points.at(idx + 1)); + if (sum > turning_point_dist) { + break; + } + } + return idx; + }; + + [[maybe_unused]] const auto pose_idx_max = + [&](const double accel, const double jerk, const double param_time, const double min_dist) { + if (ego_pose_idx > lane_changing_end_pose_idx) { + return ego_pose_idx; + } + const double turning_point_dist = + std::max(std::invoke(abort_point_dist, accel, jerk, param_time), min_dist); + const auto & points = resampled_selected_path.points; + double sum{0.0}; + size_t idx{0}; + for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { + sum += tier4_autoware_utils::calcDistance2d(points.at(idx), points.at(idx + 1)); + if (sum > turning_point_dist) { + break; + } + } + return idx; + }; + const auto abort_start_idx = pose_idx_min(0.0, 0.5, 3.0, 6.0); + const auto abort_end_idx = pose_idx_max(0.0, 0.5, 6.0, 12.0); + if (abort_start_idx >= abort_end_idx) { + return std::nullopt; + } + + const auto reference_lanelets = selected_path.reference_lanelets; + const auto abort_start_pose = resampled_selected_path.points.at(abort_start_idx).point.pose; + const auto abort_end_pose = resampled_selected_path.points.at(abort_end_idx).point.pose; + const auto arc_position = lanelet::utils::getArcCoordinates(reference_lanelets, abort_end_pose); + const PathWithLaneId reference_lane_segment = std::invoke([&]() { + constexpr double minimum_lane_change_length{17.0}; + + double s_start = arc_position.length; + double s_end = + lanelet::utils::getLaneletLength2d(reference_lanelets) - 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, 5.6f); + return ref; + }); + + ShiftLine shift_line; + shift_line.start = abort_start_pose; + shift_line.end = abort_end_pose; + shift_line.end_shift_length = -arc_position.distance; + shift_line.start_idx = abort_start_idx; + shift_line.end_idx = abort_end_idx; + shift = shift_line; + + PathShifter path_shifter; + path_shifter.setPath(resampled_selected_path); + path_shifter.addShiftLine(shift_line); + + 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 shifted path."); + } + + PathWithLaneId start_to_abort_end_pose; + start_to_abort_end_pose.points.insert( + start_to_abort_end_pose.points.end(), shifted_path.path.points.begin(), + shifted_path.path.points.begin() + abort_end_idx); + start_to_abort_end_pose.points.insert( + start_to_abort_end_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_end_pose; + abort_path.prev_path = resampled_selected_path; + 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); +} + } // 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; } From 5a4c58d343fad5d6d6394dd97fe43d506d8a8198 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 24 Nov 2022 19:50:29 +0900 Subject: [PATCH 02/27] change Revert -> Cancel Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/lane_change_module.hpp | 4 ++-- .../scene_module/lane_change/lane_change_module_data.hpp | 2 +- .../src/scene_module/lane_change/lane_change_module.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) 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 9d6fbecbf96cc..1fcffcda8b7b7 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 @@ -53,8 +53,8 @@ inline std::string toStr(const LaneChangeStates state) return "Trying"; } else if (state == LaneChangeStates::Success) { return "Success"; - } else if (state == LaneChangeStates::Revert) { - return "Revert"; + } else if (state == LaneChangeStates::Cancel) { + return "Cancel"; } else if (state == LaneChangeStates::Abort) { return "Abort"; } 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 1752182ce1db4..8a74032fba0ca 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 @@ -64,7 +64,7 @@ struct LaneChangeStatus enum class LaneChangeStates { Trying = 0, Success, - Revert, + Cancel, Abort, }; 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 0d9c75de6fe06..731d857755d9e 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 @@ -530,7 +530,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { // check vehicle velocity thresh - current_lane_change_state_ = LaneChangeStates::Revert; + current_lane_change_state_ = LaneChangeStates::Cancel; const bool is_velocity_low = util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; const bool is_within_original_lane = @@ -582,7 +582,7 @@ bool LaneChangeModule::isAbort() const bool LaneChangeModule::isCancel() const { - return current_lane_change_state_ == LaneChangeStates::Revert; + return current_lane_change_state_ == LaneChangeStates::Cancel; } bool LaneChangeModule::hasFinishedLaneChange() const From bb4189852da0b375fa93bae27765fb092f635883 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 24 Nov 2022 22:42:32 +0900 Subject: [PATCH 03/27] Remove some unwanted functions and and STOP state Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 15 ++-- .../lane_change/lane_change_module_data.hpp | 4 +- .../scene_module/lane_change/util.hpp | 5 +- .../lane_change/lane_change_module.cpp | 32 ++++---- .../src/scene_module/lane_change/util.cpp | 75 ++++++++----------- 5 files changed, 60 insertions(+), 71 deletions(-) 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 1fcffcda8b7b7..005a39ea27faa 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 @@ -47,16 +47,16 @@ using marker_utils::CollisionCheckDebug; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; -inline std::string toStr(const LaneChangeStates state) +inline std::string_view toStr(const LaneChangeStates state) { - if (state == LaneChangeStates::Trying) { - return "Trying"; - } else if (state == LaneChangeStates::Success) { - return "Success"; + if (state == LaneChangeStates::Normal) { + return "Normal"; } else if (state == LaneChangeStates::Cancel) { return "Cancel"; } else if (state == LaneChangeStates::Abort) { return "Abort"; + } else if (state == LaneChangeStates::Stop) { + return "Stop"; } return "UNKNOWN"; } @@ -252,8 +252,9 @@ class LaneChangeModule : public SceneModuleInterface bool isCurrentSpeedLow() const; bool isAbortConditionSatisfied(); bool hasFinishedLaneChange() const; - bool isCancel() const; - bool isAbort() const; + bool isCancelState() const; + bool isAbortState() const; + bool isStopState() const; // getter Pose getEgoPose() const; Twist getEgoTwist() const; 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 8a74032fba0ca..012b0e57b843b 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 @@ -62,10 +62,10 @@ struct LaneChangeStatus }; enum class LaneChangeStates { - Trying = 0, - Success, + Normal = 0, Cancel, Abort, + Stop, }; } // namespace behavior_path_planner 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 472440b06cfa0..bb0c2b03d1e54 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 @@ -154,9 +154,10 @@ std::vector generateDrivableLanes( std::optional getAbortPaths( const std::shared_ptr & planner_data, const LaneChangePath & selected_path, - const Pose & ego_lerp_pose_before_collision, ShiftLine & shift); -double getLateralShift(const LaneChangePath & path); + const Pose & ego_lerp_pose_before_collision, const BehaviorPathPlannerParameters & common_param, + const LaneChangeParameters & lane_change_param); +double getLateralShift(const LaneChangePath & path); } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ 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 731d857755d9e..96c5f3d53b6eb 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 @@ -65,7 +65,7 @@ void LaneChangeModule::onEntry() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onEntry"); current_state_ = BT::NodeStatus::SUCCESS; - current_lane_change_state_ = LaneChangeStates::Trying; + current_lane_change_state_ = LaneChangeStates::Normal; updateLaneChangeStatus(); } @@ -117,7 +117,7 @@ BT::NodeStatus LaneChangeModule::updateState() } if (isAbortConditionSatisfied()) { - if ((isNearEndOfLane() && isCurrentSpeedLow()) || isAbort()) { + if ((isNearEndOfLane() && isCurrentSpeedLow()) || isAbortState()) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } @@ -144,10 +144,10 @@ BehaviorModuleOutput LaneChangeModule::plan() RCLCPP_INFO( getLogger(), "[plan] current_lane_change_state_ = %s", - toStr(current_lane_change_state_).c_str()); + toStr(current_lane_change_state_).data()); PathWithLaneId selected_path = status_.lane_change_path.path; PathWithLaneId path; - if (!isAbort()) { + if (!isAbortState()) { path = util::resamplePathWithSpline(selected_path, resample_interval); if (!isValidPath(path)) { status_.is_safe = false; @@ -216,7 +216,7 @@ CandidateOutput LaneChangeModule::planCandidate() const getSafePath(lane_change_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - if (isAbort()) { + if (isAbortState()) { selected_path = *abort_path_; } @@ -238,7 +238,7 @@ CandidateOutput LaneChangeModule::planCandidate() const BehaviorModuleOutput LaneChangeModule::planWaitingApproval() { BehaviorModuleOutput out; - if (!isAbort()) { + if (!isAbortState()) { const auto lane_keeping_path = getReferencePath(); prev_approved_path_ = lane_keeping_path; out.path = std::make_shared(lane_keeping_path); @@ -482,12 +482,12 @@ bool LaneChangeModule::isAbortConditionSatisfied() // check abort enable flag if (!parameters_->enable_cancel_lane_change) { - current_lane_change_state_ = LaneChangeStates::Success; + current_lane_change_state_ = LaneChangeStates::Normal; return false; } if (!is_activated_) { - current_lane_change_state_ = LaneChangeStates::Success; + current_lane_change_state_ = LaneChangeStates::Normal; return false; } @@ -560,27 +560,29 @@ bool LaneChangeModule::isAbortConditionSatisfied() "DANGER!!! Path is not safe anymore, but it is too late to abort! Please be cautious"); current_lane_change_state_ = LaneChangeStates::Abort; - ShiftLine shift_line; - const auto abort_path = - lane_change_utils::getAbortPaths(planner_data_, path, ego_pose_before_collision, shift_line); + const auto abort_path = lane_change_utils::getAbortPaths( + planner_data_, path, ego_pose_before_collision, common_parameters, *parameters_); abort_non_collision_pose_ = ego_pose_before_collision; if (abort_path) { if (isWaitingApproval()) { abort_path_ = std::make_shared(*abort_path); } + return true; } + + current_lane_change_state_ = LaneChangeStates::Stop; return true; } return false; } -bool LaneChangeModule::isAbort() const +bool LaneChangeModule::isAbortState() const { - return (current_lane_change_state_ == LaneChangeStates::Abort); + return (current_lane_change_state_ == LaneChangeStates::Abort) && abort_path_; } -bool LaneChangeModule::isCancel() const +bool LaneChangeModule::isCancelState() const { return current_lane_change_state_ == LaneChangeStates::Cancel; } @@ -715,7 +717,7 @@ void LaneChangeModule::resetParameters() { is_abort_path_approved_ = false; is_abort_approval_requested_ = false; - current_lane_change_state_ = LaneChangeStates::Trying; + current_lane_change_state_ = LaneChangeStates::Normal; abort_path_.reset(); clearWaitingApproval(); 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 8fa2c17b9ffd6..0e0eb6ac3dd97 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 @@ -771,7 +771,9 @@ std::vector generateDrivableLanes( std::optional getAbortPaths( const std::shared_ptr & planner_data, const LaneChangePath & selected_path, - [[maybe_unused]] const Pose & ego_pose_before_collision, ShiftLine & shift) + [[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); @@ -796,45 +798,28 @@ std::optional getAbortPaths( resampled_selected_path.points, selected_path.shift_line.end, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - [[maybe_unused]] const auto pose_idx_min = - [&](const double accel, const double jerk, const double param_time, const double min_dist) { - if (ego_pose_idx > lane_changing_end_pose_idx) { - return ego_pose_idx; - } - const double turning_point_dist = - std::min(std::invoke(abort_point_dist, accel, jerk, param_time), min_dist); - const auto & points = resampled_selected_path.points; - double sum{0.0}; - size_t idx{0}; - for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { - sum += tier4_autoware_utils::calcDistance2d(points.at(idx), points.at(idx + 1)); - if (sum > turning_point_dist) { - break; - } + const auto pose_idx_min = [&]( + const double accel, const double jerk, const double param_time, + const double min_dist, const double max_dist) { + if (ego_pose_idx > lane_changing_end_pose_idx) { + return ego_pose_idx; + } + const double turning_point_dist = + std::clamp(std::invoke(abort_point_dist, accel, jerk, param_time), min_dist, max_dist); + const auto & points = resampled_selected_path.points; + double sum{0.0}; + size_t idx{0}; + for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { + sum += tier4_autoware_utils::calcDistance2d(points.at(idx), points.at(idx + 1)); + if (sum > turning_point_dist) { + break; } - return idx; - }; + } + return idx; + }; - [[maybe_unused]] const auto pose_idx_max = - [&](const double accel, const double jerk, const double param_time, const double min_dist) { - if (ego_pose_idx > lane_changing_end_pose_idx) { - return ego_pose_idx; - } - const double turning_point_dist = - std::max(std::invoke(abort_point_dist, accel, jerk, param_time), min_dist); - const auto & points = resampled_selected_path.points; - double sum{0.0}; - size_t idx{0}; - for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { - sum += tier4_autoware_utils::calcDistance2d(points.at(idx), points.at(idx + 1)); - if (sum > turning_point_dist) { - break; - } - } - return idx; - }; - const auto abort_start_idx = pose_idx_min(0.0, 0.5, 3.0, 6.0); - const auto abort_end_idx = pose_idx_max(0.0, 0.5, 6.0, 12.0); + const auto abort_start_idx = pose_idx_min(0.0, 0.5, 3.0, 4.0, 6.0); + const auto abort_end_idx = pose_idx_min(0.0, 0.5, 6.0, 12.0, 16.0); if (abort_start_idx >= abort_end_idx) { return std::nullopt; } @@ -844,15 +829,17 @@ std::optional getAbortPaths( const auto abort_end_pose = resampled_selected_path.points.at(abort_end_idx).point.pose; const auto arc_position = lanelet::utils::getArcCoordinates(reference_lanelets, abort_end_pose); const PathWithLaneId reference_lane_segment = std::invoke([&]() { - constexpr double minimum_lane_change_length{17.0}; + const double minimum_lane_change_length = + common_param.backward_length_buffer_for_end_of_lane + common_param.minimum_lane_change_length; - double s_start = arc_position.length; - double s_end = + const double s_start = arc_position.length; + const double s_end = lanelet::utils::getLaneletLength2d(reference_lanelets) - 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, 5.6f); + 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; }); @@ -862,7 +849,6 @@ std::optional getAbortPaths( shift_line.end_shift_length = -arc_position.distance; shift_line.start_idx = abort_start_idx; shift_line.end_idx = abort_end_idx; - shift = shift_line; PathShifter path_shifter; path_shifter.setPath(resampled_selected_path); @@ -888,7 +874,6 @@ std::optional getAbortPaths( auto abort_path = selected_path; abort_path.shifted_path = shifted_path; abort_path.path = start_to_abort_end_pose; - abort_path.prev_path = resampled_selected_path; abort_path.shift_line = shift_line; return std::optional{abort_path}; } From 87712a25db620b2d9f4cedb2cedc988f7e95fb3a Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 28 Nov 2022 14:38:24 +0900 Subject: [PATCH 04/27] update steering factor (accidentally removed) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/lane_change_module.cpp | 1 + 1 file changed, 1 insertion(+) 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 96c5f3d53b6eb..ba9a0a5f6425c 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 @@ -171,6 +171,7 @@ BehaviorModuleOutput LaneChangeModule::plan() if (!isSafe()) { current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop } + updateSteeringFactorPtr(output); return output; } From 741444f5910471c982e7c011b9b793f8c39aaab9 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 28 Nov 2022 15:09:03 +0900 Subject: [PATCH 05/27] include is_abort_condition_satisfied_ flag Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 1 + .../lane_change/lane_change_module.cpp | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) 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 005a39ea27faa..e8a037be00f5e 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 @@ -132,6 +132,7 @@ class LaneChangeModule : public SceneModuleInterface bool is_abort_path_approved_ = false; bool is_abort_approval_requested_ = false; + bool is_abort_condition_satisfied_ = false; bool is_activated_ = false; 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 ba9a0a5f6425c..a2442ae9ae233 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 @@ -155,6 +155,9 @@ BehaviorModuleOutput LaneChangeModule::plan() } generateExtendedDrivableArea(path); prev_approved_path_ = path; + if(is_abort_condition_satisfied_){ + const auto stop_point = util::insertStopPoint(0.1, &path); + } } else { resetPathIfAbort(selected_path); path = util::resamplePathWithSpline(selected_path, resample_interval); @@ -478,8 +481,8 @@ bool LaneChangeModule::isAbortConditionSatisfied() 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; // check abort enable flag if (!parameters_->enable_cancel_lane_change) { @@ -530,12 +533,16 @@ bool LaneChangeModule::isAbortConditionSatisfied() // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { - // check vehicle velocity thresh + is_abort_condition_satisfied_ = true; + current_lane_change_state_ = LaneChangeStates::Cancel; + 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) { return true; } @@ -559,10 +566,12 @@ bool LaneChangeModule::isAbortConditionSatisfied() 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; const auto abort_path = lane_change_utils::getAbortPaths( planner_data_, path, ego_pose_before_collision, common_parameters, *parameters_); + abort_non_collision_pose_ = ego_pose_before_collision; if (abort_path) { @@ -573,6 +582,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() } current_lane_change_state_ = LaneChangeStates::Stop; + return true; } From a725cb3650d19d88627d19ed6562d5ab78ed4793 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 28 Nov 2022 15:45:14 +0900 Subject: [PATCH 06/27] use only check ego in current lane Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.cpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) 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 a2442ae9ae233..1b24cbb82b984 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 @@ -155,7 +155,7 @@ BehaviorModuleOutput LaneChangeModule::plan() } generateExtendedDrivableArea(path); prev_approved_path_ = path; - if(is_abort_condition_satisfied_){ + if (is_abort_condition_satisfied_ && (isNearEndOfLane() && isCurrentSpeedLow())) { const auto stop_point = util::insertStopPoint(0.1, &path); } } else { @@ -537,26 +537,27 @@ bool LaneChangeModule::isAbortConditionSatisfied() current_lane_change_state_ = LaneChangeStates::Cancel; - const bool is_velocity_low = - util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_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) { + if (is_within_original_lane) { return true; } - const bool is_distance_small = - lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, *parameters_); + // const bool is_distance_small = + // lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, + // *parameters_); // check angle thresh from original lane - const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( - closest_lanelet, current_pose, *parameters_); + // const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( + // closest_lanelet, current_pose, *parameters_); - if (is_distance_small && is_angle_diff_small) { - return true; - } + // if (is_angle_diff_small) { + // return true; + // } if (!parameters_->enable_abort_lane_change) { return true; From 0f9dcfbec06cb5b4f0668214be8f864aebef66fb Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 28 Nov 2022 15:47:11 +0900 Subject: [PATCH 07/27] Revert "use only check ego in current lane" This reverts commit 4f97408ed41f22d97012ee0bc1918570b8fbdea7. --- .../lane_change/lane_change_module.cpp | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) 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 1b24cbb82b984..a2442ae9ae233 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 @@ -155,7 +155,7 @@ BehaviorModuleOutput LaneChangeModule::plan() } generateExtendedDrivableArea(path); prev_approved_path_ = path; - if (is_abort_condition_satisfied_ && (isNearEndOfLane() && isCurrentSpeedLow())) { + if(is_abort_condition_satisfied_){ const auto stop_point = util::insertStopPoint(0.1, &path); } } else { @@ -537,27 +537,26 @@ bool LaneChangeModule::isAbortConditionSatisfied() current_lane_change_state_ = LaneChangeStates::Cancel; - // const bool is_velocity_low = - // util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_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_within_original_lane) { + if (is_velocity_low && is_within_original_lane) { return true; } - // const bool is_distance_small = - // lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, - // *parameters_); + const bool is_distance_small = + lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, *parameters_); // check angle thresh from original lane - // const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( - // closest_lanelet, current_pose, *parameters_); + const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( + closest_lanelet, current_pose, *parameters_); - // if (is_angle_diff_small) { - // return true; - // } + if (is_distance_small && is_angle_diff_small) { + return true; + } if (!parameters_->enable_abort_lane_change) { return true; From ff89efe3d2d144e5edbae08caea3d379fb1b7ab6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 28 Nov 2022 06:12:19 +0000 Subject: [PATCH 08/27] ci(pre-commit): autofix --- .../src/scene_module/lane_change/lane_change_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 a2442ae9ae233..17f3b146ab3db 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 @@ -155,7 +155,7 @@ BehaviorModuleOutput LaneChangeModule::plan() } generateExtendedDrivableArea(path); prev_approved_path_ = path; - if(is_abort_condition_satisfied_){ + if (is_abort_condition_satisfied_) { const auto stop_point = util::insertStopPoint(0.1, &path); } } else { From c289717206f52e288c6bcc5dfb2932bdca46fda5 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 28 Nov 2022 15:45:14 +0900 Subject: [PATCH 09/27] use only check ego in current lane Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.cpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) 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 17f3b146ab3db..1b24cbb82b984 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 @@ -155,7 +155,7 @@ BehaviorModuleOutput LaneChangeModule::plan() } generateExtendedDrivableArea(path); prev_approved_path_ = path; - if (is_abort_condition_satisfied_) { + if (is_abort_condition_satisfied_ && (isNearEndOfLane() && isCurrentSpeedLow())) { const auto stop_point = util::insertStopPoint(0.1, &path); } } else { @@ -537,26 +537,27 @@ bool LaneChangeModule::isAbortConditionSatisfied() current_lane_change_state_ = LaneChangeStates::Cancel; - const bool is_velocity_low = - util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_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) { + if (is_within_original_lane) { return true; } - const bool is_distance_small = - lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, *parameters_); + // const bool is_distance_small = + // lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, + // *parameters_); // check angle thresh from original lane - const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( - closest_lanelet, current_pose, *parameters_); + // const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( + // closest_lanelet, current_pose, *parameters_); - if (is_distance_small && is_angle_diff_small) { - return true; - } + // if (is_angle_diff_small) { + // return true; + // } if (!parameters_->enable_abort_lane_change) { return true; From d27ee215eb21f4941460dc0053eb15f07e93ef35 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 28 Nov 2022 17:15:45 +0900 Subject: [PATCH 10/27] improve isAbortConditionSatisfied by using ego polygon check Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 5 +- .../lane_change/lane_change_module.cpp | 87 +++++++++---------- 2 files changed, 44 insertions(+), 48 deletions(-) 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 e8a037be00f5e..72f9a48d54f03 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 @@ -244,6 +244,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; @@ -253,9 +254,9 @@ class LaneChangeModule : public SceneModuleInterface bool isCurrentSpeedLow() const; bool isAbortConditionSatisfied(); bool hasFinishedLaneChange() const; - bool isCancelState() const; - bool isAbortState() const; bool isStopState() const; + bool isAbortState() const; + // getter Pose getEgoPose() const; Twist getEgoTwist() const; 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 1b24cbb82b984..b44d9ecf452b5 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 @@ -155,7 +155,9 @@ BehaviorModuleOutput LaneChangeModule::plan() } generateExtendedDrivableArea(path); prev_approved_path_ = path; - if (is_abort_condition_satisfied_ && (isNearEndOfLane() && isCurrentSpeedLow())) { + if ( + (is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow()) || + isStopState()) { const auto stop_point = util::insertStopPoint(0.1, &path); } } else { @@ -476,12 +478,7 @@ bool LaneChangeModule::isCurrentSpeedLow() 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; // check abort enable flag @@ -496,40 +493,10 @@ bool LaneChangeModule::isAbortConditionSatisfied() } // 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; - } + Pose ego_pose_before_collision; + const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - const auto path = status_.lane_change_path; // check if lane change path is still safe - Pose ego_pose_before_collision; - const bool is_path_safe = - std::invoke([this, &route_handler, &dynamic_objects, &path, ¤t_lanes, ¤t_pose, - ¤t_twist, &common_parameters, &ego_pose_before_collision]() { - 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); - }); // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { @@ -537,11 +504,8 @@ bool LaneChangeModule::isAbortConditionSatisfied() current_lane_change_state_ = LaneChangeStates::Cancel; - // 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); + const bool is_within_original_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), common_parameters); if (is_within_original_lane) { return true; @@ -571,7 +535,8 @@ bool LaneChangeModule::isAbortConditionSatisfied() current_lane_change_state_ = LaneChangeStates::Abort; const auto abort_path = lane_change_utils::getAbortPaths( - planner_data_, path, ego_pose_before_collision, common_parameters, *parameters_); + planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, + *parameters_); abort_non_collision_pose_ = ego_pose_before_collision; @@ -594,9 +559,9 @@ bool LaneChangeModule::isAbortState() const return (current_lane_change_state_ == LaneChangeStates::Abort) && abort_path_; } -bool LaneChangeModule::isCancelState() const +bool LaneChangeModule::isStopState() const { - return current_lane_change_state_ == LaneChangeStates::Cancel; + return current_lane_change_state_ == LaneChangeStates::Stop; } bool LaneChangeModule::hasFinishedLaneChange() const @@ -714,6 +679,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( From 5eb090d24826677171993d30ead3cd8c10904bf9 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 29 Nov 2022 00:39:16 +0900 Subject: [PATCH 11/27] add lateral jerk and path doesn't keep on updating anymore Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.cpp | 36 ++++++------------- .../src/scene_module/lane_change/util.cpp | 13 ++++--- 2 files changed, 19 insertions(+), 30 deletions(-) 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 b44d9ecf452b5..ad66bc0f83d9b 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 @@ -147,6 +147,7 @@ BehaviorModuleOutput LaneChangeModule::plan() toStr(current_lane_change_state_).data()); PathWithLaneId selected_path = status_.lane_change_path.path; PathWithLaneId path; + if (!isAbortState()) { path = util::resamplePathWithSpline(selected_path, resample_interval); if (!isValidPath(path)) { @@ -164,9 +165,6 @@ BehaviorModuleOutput LaneChangeModule::plan() resetPathIfAbort(selected_path); path = util::resamplePathWithSpline(selected_path, resample_interval); generateExtendedDrivableArea(path); - if (is_abort_path_approved_) { - clearWaitingApproval(); - } } BehaviorModuleOutput output; @@ -202,6 +200,7 @@ void LaneChangeModule::resetPathIfAbort(PathWithLaneId & selected_path) RCLCPP_INFO(getLogger(), "[plan] isActivated() is true. set is_abort_path_approved to true."); selected_path = abort_path_->path; is_abort_path_approved_ = true; + clearWaitingApproval(); } else { RCLCPP_INFO(getLogger(), "[plan] isActivated() is False."); is_abort_path_approved_ = false; @@ -256,6 +255,7 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval() path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus(candidate); waitApproval(); + is_abort_path_approved_ = false; return out; } @@ -481,7 +481,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const auto & common_parameters = planner_data_->parameters; is_abort_condition_satisfied_ = false; - // check abort enable flag + // check cancel enable flag if (!parameters_->enable_cancel_lane_change) { current_lane_change_state_ = LaneChangeStates::Normal; return false; @@ -492,16 +492,11 @@ bool LaneChangeModule::isAbortConditionSatisfied() return false; } - // find closest lanelet in original lane + // check if lane change path is still safe Pose ego_pose_before_collision; const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - // check if lane change path is still safe - - // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { - is_abort_condition_satisfied_ = true; - current_lane_change_state_ = LaneChangeStates::Cancel; const bool is_within_original_lane = lane_change_utils::isEgoWithinOriginalLane( @@ -511,18 +506,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() return true; } - // const bool is_distance_small = - // lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, - // *parameters_); - - // check angle thresh from original lane - // const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( - // closest_lanelet, current_pose, *parameters_); - - // if (is_angle_diff_small) { - // return true; - // } - + // check abort enable flag if (!parameters_->enable_abort_lane_change) { return true; } @@ -534,15 +518,15 @@ bool LaneChangeModule::isAbortConditionSatisfied() current_lane_change_state_ = LaneChangeStates::Abort; - const auto abort_path = lane_change_utils::getAbortPaths( + const auto found_abort_path = lane_change_utils::getAbortPaths( planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, *parameters_); abort_non_collision_pose_ = ego_pose_before_collision; - if (abort_path) { - if (isWaitingApproval()) { - abort_path_ = std::make_shared(*abort_path); + if (found_abort_path) { + if (!is_abort_path_approved_) { + abort_path_ = std::make_shared(*found_abort_path); } return true; } 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 0e0eb6ac3dd97..2e8abe8cc1bf5 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 @@ -800,11 +800,12 @@ std::optional getAbortPaths( const auto pose_idx_min = [&]( const double accel, const double jerk, const double param_time, - const double min_dist, const double max_dist) { + const double min_dist, const double max_dist, + double & turning_point_dist) { if (ego_pose_idx > lane_changing_end_pose_idx) { return ego_pose_idx; } - const double turning_point_dist = + turning_point_dist = std::clamp(std::invoke(abort_point_dist, accel, jerk, param_time), min_dist, max_dist); const auto & points = resampled_selected_path.points; double sum{0.0}; @@ -818,8 +819,10 @@ std::optional getAbortPaths( return idx; }; - const auto abort_start_idx = pose_idx_min(0.0, 0.5, 3.0, 4.0, 6.0); - const auto abort_end_idx = pose_idx_min(0.0, 0.5, 6.0, 12.0, 16.0); + double abort_start_dist{0.0}; + const auto abort_start_idx = pose_idx_min(0.0, 0.5, 3.0, 4.0, 6.0, abort_start_dist); + double abort_end_dist{0.0}; + const auto abort_end_idx = pose_idx_min(0.0, 0.5, 6.0, 12.0, 16.0, abort_end_dist); if (abort_start_idx >= abort_end_idx) { return std::nullopt; } @@ -853,6 +856,8 @@ std::optional getAbortPaths( PathShifter path_shifter; path_shifter.setPath(resampled_selected_path); path_shifter.addShiftLine(shift_line); + const auto lateral_jerk = path_shifter.calcJerkFromLatLonDistance( + shift_line.end_shift_length, abort_start_dist, current_speed); ShiftedPath shifted_path; // offset front side From 1746b44a0a0ce0bfca11d78e93bb457a94c2d866 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 29 Nov 2022 11:57:21 +0900 Subject: [PATCH 12/27] parameterized all abort related values Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 32 +++++++++-- .../lane_change/lane_change_module_data.hpp | 36 ++++++++++--- .../src/behavior_path_planner_node.cpp | 54 ++++++++++++++++--- .../src/scene_module/lane_change/util.cpp | 33 ++++++++++-- 4 files changed, 132 insertions(+), 23 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..36e8b87355afb 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,37 @@ maximum_deceleration: 1.0 # [m/s2] lane_change_sampling_num: 10 + # collision check + enable_collision_check_at_prepare_phase: false + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] + use_predicted_path_outside_lanelet: true + use_all_predicted_path: true + + # abort + enable_cancel_lane_change: true + enable_abort_lane_change: true + abort_lane_change_velocity_thresh: 0.5 abort_lane_change_angle_thresh: 10.0 # [deg] abort_lane_change_distance_thresh: 0.3 # [m] 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 + abort_lane_change_velocity_thresh: 0.5 # [m] + abort_lane_change_angle_thresh: 0.174533 # [rad] + abort_lane_change_distance_thresh: 0.3 # [m] + + abort_begin_min_longitudinal_thresh: 4.0 # [m] + abort_begin_max_longitudinal_thresh: 6.0 # [m] + abort_begin_duration: 3.0 # [s] + + abort_return_min_longitudinal_thresh: 12.0 # [m] + abort_return_max_longitudinal_thresh: 16.0 # [m] + abort_return_duration: 6.0 # [s] + + abort_expected_deceleration: 0.1 # [m/s2] + abort_longitudinal_jerk: 0.5 # [m/s3] + + 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_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index 012b0e57b843b..4f39f72d5c597 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,19 +35,40 @@ 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_cancel_lane_change{true}; - bool enable_abort_lane_change{false}; + + // 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_lane_change_velocity_thresh{0.5}; + double abort_lane_change_angle_thresh{0.174533}; + double abort_lane_change_distance_thresh{0.3}; + + double abort_begin_min_longitudinal_thresh{4.0}; + double abort_begin_max_longitudinal_thresh{6.0}; + double abort_begin_duration{3.0}; + + double abort_return_min_longitudinal_thresh{12.0}; + double abort_return_max_longitudinal_thresh{16.0}; + double abort_return_duration{6.0}; + + double abort_expected_deceleration{0.1}; + double abort_longitudinal_jerk{0.5}; + + double abort_max_lateral_jerk{5.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 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 dfa60d7443c5a..70cb4c61ca098 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,20 +392,42 @@ 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); + + // 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); + + // 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_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_cancel_lane_change = dp("enable_cancel_lane_change", true); - p.enable_abort_lane_change = dp("enable_abort_lane_change", false); - p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); - 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); + + p.abort_begin_min_longitudinal_thresh = dp("abort_begin_min_longitudinal_thresh", 4.0); + p.abort_begin_max_longitudinal_thresh = dp("abort_begin_max_longitudinal_thresh", 6.0); + p.abort_begin_duration = dp("abort_begin_duration", 3.0); + + p.abort_return_min_longitudinal_thresh = dp("abort_return_min_longitudinal_thresh", 12.0); + p.abort_return_max_longitudinal_thresh = dp("abort_return_max_longitudinal_thresh", 16.0); + p.abort_return_duration = dp("abort_return_duration", 6.0); + + p.abort_expected_deceleration = dp("abort_expected_deceleration", 0.1); + p.abort_longitudinal_jerk = dp("abort_longitudinal_jerk", 0.5); + + p.abort_max_lateral_jerk = dp("abort_max_lateral_jerk", 5.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( @@ -420,6 +444,22 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() exit(EXIT_FAILURE); } + if (p.abort_expected_deceleration < 0.0) { + RCLCPP_FATAL_STREAM( + get_logger(), "abort_expected_deceleration cannot be negative value. Given parameter: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + if (p.abort_return_duration <= p.abort_begin_duration) { + RCLCPP_FATAL_STREAM( + get_logger(), "abort_return_duration must be more than abort_begin_duration: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + return p; } 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 2e8abe8cc1bf5..5f9f7d06fd526 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 @@ -781,8 +781,8 @@ std::optional getAbortPaths( const auto abort_point_dist = [&](const double param_accel, const double param_jerk, const double param_time) { - return std::max(1.0, current_speed) * param_time + - param_accel * std::pow(param_time, 2) / 2. - param_jerk * std::pow(param_jerk, 3) / 6.; + return current_speed * param_time + (-param_accel) * std::pow(param_time, 2) / 2. - + param_jerk * std::pow(param_jerk, 3) / 6.; }; const auto ego_nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; @@ -819,10 +819,29 @@ std::optional getAbortPaths( return idx; }; + const auto abort_expected_deceleration = lane_change_param.abort_expected_deceleration; + const auto abort_longitudinal_jerk = lane_change_param.abort_longitudinal_jerk; + const auto abort_begin_min_longitudinal_thresh = + lane_change_param.abort_begin_min_longitudinal_thresh; + const auto abort_begin_max_longitudinal_thresh = + lane_change_param.abort_begin_max_longitudinal_thresh; + const auto abort_begin_duration = lane_change_param.abort_begin_duration; + double abort_start_dist{0.0}; - const auto abort_start_idx = pose_idx_min(0.0, 0.5, 3.0, 4.0, 6.0, abort_start_dist); + const auto abort_start_idx = pose_idx_min( + abort_expected_deceleration, abort_longitudinal_jerk, abort_begin_duration, + abort_begin_min_longitudinal_thresh, abort_begin_max_longitudinal_thresh, abort_start_dist); + + const auto abort_return_min_longitudinal_thresh = + lane_change_param.abort_return_min_longitudinal_thresh; + const auto abort_return_max_longitudinal_thresh = + lane_change_param.abort_return_max_longitudinal_thresh; + const auto abort_return_duration = lane_change_param.abort_return_duration; + double abort_end_dist{0.0}; - const auto abort_end_idx = pose_idx_min(0.0, 0.5, 6.0, 12.0, 16.0, abort_end_dist); + const auto abort_end_idx = pose_idx_min( + abort_expected_deceleration, abort_longitudinal_jerk, abort_return_duration, + abort_return_min_longitudinal_thresh, abort_return_max_longitudinal_thresh, abort_end_dist); if (abort_start_idx >= abort_end_idx) { return std::nullopt; } @@ -856,9 +875,13 @@ std::optional getAbortPaths( PathShifter path_shifter; path_shifter.setPath(resampled_selected_path); path_shifter.addShiftLine(shift_line); - const auto lateral_jerk = path_shifter.calcJerkFromLatLonDistance( + 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; From 6a474d550c67e23f482623536d2095372a83f8e4 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 29 Nov 2022 13:46:25 +0900 Subject: [PATCH 13/27] rename abort_end -> abort_return Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) 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 5f9f7d06fd526..21be3a88ec75b 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 @@ -838,18 +838,19 @@ std::optional getAbortPaths( lane_change_param.abort_return_max_longitudinal_thresh; const auto abort_return_duration = lane_change_param.abort_return_duration; - double abort_end_dist{0.0}; - const auto abort_end_idx = pose_idx_min( + double abort_return_dist{0.0}; + const auto abort_return_idx = pose_idx_min( abort_expected_deceleration, abort_longitudinal_jerk, abort_return_duration, - abort_return_min_longitudinal_thresh, abort_return_max_longitudinal_thresh, abort_end_dist); - if (abort_start_idx >= abort_end_idx) { + abort_return_min_longitudinal_thresh, abort_return_max_longitudinal_thresh, abort_return_dist); + if (abort_start_idx >= abort_return_idx) { return std::nullopt; } const auto reference_lanelets = selected_path.reference_lanelets; const auto abort_start_pose = resampled_selected_path.points.at(abort_start_idx).point.pose; - const auto abort_end_pose = resampled_selected_path.points.at(abort_end_idx).point.pose; - const auto arc_position = lanelet::utils::getArcCoordinates(reference_lanelets, abort_end_pose); + const auto abort_return_pose = resampled_selected_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; @@ -867,10 +868,10 @@ std::optional getAbortPaths( ShiftLine shift_line; shift_line.start = abort_start_pose; - shift_line.end = abort_end_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_end_idx; + shift_line.end_idx = abort_return_idx; PathShifter path_shifter; path_shifter.setPath(resampled_selected_path); @@ -891,17 +892,17 @@ std::optional getAbortPaths( "failed to generate shifted path."); } - PathWithLaneId start_to_abort_end_pose; - start_to_abort_end_pose.points.insert( - start_to_abort_end_pose.points.end(), shifted_path.path.points.begin(), - shifted_path.path.points.begin() + abort_end_idx); - start_to_abort_end_pose.points.insert( - start_to_abort_end_pose.points.end(), reference_lane_segment.points.begin(), + 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_end_pose; + abort_path.path = start_to_abort_return_pose; abort_path.shift_line = shift_line; return std::optional{abort_path}; } From 36932748896ed8ba018a007f0e76535c33e16631 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 29 Nov 2022 14:37:05 +0900 Subject: [PATCH 14/27] fix some parameter issue Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/src/scene_module/lane_change/util.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 21be3a88ec75b..5bf398d0396b3 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 @@ -782,7 +782,7 @@ std::optional getAbortPaths( const auto abort_point_dist = [&](const double param_accel, const double param_jerk, const double param_time) { return current_speed * param_time + (-param_accel) * std::pow(param_time, 2) / 2. - - param_jerk * std::pow(param_jerk, 3) / 6.; + param_jerk * std::pow(param_time, 3) / 6.; }; const auto ego_nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; From 050845adfc92068c8b945aeddc02ed789c2597a9 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 29 Nov 2022 15:51:15 +0900 Subject: [PATCH 15/27] check if lane change distance is enough after abort Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/util.hpp | 6 +++ .../src/scene_module/lane_change/util.cpp | 53 +++++++++++++++++-- 2 files changed, 55 insertions(+), 4 deletions(-) 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 bb0c2b03d1e54..89512331ca697 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 @@ -158,6 +158,12 @@ std::optional getAbortPaths( 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, + const LaneChangeParameters & lane_change_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/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 5bf398d0396b3..7b2392cb0d34e 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 @@ -778,6 +778,7 @@ std::optional getAbortPaths( 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 current_lanes = selected_path.reference_lanelets; const auto abort_point_dist = [&](const double param_accel, const double param_jerk, const double param_time) { @@ -805,14 +806,15 @@ std::optional getAbortPaths( if (ego_pose_idx > lane_changing_end_pose_idx) { return ego_pose_idx; } - turning_point_dist = + const auto desired_distance = std::clamp(std::invoke(abort_point_dist, accel, jerk, param_time), min_dist, max_dist); const auto & points = resampled_selected_path.points; - double sum{0.0}; size_t idx{0}; for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { - sum += tier4_autoware_utils::calcDistance2d(points.at(idx), points.at(idx + 1)); - if (sum > turning_point_dist) { + const auto dist_to_ego = + util::getSignedDistance(current_pose, points.at(idx).point.pose, current_lanes); + turning_point_dist = dist_to_ego; + if (dist_to_ego > desired_distance) { break; } } @@ -842,10 +844,17 @@ std::optional getAbortPaths( const auto abort_return_idx = pose_idx_min( abort_expected_deceleration, abort_longitudinal_jerk, abort_return_duration, abort_return_min_longitudinal_thresh, abort_return_max_longitudinal_thresh, abort_return_dist); + if (abort_start_idx >= abort_return_idx) { return std::nullopt; } + if (!hasEnoughDistanceToLaneChangeAfterAbort( + *route_handler, current_lanes, current_pose, abort_return_dist, common_param, + lane_change_param)) { + return std::nullopt; + } + const auto reference_lanelets = selected_path.reference_lanelets; const auto abort_start_pose = resampled_selected_path.points.at(abort_start_idx).point.pose; const auto abort_return_pose = resampled_selected_path.points.at(abort_return_idx).point.pose; @@ -915,4 +924,40 @@ double getLateralShift(const LaneChangePath & path) 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 LaneChangeParameters & lane_change_param) +{ + const auto minimum_lane_change_distance = lane_change_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 From abc3b2af81c9bc77d8b3bbc704a5223c37cfdf3f Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Fri, 2 Dec 2022 14:10:05 +0900 Subject: [PATCH 16/27] improve the code flow of isAbortConditionSatisfied Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 1 - .../lane_change/lane_change_module.cpp | 40 +++++++++---------- 2 files changed, 18 insertions(+), 23 deletions(-) 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 72f9a48d54f03..b04aa5caf2d1b 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 @@ -115,7 +115,6 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - LaneDepartureChecker lane_departure_checker_; mutable LaneChangeStates current_lane_change_state_; mutable std::shared_ptr abort_path_; PathWithLaneId prev_approved_path_; 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 ad66bc0f83d9b..b7119bbdbe832 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 @@ -117,7 +117,7 @@ BT::NodeStatus LaneChangeModule::updateState() } if (isAbortConditionSatisfied()) { - if ((isNearEndOfLane() && isCurrentSpeedLow()) || isAbortState()) { + if ((isNearEndOfLane() && isCurrentSpeedLow()) || isAbortState() || isStopState()) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } @@ -478,27 +478,23 @@ bool LaneChangeModule::isCurrentSpeedLow() const bool LaneChangeModule::isAbortConditionSatisfied() { - const auto & common_parameters = planner_data_->parameters; is_abort_condition_satisfied_ = false; + current_lane_change_state_ = LaneChangeStates::Normal; - // check cancel enable flag if (!parameters_->enable_cancel_lane_change) { - current_lane_change_state_ = LaneChangeStates::Normal; return false; } if (!is_activated_) { - current_lane_change_state_ = LaneChangeStates::Normal; return false; } - // check if lane change path is still safe - Pose ego_pose_before_collision; - const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); + const auto is_path_safe = isApprovedPathSafe(abort_non_collision_pose_); if (!is_path_safe) { current_lane_change_state_ = LaneChangeStates::Cancel; + const auto & common_parameters = planner_data_->parameters; const bool is_within_original_lane = lane_change_utils::isEgoWithinOriginalLane( status_.current_lanes, getEgoPose(), common_parameters); @@ -507,37 +503,37 @@ bool LaneChangeModule::isAbortConditionSatisfied() } // check abort enable flag - if (!parameters_->enable_abort_lane_change) { - 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"); + "DANGER!!! Path is not safe anymore, but it is too late to cancel! Please be cautious"); - current_lane_change_state_ = LaneChangeStates::Abort; + if (!parameters_->enable_abort_lane_change) { + current_lane_change_state_ = LaneChangeStates::Stop; + return true; + } const auto found_abort_path = lane_change_utils::getAbortPaths( - planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, + planner_data_, status_.lane_change_path, abort_non_collision_pose_, common_parameters, *parameters_); - abort_non_collision_pose_ = ego_pose_before_collision; - - if (found_abort_path) { - if (!is_abort_path_approved_) { - abort_path_ = std::make_shared(*found_abort_path); - } + if (!found_abort_path) { + current_lane_change_state_ = LaneChangeStates::Stop; return true; } - current_lane_change_state_ = LaneChangeStates::Stop; + 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 { return (current_lane_change_state_ == LaneChangeStates::Abort) && abort_path_; From 9b8deb47081a9c7538e55222528b725988437995 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Fri, 2 Dec 2022 14:34:01 +0900 Subject: [PATCH 17/27] Place warning message in corresponding states. Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 33 ++++--------------- .../lane_change/lane_change_module.cpp | 25 ++++++++++---- 2 files changed, 25 insertions(+), 33 deletions(-) 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 b04aa5caf2d1b..e908b48c1ff75 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 @@ -47,20 +47,6 @@ using marker_utils::CollisionCheckDebug; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; -inline std::string_view toStr(const LaneChangeStates state) -{ - if (state == LaneChangeStates::Normal) { - return "Normal"; - } else if (state == LaneChangeStates::Cancel) { - return "Cancel"; - } else if (state == LaneChangeStates::Abort) { - return "Abort"; - } else if (state == LaneChangeStates::Stop) { - return "Stop"; - } - return "UNKNOWN"; -} - class LaneChangeModule : public SceneModuleInterface { public: @@ -115,13 +101,16 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - mutable LaneChangeStates current_lane_change_state_; - mutable std::shared_ptr abort_path_; + LaneChangeStates current_lane_change_state_; + std::shared_ptr abort_path_; PathWithLaneId prev_approved_path_; - mutable Pose abort_non_collision_pose_; 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_; @@ -129,12 +118,6 @@ class LaneChangeModule : public SceneModuleInterface UUID uuid_right_; UUID candidate_uuid_; - bool is_abort_path_approved_ = false; - bool is_abort_approval_requested_ = false; - bool is_abort_condition_satisfied_ = false; - - bool is_activated_ = false; - void resetParameters(); void waitApprovalLeft(const double start_distance, const double finish_distance) @@ -263,11 +246,9 @@ class LaneChangeModule : public SceneModuleInterface void resetPathIfAbort(PathWithLaneId & selected_path); // debug - void append_marker_array(const MarkerArray & marker_array) const; + 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/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index b7119bbdbe832..59a449de96ff3 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 @@ -142,9 +142,6 @@ BehaviorModuleOutput LaneChangeModule::plan() is_activated_ = isActivated(); constexpr double resample_interval{1.0}; - RCLCPP_INFO( - getLogger(), "[plan] current_lane_change_state_ = %s", - toStr(current_lane_change_state_).data()); PathWithLaneId selected_path = status_.lane_change_path.path; PathWithLaneId path; @@ -489,7 +486,8 @@ bool LaneChangeModule::isAbortConditionSatisfied() return false; } - const auto is_path_safe = isApprovedPathSafe(abort_non_collision_pose_); + Pose ego_pose_before_collision; + const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); if (!is_path_safe) { current_lane_change_state_ = LaneChangeStates::Cancel; @@ -514,7 +512,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() } const auto found_abort_path = lane_change_utils::getAbortPaths( - planner_data_, status_.lane_change_path, abort_non_collision_pose_, common_parameters, + planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, *parameters_); if (!found_abort_path) { @@ -536,12 +534,25 @@ bool LaneChangeModule::isAbortConditionSatisfied() bool LaneChangeModule::isAbortState() const { - return (current_lane_change_state_ == LaneChangeStates::Abort) && abort_path_; + if ((current_lane_change_state_ == LaneChangeStates::Abort) && abort_path_) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), clock, 1000, + "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); + return true; + } + return false; } bool LaneChangeModule::isStopState() const { - return current_lane_change_state_ == LaneChangeStates::Stop; + if (current_lane_change_state_ == LaneChangeStates::Stop) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), clock, 1000, "DANGER!!! Lane change transition to STOP state!"); + return true; + } + return false; } bool LaneChangeModule::hasFinishedLaneChange() const From b70df2c49e11e618efe9579bff2532069fe02957 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 12 Dec 2022 14:48:47 +0900 Subject: [PATCH 18/27] fix clock and rebase Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/util.hpp | 3 +-- .../lane_change/lane_change_module.cpp | 18 +++++++----------- .../src/scene_module/lane_change/util.cpp | 11 ++++------- 3 files changed, 12 insertions(+), 20 deletions(-) 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 89512331ca697..50297bdfe5ec4 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 @@ -162,8 +162,7 @@ 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, - const LaneChangeParameters & lane_change_param); + 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/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 59a449de96ff3..85f41cef1a854 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 @@ -140,13 +140,12 @@ BehaviorModuleOutput LaneChangeModule::plan() auto path = status_.lane_change_path.path; is_activated_ = isActivated(); - constexpr double resample_interval{1.0}; PathWithLaneId selected_path = status_.lane_change_path.path; PathWithLaneId path; if (!isAbortState()) { - path = util::resamplePathWithSpline(selected_path, resample_interval); + path = selected_path; if (!isValidPath(path)) { status_.is_safe = false; return BehaviorModuleOutput{}; @@ -160,7 +159,7 @@ BehaviorModuleOutput LaneChangeModule::plan() } } else { resetPathIfAbort(selected_path); - path = util::resamplePathWithSpline(selected_path, resample_interval); + path = selected_path; generateExtendedDrivableArea(path); } @@ -501,10 +500,9 @@ bool LaneChangeModule::isAbortConditionSatisfied() } // check abort enable flag - 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 cancel! Please be cautious"); + 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; @@ -515,7 +513,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, *parameters_); - if (!found_abort_path) { + if (!found_abort_path && !is_abort_path_approved_) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -535,9 +533,8 @@ bool LaneChangeModule::isAbortConditionSatisfied() bool LaneChangeModule::isAbortState() const { if ((current_lane_change_state_ == LaneChangeStates::Abort) && abort_path_) { - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), clock, 1000, + getLogger(), *clock_, 1000, "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); return true; } @@ -547,9 +544,8 @@ bool LaneChangeModule::isAbortState() const bool LaneChangeModule::isStopState() const { if (current_lane_change_state_ == LaneChangeStates::Stop) { - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), clock, 1000, "DANGER!!! Lane change transition to STOP state!"); + getLogger(), *clock_, 1000, "DANGER!!! Lane change transition to STOP state!"); return true; } return false; 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 7b2392cb0d34e..27f6b38be8ef3 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 @@ -789,8 +789,7 @@ std::optional getAbortPaths( 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; - constexpr double resample_path{1.0}; - auto resampled_selected_path = util::resamplePathWithSpline(selected_path.path, resample_path); + auto resampled_selected_path = selected_path.path; const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( resampled_selected_path.points, current_pose, ego_nearest_dist_threshold, @@ -850,8 +849,7 @@ std::optional getAbortPaths( } if (!hasEnoughDistanceToLaneChangeAfterAbort( - *route_handler, current_lanes, current_pose, abort_return_dist, common_param, - lane_change_param)) { + *route_handler, current_lanes, current_pose, abort_return_dist, common_param)) { return std::nullopt; } @@ -927,10 +925,9 @@ double getLateralShift(const LaneChangePath & path) 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 LaneChangeParameters & lane_change_param) + const BehaviorPathPlannerParameters & common_param) { - const auto minimum_lane_change_distance = lane_change_param.minimum_lane_change_prepare_distance + + 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; From 247d047ec86fdf993a6976ae7de5108da716d0fa Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 12 Dec 2022 15:41:49 +0900 Subject: [PATCH 19/27] remove accel and jerk parameters Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 22 +++++-------------- 1 file changed, 6 insertions(+), 16 deletions(-) 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 27f6b38be8ef3..df6178745107e 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 @@ -780,12 +780,6 @@ std::optional getAbortPaths( const auto current_pose = planner_data->self_pose->pose; const auto current_lanes = selected_path.reference_lanelets; - const auto abort_point_dist = - [&](const double param_accel, const double param_jerk, const double param_time) { - return current_speed * param_time + (-param_accel) * std::pow(param_time, 2) / 2. - - param_jerk * std::pow(param_time, 3) / 6.; - }; - 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; @@ -799,14 +793,12 @@ std::optional getAbortPaths( ego_nearest_yaw_threshold); const auto pose_idx_min = [&]( - const double accel, const double jerk, const double param_time, - const double min_dist, const double max_dist, + const double param_time, const double min_dist, const double max_dist, double & turning_point_dist) { if (ego_pose_idx > lane_changing_end_pose_idx) { return ego_pose_idx; } - const auto desired_distance = - std::clamp(std::invoke(abort_point_dist, accel, jerk, param_time), min_dist, max_dist); + const auto desired_distance = std::clamp(current_speed * param_time, min_dist, max_dist); const auto & points = resampled_selected_path.points; size_t idx{0}; for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { @@ -820,8 +812,6 @@ std::optional getAbortPaths( return idx; }; - const auto abort_expected_deceleration = lane_change_param.abort_expected_deceleration; - const auto abort_longitudinal_jerk = lane_change_param.abort_longitudinal_jerk; const auto abort_begin_min_longitudinal_thresh = lane_change_param.abort_begin_min_longitudinal_thresh; const auto abort_begin_max_longitudinal_thresh = @@ -830,8 +820,8 @@ std::optional getAbortPaths( double abort_start_dist{0.0}; const auto abort_start_idx = pose_idx_min( - abort_expected_deceleration, abort_longitudinal_jerk, abort_begin_duration, - abort_begin_min_longitudinal_thresh, abort_begin_max_longitudinal_thresh, abort_start_dist); + abort_begin_duration, abort_begin_min_longitudinal_thresh, abort_begin_max_longitudinal_thresh, + abort_start_dist); const auto abort_return_min_longitudinal_thresh = lane_change_param.abort_return_min_longitudinal_thresh; @@ -841,8 +831,8 @@ std::optional getAbortPaths( double abort_return_dist{0.0}; const auto abort_return_idx = pose_idx_min( - abort_expected_deceleration, abort_longitudinal_jerk, abort_return_duration, - abort_return_min_longitudinal_thresh, abort_return_max_longitudinal_thresh, abort_return_dist); + abort_return_duration, abort_return_min_longitudinal_thresh, + abort_return_max_longitudinal_thresh, abort_return_dist); if (abort_start_idx >= abort_return_idx) { return std::nullopt; From d68c2acff1b0e71867f406d1c5f2bc9b8a80904d Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 12 Dec 2022 18:36:30 +0900 Subject: [PATCH 20/27] remove unnecessary parameters Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module_data.hpp | 18 +----- .../scene_module/lane_change/util.hpp | 6 -- .../src/behavior_path_planner_node.cpp | 34 ++--------- .../src/scene_module/lane_change/util.cpp | 56 +++++-------------- 4 files changed, 20 insertions(+), 94 deletions(-) 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 4f39f72d5c597..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 @@ -46,22 +46,8 @@ struct LaneChangeParameters bool enable_cancel_lane_change{true}; bool enable_abort_lane_change{false}; - 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 abort_begin_min_longitudinal_thresh{4.0}; - double abort_begin_max_longitudinal_thresh{6.0}; - double abort_begin_duration{3.0}; - - double abort_return_min_longitudinal_thresh{12.0}; - double abort_return_max_longitudinal_thresh{16.0}; - double abort_return_duration{6.0}; - - double abort_expected_deceleration{0.1}; - double abort_longitudinal_jerk{0.5}; - - double abort_max_lateral_jerk{5.0}; + double abort_delta_time{3.0}; + double abort_max_lateral_jerk{10.0}; // drivable area expansion double drivable_area_right_bound_offset{0.0}; 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 50297bdfe5ec4..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 @@ -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, 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 70cb4c61ca098..3bdde3fd5cf91 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -403,23 +403,8 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.enable_cancel_lane_change = dp("enable_cancel_lane_change", true); p.enable_abort_lane_change = dp("enable_abort_lane_change", false); - 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.abort_begin_min_longitudinal_thresh = dp("abort_begin_min_longitudinal_thresh", 4.0); - p.abort_begin_max_longitudinal_thresh = dp("abort_begin_max_longitudinal_thresh", 6.0); - p.abort_begin_duration = dp("abort_begin_duration", 3.0); - - p.abort_return_min_longitudinal_thresh = dp("abort_return_min_longitudinal_thresh", 12.0); - p.abort_return_max_longitudinal_thresh = dp("abort_return_max_longitudinal_thresh", 16.0); - p.abort_return_duration = dp("abort_return_duration", 6.0); - - p.abort_expected_deceleration = dp("abort_expected_deceleration", 0.1); - p.abort_longitudinal_jerk = dp("abort_longitudinal_jerk", 0.5); - - p.abort_max_lateral_jerk = dp("abort_max_lateral_jerk", 5.0); + 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); @@ -444,19 +429,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() exit(EXIT_FAILURE); } - if (p.abort_expected_deceleration < 0.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "abort_expected_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - - if (p.abort_return_duration <= p.abort_begin_duration) { + if (p.abort_delta_time < 1.0) { RCLCPP_FATAL_STREAM( - get_logger(), "abort_return_duration must be more than abort_begin_duration: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); + get_logger(), "abort_delta_time: " << p.abort_delta_time << ", is too short.\n" + << "Terminating the program..."); exit(EXIT_FAILURE); } 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 df6178745107e..185feccf2a57e 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 @@ -665,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, @@ -789,16 +769,15 @@ std::optional getAbortPaths( resampled_selected_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); const auto lane_changing_end_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - resampled_selected_path.points, selected_path.shift_line.end, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); + resampled_selected_path.points, resampled_selected_path.points.back().point.pose, + ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - const auto pose_idx_min = [&]( - const double param_time, const double min_dist, const double max_dist, - double & turning_point_dist) { + const auto pose_idx_min = [&](const double param_time, double & turning_point_dist) { if (ego_pose_idx > lane_changing_end_pose_idx) { return ego_pose_idx; } - const auto desired_distance = std::clamp(current_speed * param_time, min_dist, max_dist); + + const auto desired_distance = std::max(2.77, current_speed) * param_time; const auto & points = resampled_selected_path.points; size_t idx{0}; for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { @@ -812,34 +791,25 @@ std::optional getAbortPaths( return idx; }; - const auto abort_begin_min_longitudinal_thresh = - lane_change_param.abort_begin_min_longitudinal_thresh; - const auto abort_begin_max_longitudinal_thresh = - lane_change_param.abort_begin_max_longitudinal_thresh; - const auto abort_begin_duration = lane_change_param.abort_begin_duration; + const auto abort_delta_time = lane_change_param.abort_delta_time; double abort_start_dist{0.0}; - const auto abort_start_idx = pose_idx_min( - abort_begin_duration, abort_begin_min_longitudinal_thresh, abort_begin_max_longitudinal_thresh, - abort_start_dist); - - const auto abort_return_min_longitudinal_thresh = - lane_change_param.abort_return_min_longitudinal_thresh; - const auto abort_return_max_longitudinal_thresh = - lane_change_param.abort_return_max_longitudinal_thresh; - const auto abort_return_duration = lane_change_param.abort_return_duration; + const auto abort_start_idx = pose_idx_min(abort_delta_time, abort_start_dist); double abort_return_dist{0.0}; - const auto abort_return_idx = pose_idx_min( - abort_return_duration, abort_return_min_longitudinal_thresh, - abort_return_max_longitudinal_thresh, abort_return_dist); + const auto abort_return_idx = pose_idx_min(abort_delta_time * 2, abort_return_dist); if (abort_start_idx >= abort_return_idx) { + std::cerr << "idx issue, start idx " << abort_start_idx << " return index " << abort_return_idx + << '\n'; + std::cerr << "idx issue, abort_start_dist " << abort_start_dist << " return dist " + << abort_return_dist << '\n'; return std::nullopt; } if (!hasEnoughDistanceToLaneChangeAfterAbort( *route_handler, current_lanes, current_pose, abort_return_dist, common_param)) { + std::cerr << "distance issue\n"; return std::nullopt; } From 595ddfaf21318a4db551d1be17e23b8a4f9e082c Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 12 Dec 2022 19:03:33 +0900 Subject: [PATCH 21/27] fix param file in config Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 23 ++----------------- 1 file changed, 2 insertions(+), 21 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 36e8b87355afb..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 @@ -25,28 +25,9 @@ # abort enable_cancel_lane_change: true - enable_abort_lane_change: true - - abort_lane_change_velocity_thresh: 0.5 - abort_lane_change_angle_thresh: 10.0 # [deg] - abort_lane_change_distance_thresh: 0.3 # [m] - prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] - - abort_lane_change_velocity_thresh: 0.5 # [m] - abort_lane_change_angle_thresh: 0.174533 # [rad] - abort_lane_change_distance_thresh: 0.3 # [m] - - abort_begin_min_longitudinal_thresh: 4.0 # [m] - abort_begin_max_longitudinal_thresh: 6.0 # [m] - abort_begin_duration: 3.0 # [s] - - abort_return_min_longitudinal_thresh: 12.0 # [m] - abort_return_max_longitudinal_thresh: 16.0 # [m] - abort_return_duration: 6.0 # [s] - - abort_expected_deceleration: 0.1 # [m/s2] - abort_longitudinal_jerk: 0.5 # [m/s3] + enable_abort_lane_change: false + abort_delta_time: 3.0 # [s] abort_max_lateral_jerk: 5.0 # [m/s3] # debug From 5eeb5098737ab687d4196c70d1d238ab67a5c65c Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 15 Dec 2022 14:28:32 +0900 Subject: [PATCH 22/27] Update planning/behavior_path_planner/src/scene_module/lane_change/util.cpp Co-authored-by: Takamasa Horibe --- .../src/scene_module/lane_change/util.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 185feccf2a57e..bd3395cb784d3 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 @@ -777,7 +777,8 @@ std::optional getAbortPaths( return ego_pose_idx; } - const auto desired_distance = std::max(2.77, current_speed) * param_time; + constexpr auto min_speed = 2.77; + const auto desired_distance = std::max(min_speed, current_speed) * param_time; const auto & points = resampled_selected_path.points; size_t idx{0}; for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { From 477351db25d35c992db03934b9768ee03c4ded57 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 19 Dec 2022 11:06:36 +0900 Subject: [PATCH 23/27] remove isStopState and refactoring Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.cpp | 7 +- .../src/scene_module/lane_change/util.cpp | 65 ++++++++++--------- 2 files changed, 37 insertions(+), 35 deletions(-) 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 85f41cef1a854..50c9f054f2366 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 @@ -117,7 +117,7 @@ BT::NodeStatus LaneChangeModule::updateState() } if (isAbortConditionSatisfied()) { - if ((isNearEndOfLane() && isCurrentSpeedLow()) || isAbortState() || isStopState()) { + if ((isNearEndOfLane() && isCurrentSpeedLow()) || isAbortState()) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } @@ -138,7 +138,6 @@ BehaviorModuleOutput LaneChangeModule::plan() { resetPathCandidate(); - auto path = status_.lane_change_path.path; is_activated_ = isActivated(); PathWithLaneId selected_path = status_.lane_change_path.path; @@ -152,9 +151,7 @@ BehaviorModuleOutput LaneChangeModule::plan() } generateExtendedDrivableArea(path); prev_approved_path_ = path; - if ( - (is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow()) || - isStopState()) { + if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow())) { const auto stop_point = util::insertStopPoint(0.1, &path); } } else { 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 bd3395cb784d3..27270a1145977 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 @@ -758,65 +758,70 @@ std::optional getAbortPaths( 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 current_lanes = selected_path.reference_lanelets; + 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); - auto resampled_selected_path = selected_path.path; + 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( - resampled_selected_path.points, current_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - const auto lane_changing_end_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - resampled_selected_path.points, resampled_selected_path.points.back().point.pose, - ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + lane_changing_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - const auto pose_idx_min = [&](const double param_time, double & turning_point_dist) { + 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 ego_pose_idx; + return std::make_pair(ego_pose_idx, turning_point_dist); } - constexpr auto min_speed = 2.77; + constexpr auto min_speed{2.77}; const auto desired_distance = std::max(min_speed, current_speed) * param_time; - const auto & points = resampled_selected_path.points; + 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, current_lanes); + 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 idx; + return std::make_pair(idx, turning_point_dist); }; const auto abort_delta_time = lane_change_param.abort_delta_time; - - double abort_start_dist{0.0}; - const auto abort_start_idx = pose_idx_min(abort_delta_time, abort_start_dist); - - double abort_return_dist{0.0}; - const auto abort_return_idx = pose_idx_min(abort_delta_time * 2, abort_return_dist); + 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) { - std::cerr << "idx issue, start idx " << abort_start_idx << " return index " << abort_return_idx - << '\n'; - std::cerr << "idx issue, abort_start_dist " << abort_start_dist << " return dist " - << abort_return_dist << '\n'; + 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, current_lanes, current_pose, abort_return_dist, common_param)) { - std::cerr << "distance issue\n"; + *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 reference_lanelets = selected_path.reference_lanelets; - const auto abort_start_pose = resampled_selected_path.points.at(abort_start_idx).point.pose; - const auto abort_return_pose = resampled_selected_path.points.at(abort_return_idx).point.pose; + 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([&]() { @@ -842,7 +847,7 @@ std::optional getAbortPaths( shift_line.end_idx = abort_return_idx; PathShifter path_shifter; - path_shifter.setPath(resampled_selected_path); + 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); @@ -857,7 +862,7 @@ std::optional getAbortPaths( 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 shifted path."); + "failed to generate abort shifted path."); } PathWithLaneId start_to_abort_return_pose; From 4bc49a119ff4822af021305f1d7f53568790ea01 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Wed, 28 Dec 2022 01:16:30 +0900 Subject: [PATCH 24/27] Fixed CANCEL when ego is out of lane Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 41 +------- .../lane_change/lane_change_module.cpp | 95 +++++++++---------- 2 files changed, 48 insertions(+), 88 deletions(-) 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 e908b48c1ff75..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 @@ -134,35 +134,6 @@ class LaneChangeModule : public SceneModuleInterface is_waiting_approval_ = true; } - void updateRegisteredRTCStatus(const LaneChangePath & path) - { - const auto start_distance_to_path_change = motion_utils::calcSignedArcLength( - path.path.points, planner_data_->self_pose->pose.position, path.shift_line.start.position); - - const auto finish_distance_to_path_change = motion_utils::calcSignedArcLength( - path.path.points, planner_data_->self_pose->pose.position, path.shift_line.end.position); - - const auto & start_idx = path.shift_line.start_idx; - const auto & end_idx = path.shift_line.end_idx; - const auto lateral_shift = - path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); - - if (lateral_shift > 0.0) { - rtc_interface_left_.updateCooperateStatus( - uuid_left_, isExecutionReady(), start_distance_to_path_change, - finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_left_; - } else { - rtc_interface_right_.updateCooperateStatus( - uuid_right_, isExecutionReady(), start_distance_to_path_change, - finish_distance_to_path_change, clock_->now()); - candidate_uuid_ = uuid_right_; - } - - RCLCPP_WARN_STREAM( - getLogger(), "Direction is UNKNOWN, start_distance = " << start_distance_to_path_change); - } - void updateRTCStatus(const CandidateOutput & candidate) { if (candidate.lateral_shift > 0.0) { @@ -191,15 +162,6 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_right_.clearCooperateStatus(); } - void removeCandidateRTCStatus() - { - if (rtc_interface_left_.isRegistered(candidate_uuid_)) { - rtc_interface_left_.removeCooperateStatus(candidate_uuid_); - } else if (rtc_interface_right_.isRegistered(candidate_uuid_)) { - rtc_interface_right_.removeCooperateStatus(candidate_uuid_); - } - } - void removePreviousRTCStatusLeft() { if (rtc_interface_left_.isRegistered(uuid_left_)) { @@ -236,14 +198,13 @@ class LaneChangeModule : public SceneModuleInterface bool isCurrentSpeedLow() const; bool isAbortConditionSatisfied(); bool hasFinishedLaneChange() const; - bool isStopState() const; bool isAbortState() const; // getter Pose getEgoPose() const; Twist getEgoTwist() const; std_msgs::msg::Header getRouteHeader() const; - void resetPathIfAbort(PathWithLaneId & selected_path); + void resetPathIfAbort(); // debug void setObjectDebugVisualization() const; 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 50c9f054f2366..1a19311e2fb2e 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 @@ -117,7 +117,9 @@ BT::NodeStatus LaneChangeModule::updateState() } if (isAbortConditionSatisfied()) { - if ((isNearEndOfLane() && isCurrentSpeedLow()) || isAbortState()) { + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + if ((isNearEndOfLane() && isCurrentSpeedLow()) || !is_within_current_lane || isAbortState()) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } @@ -137,42 +139,41 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { resetPathCandidate(); - is_activated_ = isActivated(); - PathWithLaneId selected_path = status_.lane_change_path.path; - PathWithLaneId path; + PathWithLaneId path = status_.lane_change_path.path; + if (!isValidPath(path)) { + status_.is_safe = false; + return BehaviorModuleOutput{}; + } - if (!isAbortState()) { - path = selected_path; - if (!isValidPath(path)) { - status_.is_safe = false; - return BehaviorModuleOutput{}; + 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); + } + + generateExtendedDrivableArea(path); + + if (!isAbortState()) { prev_approved_path_ = path; - if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow())) { - const auto stop_point = util::insertStopPoint(0.1, &path); - } - } else { - resetPathIfAbort(selected_path); - path = selected_path; - generateExtendedDrivableArea(path); } BehaviorModuleOutput output; output.path = std::make_shared(path); updateOutputTurnSignal(output); - if (!isSafe()) { - current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop - } updateSteeringFactorPtr(output); return output; } -void LaneChangeModule::resetPathIfAbort(PathWithLaneId & selected_path) +void LaneChangeModule::resetPathIfAbort() { if (!is_abort_approval_requested_) { const auto lateral_shift = lane_change_utils::getLateralShift(*abort_path_); @@ -183,15 +184,14 @@ void LaneChangeModule::resetPathIfAbort(PathWithLaneId & selected_path) removePreviousRTCStatusLeft(); uuid_left_ = generateUUID(); } + RCLCPP_ERROR(getLogger(), "[plan] uuid is reset to request abort approval."); is_abort_approval_requested_ = true; is_abort_path_approved_ = false; - RCLCPP_ERROR(getLogger(), "[plan] uuid is reset to request abort approval."); return; } if (isActivated()) { RCLCPP_INFO(getLogger(), "[plan] isActivated() is true. set is_abort_path_approved to true."); - selected_path = abort_path_->path; is_abort_path_approved_ = true; clearWaitingApproval(); } else { @@ -235,15 +235,16 @@ CandidateOutput LaneChangeModule::planCandidate() const BehaviorModuleOutput LaneChangeModule::planWaitingApproval() { - BehaviorModuleOutput out; - if (!isAbortState()) { - const auto lane_keeping_path = getReferencePath(); - prev_approved_path_ = lane_keeping_path; - out.path = std::make_shared(lane_keeping_path); - } else { - out.path = std::make_shared(prev_approved_path_); + 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(prev_approved_path_); + const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus(candidate); @@ -486,13 +487,12 @@ bool LaneChangeModule::isAbortConditionSatisfied() const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); if (!is_path_safe) { - current_lane_change_state_ = LaneChangeStates::Cancel; - 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; } @@ -503,7 +503,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() if (!parameters_->enable_abort_lane_change) { current_lane_change_state_ = LaneChangeStates::Stop; - return true; + return false; } const auto found_abort_path = lane_change_utils::getAbortPaths( @@ -529,23 +529,22 @@ bool LaneChangeModule::isAbortConditionSatisfied() bool LaneChangeModule::isAbortState() const { - if ((current_lane_change_state_ == LaneChangeStates::Abort) && abort_path_) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); - return true; + if (!parameters_->enable_abort_lane_change) { + return false; } - return false; -} -bool LaneChangeModule::isStopState() const -{ - if (current_lane_change_state_ == LaneChangeStates::Stop) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, "DANGER!!! Lane change transition to STOP state!"); - return true; + if (current_lane_change_state_ != LaneChangeStates::Abort) { + return false; } - 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 @@ -709,7 +708,7 @@ void LaneChangeModule::resetParameters() is_abort_path_approved_ = false; is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; - abort_path_.reset(); + abort_path_ = nullptr; clearWaitingApproval(); removeRTCStatus(); From 0103416496d35eaa9349836b0630a9f76ed78649 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Wed, 28 Dec 2022 01:41:11 +0900 Subject: [PATCH 25/27] fix path reset during abort Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) 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 1a19311e2fb2e..701c3356e2b2e 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 @@ -116,10 +116,15 @@ 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()) { - const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if ((isNearEndOfLane() && isCurrentSpeedLow()) || !is_within_current_lane || isAbortState()) { + if ((isNearEndOfLane() && isCurrentSpeedLow()) || !is_within_current_lane) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } @@ -160,9 +165,7 @@ BehaviorModuleOutput LaneChangeModule::plan() generateExtendedDrivableArea(path); - if (!isAbortState()) { - prev_approved_path_ = path; - } + prev_approved_path_ = path; BehaviorModuleOutput output; output.path = std::make_shared(path); From 4d1413294601d5f96d0abb603803ea293454a981 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Wed, 28 Dec 2022 01:50:08 +0900 Subject: [PATCH 26/27] fix abort path exceed goal Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 27270a1145977..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 @@ -829,9 +829,14 @@ std::optional getAbortPaths( common_param.backward_length_buffer_for_end_of_lane + common_param.minimum_lane_change_length; const double s_start = arc_position.length; - const double s_end = + 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, From 0ee8fde06c4cb36900467b71d00662f15a399751 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Wed, 28 Dec 2022 11:17:16 +0900 Subject: [PATCH 27/27] fix logger to debug Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/lane_change_module.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 701c3356e2b2e..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 @@ -187,18 +187,18 @@ void LaneChangeModule::resetPathIfAbort() removePreviousRTCStatusLeft(); uuid_left_ = generateUUID(); } - RCLCPP_ERROR(getLogger(), "[plan] uuid is reset to request abort approval."); + 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_INFO(getLogger(), "[plan] isActivated() is true. set is_abort_path_approved to true."); + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); is_abort_path_approved_ = true; clearWaitingApproval(); } else { - RCLCPP_INFO(getLogger(), "[plan] isActivated() is False."); + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); is_abort_path_approved_ = false; waitApproval(); }