From 9b10ca9f3524dea8b96850e6a767a7aab16163e2 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 13 Oct 2023 11:38:47 +0900 Subject: [PATCH] feat(avoidance): add feature to guard unfeasible path Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 1 + .../utils/avoidance/avoidance_module_data.hpp | 11 ++++ .../avoidance/avoidance_module.cpp | 63 +++++++++++++++++++ .../src/scene_module/avoidance/manager.cpp | 2 + 4 files changed, 77 insertions(+) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 589233cf5a441..733f2d69cdb89 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -165,6 +165,7 @@ hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 + max_deviation_from_lane: 0.5 # [m] # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 3264ec1e9ddfd..07b4e831efe5b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -239,6 +239,9 @@ struct AvoidanceParameters // Even if the obstacle is very large, it will not avoid more than this length for left direction double max_left_shift_length; + // Validate vehicle departure from driving lane. + double max_deviation_from_lane{0.0}; + // To prevent large acceleration while avoidance. double max_lateral_acceleration; @@ -476,8 +479,16 @@ struct AvoidancePlanningData // safe shift point AvoidLineArray safe_new_sl{}; + std::vector drivable_lanes{}; + + lanelet::BasicLineString3d right_bound{}; + + lanelet::BasicLineString3d left_bound{}; + bool safe{false}; + bool valid{false}; + bool comfortable{false}; bool avoid_required{false}; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 060ad4d462744..f0ffd1648f0f3 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -85,6 +85,14 @@ bool isBestEffort(const std::string & policy) { return policy == "best_effort"; } + +lanelet::BasicLineString3d toLineString3d(const std::vector & bound) +{ + lanelet::BasicLineString3d ret{}; + std::for_each( + bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); + return ret; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -190,6 +198,23 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( *getPreviousModuleOutput().reference_path, planner_data_); + // expand drivable lanes + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + }); + + // calc drivable bound + const auto shorten_lanes = + utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes); + data.left_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, + parameters_->use_intersection_areas, true)); + data.right_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, + parameters_->use_intersection_areas, false)); + // reference path if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); @@ -394,6 +419,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de const auto new_sp = findNewShiftLine(processed_raw_sp); if (isValidShiftLine(new_sp, path_shifter)) { data.unapproved_new_sl = new_sp; + data.valid = true; } const auto found_new_sl = data.unapproved_new_sl.size() > 0; @@ -431,6 +457,17 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de void AvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { + /** + * TODO(someone): prevent meaningless stop point insertion in other way. + * If the candidate shift line is invalid, manage all objects as unavoidable. + */ + if (!data.valid) { + std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) { + o.is_avoidable = false; + o.reason = "InvalidShiftLine"; + }); + } + /** * Find the nearest object that should be avoid. When the ego follows reference path, * if the both of following two conditions are satisfied, the module surely avoid the object. @@ -2342,6 +2379,32 @@ bool AvoidanceModule::isValidShiftLine( } } + // check if the vehicle is in road. (yaw angle is not considered) + { + const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + + parameters_->hard_road_shoulder_margin - + parameters_->max_deviation_from_lane; + + const size_t start_idx = shift_lines.front().start_idx; + const size_t end_idx = shift_lines.back().end_idx; + + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; + + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound; + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } + } + } + debug_data_.proposed_spline_shift = proposed_shift_path.shift_length; return true; // valid shift line. diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 8900e2b3e827c..b4fae9060c65f 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -186,6 +186,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); + p.max_deviation_from_lane = + getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); } // avoidance maneuver (longitudinal)