From f95e8f220f7274d37766ac7bbb9603676417d514 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 28 Nov 2022 21:10:23 +0900 Subject: [PATCH] fix(behavior_path_planner): check lane departure and relative angle for lane change (#2379) Signed-off-by: kosuke55 Signed-off-by: kosuke55 Signed-off-by: tomoya.kimura --- .../lane_departure_checker.hpp | 5 +- .../lane_departure_checker.cpp | 5 +- .../lane_change/lane_change_module.hpp | 4 + .../behavior_path_planner/utilities.hpp | 5 ++ .../lane_change/lane_change_module.cpp | 39 +++++++++- .../behavior_path_planner/src/utilities.cpp | 76 ++++++++++++++++--- .../include/route_handler/route_handler.hpp | 1 + planning/route_handler/src/route_handler.cpp | 6 ++ 8 files changed, 124 insertions(+), 17 deletions(-) diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 369bc03144efe..0ba143ac988bb 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -102,7 +102,8 @@ class LaneDepartureChecker vehicle_info_ptr_ = std::make_shared(vehicle_info); } - bool checkPathWillLeaveLane(const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path); + bool checkPathWillLeaveLane( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; vehicle_info_util::VehicleInfo vehicle_info_public_; @@ -122,7 +123,7 @@ class LaneDepartureChecker std::vector createVehicleFootprints( const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, const Param & param); - std::vector createVehicleFootprints(const PathWithLaneId & path); + std::vector createVehicleFootprints(const PathWithLaneId & path) const; static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 8cee01856e154..ce36b394ae548 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -145,7 +145,7 @@ Output LaneDepartureChecker::update(const Input & input) } bool LaneDepartureChecker::checkPathWillLeaveLane( - const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const { std::vector vehicle_footprints = createVehicleFootprints(path); lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints); @@ -242,7 +242,8 @@ std::vector LaneDepartureChecker::createVehicleFootprints( return vehicle_footprints; } -std::vector LaneDepartureChecker::createVehicleFootprints(const PathWithLaneId & path) +std::vector LaneDepartureChecker::createVehicleFootprints( + const PathWithLaneId & path) const { // Create vehicle footprint in base_link coordinate const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_); 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 9688f0426a004..8dbbd452e0446 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 @@ -18,6 +18,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/utilities.hpp" +#include "lane_departure_checker/lane_departure_checker.hpp" #include #include @@ -37,6 +38,7 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using lane_departure_checker::LaneDepartureChecker; struct LaneChangeParameters { @@ -125,6 +127,7 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeParameters parameters_; LaneChangeStatus status_; PathShifter path_shifter_; + LaneDepartureChecker lane_departure_checker_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; @@ -185,6 +188,7 @@ class LaneChangeModule : public SceneModuleInterface bool isSafe() const; bool isLaneBlocked(const lanelet::ConstLanelets & lanes) 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 1fa2d5b1edef0..3d8fdf81d13f9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -292,9 +292,14 @@ 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); +bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); + } // namespace util } // 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 892d6952031a1..23fe4baff654b 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 @@ -44,6 +44,7 @@ LaneChangeModule::LaneChangeModule( uuid_left_{generateUUID()}, uuid_right_{generateUUID()} { + lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); } BehaviorModuleOutput LaneChangeModule::run() @@ -52,6 +53,12 @@ BehaviorModuleOutput LaneChangeModule::run() current_state_ = BT::NodeStatus::RUNNING; is_activated_ = isActivated(); const auto output = plan(); + + if (!isSafe()) { + current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop + return output; + } + const auto turn_signal_info = output.turn_signal_info; if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { waitApprovalLeft(turn_signal_info.signal_distance); @@ -77,7 +84,7 @@ void LaneChangeModule::onExit() { clearWaitingApproval(); removeRTCStatus(); - current_state_ = BT::NodeStatus::IDLE; + current_state_ = BT::NodeStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); } @@ -122,6 +129,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; @@ -143,6 +155,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{}; + } // Generate drivable area { const auto common_parameters = planner_data_->parameters; @@ -397,6 +413,27 @@ std::pair LaneChangeModule::getSafePath( bool LaneChangeModule::isSafe() const { return status_.is_safe; } +bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const +{ + // check lane departure + lanelet::ConstLanelets lanes; + lanes.reserve(status_.current_lanes.size() + status_.lane_change_lanes.size()); + lanes.insert(lanes.end(), status_.current_lanes.begin(), status_.current_lanes.end()); + lanes.insert(lanes.end(), status_.lane_change_lanes.begin(), status_.lane_change_lanes.end()); + if (lane_departure_checker_.checkPathWillLeaveLane(lanes, 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 = planner_data_->self_pose->pose; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 6bfea188fe894..4219e0dadb92c 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1934,12 +1934,33 @@ 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) { const auto & route_handler = planner_data->route_handler; const auto current_pose = planner_data->self_pose->pose; const auto common_parameters = planner_data->parameters; + const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { @@ -1950,25 +1971,56 @@ 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_lanes - const auto next_lanes = route_handler->getNextLanelets(current_lanes.back()); - if (!next_lanes.empty()) { - // TODO(kosuke55) which lane should be added? - current_lanes.push_back(next_lanes.front()); + return extendLanes(route_handler, current_lanes); +} + +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; } - // Add prev_lane - lanelet::ConstLanelets prev_lanes; - if (route_handler->getPreviousLaneletsWithinRoute(current_lanes.front(), &prev_lanes)) { - // TODO(kosuke55) which lane should be added? - current_lanes.insert(current_lanes.begin(), 0, prev_lanes.front()); + 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 current_lanes; + return true; } } // namespace util diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 85aaf9c303ea3..82633d771d7f5 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -123,6 +123,7 @@ class RouteHandler */ boost::optional getLeftLanelet( const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; /** diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 607e3c7b590aa..77037d6e66da1 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -771,6 +771,12 @@ bool RouteHandler::getClosestLaneletWithinRoute( return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } +lanelet::ConstLanelets RouteHandler::getPreviousLanelets( + const lanelet::ConstLanelet & lanelet) const +{ + return routing_graph_ptr_->previous(lanelet); +} + bool RouteHandler::getNextLaneletWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const {