From 37573a18bd48f9a0437870659a718c09bf5c27cd Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 22 Sep 2023 10:53:56 +0900 Subject: [PATCH] refactor(avoidance): remove redundant function Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.hpp | 7 ---- .../avoidance/avoidance_module.cpp | 41 +++++++++---------- 2 files changed, 20 insertions(+), 28 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 37ac0125a23ce..cbf40e59535be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -477,13 +477,6 @@ class AvoidanceModule : public SceneModuleInterface */ TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; - // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep - // extending during avoidance module - // TODO(murooka) freespace during turning in intersection where there is no neighbor lanes - // NOTE: Assume that there is no situation where there is an object in the middle lane of more - // than two lanes since which way to avoid is not obvious - void generateExpandDrivableLanes(BehaviorModuleOutput & output) const; - /** * @brief fill debug markers. */ 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 e4d4c67b5bc8a..379d5e546a1c8 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 @@ -1950,25 +1950,6 @@ bool AvoidanceModule::isSafePath( return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } -void AvoidanceModule::generateExpandDrivableLanes(BehaviorModuleOutput & output) const -{ - DrivableAreaInfo current_drivable_area_info; - // generate drivable lanes - current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; - // generate obstacle polygons - current_drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); - // expand hatched road markings - current_drivable_area_info.enable_expanding_hatched_road_markings = - parameters_->use_hatched_road_markings; - // expand intersection areas - current_drivable_area_info.enable_expanding_intersection_areas = - parameters_->use_intersection_areas; - - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); -} - PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const { const auto previous_path = helper_.getPreviousReferencePath(); @@ -2102,8 +2083,26 @@ BehaviorModuleOutput AvoidanceModule::plan() utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); // Drivable area generation. - generateExpandDrivableLanes(output); - setDrivableLanes(output.drivable_area_info.drivable_lanes); + { + DrivableAreaInfo current_drivable_area_info; + // generate drivable lanes + current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; + // generate obstacle polygons + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + // expand hatched road markings + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; + // expand intersection areas + current_drivable_area_info.enable_expanding_intersection_areas = + parameters_->use_intersection_areas; + + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + + setDrivableLanes(output.drivable_area_info.drivable_lanes); + } return output; }