From bb55d7867523efcd9190d516e2da26a9b60607ea Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Sep 2023 09:15:31 +0900 Subject: [PATCH] Revert "feat(avoidance): add feature to guard unfeasible path" This reverts commit 9b10ca9f3524dea8b96850e6a767a7aab16163e2. --- .../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 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 733f2d69cdb89..589233cf5a441 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -165,7 +165,6 @@ 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 07b4e831efe5b..3264ec1e9ddfd 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,9 +239,6 @@ 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; @@ -479,16 +476,8 @@ 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 f0ffd1648f0f3..060ad4d462744 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,14 +85,6 @@ 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( @@ -198,23 +190,6 @@ 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); @@ -419,7 +394,6 @@ 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; @@ -457,17 +431,6 @@ 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. @@ -2379,32 +2342,6 @@ 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 b4fae9060c65f..8900e2b3e827c 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -186,8 +186,6 @@ 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)