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 7ca73ee950536..5f48501f325b5 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 @@ -20,6 +20,7 @@ #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" +#include "lane_departure_checker/lane_departure_checker.hpp" #include @@ -43,6 +44,7 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using lane_departure_checker::LaneDepartureChecker; using marker_utils::CollisionCheckDebug; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -103,6 +105,7 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; + LaneDepartureChecker lane_departure_checker_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; @@ -169,6 +172,7 @@ class LaneChangeModule : public SceneModuleInterface void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; bool isSafe() const; + bool isValidPath(const PathWithLaneId & path) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; bool isAbortConditionSatisfied() const; 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 f50d027c030ce..09ea9630983b4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -404,6 +404,9 @@ std::uint8_t getHighestProbLabel(const std::vector & class lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr & planner_data); +lanelet::ConstLanelets extendLanes( + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); + lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data); @@ -476,6 +479,8 @@ bool isSafeInFreeSpaceCollisionCheck( const double check_start_time, const double check_end_time, const double check_time_resolution, const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, const double front_decel, const double rear_decel, CollisionCheckDebug & debug); + +bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_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 465a4c8ce06d4..4c6cd3a2ca018 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 @@ -50,6 +50,7 @@ LaneChangeModule::LaneChangeModule( uuid_right_{generateUUID()} { steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); + lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); } BehaviorModuleOutput LaneChangeModule::run() @@ -58,6 +59,12 @@ BehaviorModuleOutput LaneChangeModule::run() 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; @@ -73,7 +80,7 @@ void LaneChangeModule::onEntry() void LaneChangeModule::onExit() { resetParameters(); - current_state_ = BT::NodeStatus::IDLE; + current_state_ = BT::NodeStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); } @@ -112,6 +119,11 @@ bool LaneChangeModule::isExecutionReady() const BT::NodeStatus LaneChangeModule::updateState() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState"); + if (!isSafe()) { + current_state_ = BT::NodeStatus::SUCCESS; + return current_state_; + } + if (isAbortConditionSatisfied()) { if (isNearEndOfLane() && isCurrentSpeedLow()) { current_state_ = BT::NodeStatus::RUNNING; @@ -134,6 +146,10 @@ BehaviorModuleOutput LaneChangeModule::plan() { constexpr double resample_interval{1.0}; auto path = util::resamplePathWithSpline(status_.lane_change_path.path, resample_interval); + if (!isValidPath(path)) { + status_.is_safe = false; + return BehaviorModuleOutput{}; + } generateExtendedDrivableArea(path); if (isAbortConditionSatisfied()) { @@ -359,6 +375,32 @@ std::pair LaneChangeModule::getSafePath( bool LaneChangeModule::isSafe() const { return status_.is_safe; } +bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const +{ + const auto & route_handler = planner_data_->route_handler; + + // check lane departure + const auto drivable_lanes = lane_change_utils::generateDrivableLanes( + *route_handler, util::extendLanes(route_handler, status_.current_lanes), + util::extendLanes(route_handler, status_.lane_change_lanes)); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_->drivable_area_left_bound_offset, + parameters_->drivable_area_right_bound_offset); + const auto lanelets = util::transformToLanelets(expanded_lanes); + if (lane_departure_checker_.checkPathWillLeaveLane(lanelets, path)) { + RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); + return false; + } + + // check relative angle + if (!util::checkPathRelativeAngle(path, M_PI)) { + RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path relative angle is invalid"); + return false; + } + + return true; +} + bool LaneChangeModule::isNearEndOfLane() const { const auto & current_pose = getEgoPose(); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 78fccb781e86f..15214a2198ac4 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2189,6 +2189,26 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr common_parameters.forward_path_length); } +lanelet::ConstLanelets extendLanes( + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) +{ + auto extended_lanes = lanes; + + // Add next lane + const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); + if (!next_lanes.empty()) { + extended_lanes.push_back(next_lanes.front()); + } + + // Add previous lane + const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); + if (!prev_lanes.empty()) { + extended_lanes.insert(extended_lanes.begin(), prev_lanes.front()); + } + + return extended_lanes; +} + lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data) { @@ -2206,23 +2226,11 @@ lanelet::ConstLanelets getExtendedCurrentLanes( } // For current_lanes with desired length - auto current_lanes = route_handler->getLaneletSequence( + const auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length); - // Add next lane - const auto next_lanes = route_handler->getNextLanelets(current_lanes.back()); - if (!next_lanes.empty()) { - current_lanes.push_back(next_lanes.front()); - } - - // Add previous lane - const auto prev_lanes = route_handler->getPreviousLanelets(current_lanes.front()); - if (!prev_lanes.empty()) { - current_lanes.insert(current_lanes.begin(), prev_lanes.front()); - } - - return current_lanes; + return extendLanes(route_handler, current_lanes); } lanelet::ConstLanelets calcLaneAroundPose( @@ -2645,4 +2653,50 @@ bool isSafeInFreeSpaceCollisionCheck( } return true; } + +bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold) +{ + // We need at least three points to compute relative angle + constexpr size_t relative_angle_points_num = 3; + if (path.points.size() < relative_angle_points_num) { + return true; + } + + for (size_t p1_id = 0; p1_id <= path.points.size() - relative_angle_points_num; ++p1_id) { + // Get Point1 + const auto & p1 = path.points.at(p1_id).point.pose.position; + + // Get Point2 + const auto & p2 = path.points.at(p1_id + 1).point.pose.position; + + // Get Point3 + const auto & p3 = path.points.at(p1_id + 2).point.pose.position; + + // ignore invert driving direction + if ( + path.points.at(p1_id).point.longitudinal_velocity_mps < 0 || + path.points.at(p1_id + 1).point.longitudinal_velocity_mps < 0 || + path.points.at(p1_id + 2).point.longitudinal_velocity_mps < 0) { + continue; + } + + // convert to p1 coordinate + const double x3 = p3.x - p1.x; + const double x2 = p2.x - p1.x; + const double y3 = p3.y - p1.y; + const double y2 = p2.y - p1.y; + + // calculate relative angle of vector p3 based on p1p2 vector + const double th = std::atan2(y2, x2); + const double th2 = + std::atan2(-x3 * std::sin(th) + y3 * std::cos(th), x3 * std::cos(th) + y3 * std::sin(th)); + if (std::abs(th2) > angle_threshold) { + // invalid angle + return false; + } + } + + return true; +} + } // namespace behavior_path_planner::util