diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index a3b371d746154..baad95096dd91 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -391,6 +391,9 @@ std::vector combineDrivableLanes( const std::vector & original_drivable_lanes_vec, const std::vector & new_drivable_lanes_vec); +DrivableAreaInfo combineDrivableAreaInfo( + const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); + void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); 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 7576709ee0d7a..aa64e2741e52d 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 @@ -2523,15 +2523,19 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output } { // for new architecture + DrivableAreaInfo current_drivable_area_info; // generate drivable lanes - output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, drivable_lanes); + current_drivable_area_info.drivable_lanes = drivable_lanes; // generate obstacle polygons - output.drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); // expand hatched road markings - output.drivable_area_info.enable_expanding_hatched_road_markings = + current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; + + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } { // for old architecture diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index d83b2056e6e91..59cf23562582d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -692,8 +692,11 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*output.path, status_.lanes); - output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } // return to lane parking if it is possible @@ -819,8 +822,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, status_.lanes); - out.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + out.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } return out; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 3d14c52fb475c..19d612f2bc671 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -130,7 +130,14 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) dp.drivable_area_types_to_skip); // for new architecture - output.drivable_area_info.drivable_lanes = expanded_lanes; + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = expanded_lanes; + if (prev_drivable_area_info_) { + output.drivable_area_info = + utils::combineDrivableAreaInfo(current_drivable_area_info, *prev_drivable_area_info_); + } else { + output.drivable_area_info = current_drivable_area_info; + } // for old architecture utils::generateDrivableArea( diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index e3b08f40a6386..e1ad7da1781af 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -179,7 +179,12 @@ BehaviorModuleOutput PullOutModule::plan() // the path of getCurrent() is generated by generateStopPath() const PathWithLaneId stop_path = getCurrentPath(); output.path = std::make_shared(stop_path); - output.drivable_area_info.drivable_lanes = status_.lanes; + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = status_.lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + output.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(stop_path); path_reference_ = getPreviousModuleOutput().reference_path; @@ -200,11 +205,13 @@ BehaviorModuleOutput PullOutModule::plan() const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); utils::generateDrivableArea( path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_); - output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.path = std::make_shared(path); - output.drivable_area_info.drivable_lanes = status_.lanes; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); @@ -322,8 +329,10 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, expanded_lanes); + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = expanded_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.path = std::make_shared(stop_path); output.drivable_area_info.drivable_lanes = status_.lanes; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 51de220f7fe42..3750c11f38bff 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2762,6 +2762,31 @@ std::vector combineDrivableLanes( return updated_drivable_lanes_vec; } +DrivableAreaInfo combineDrivableAreaInfo( + const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2) +{ + DrivableAreaInfo combined_drivable_area_info; + + // drivable lanes + combined_drivable_area_info.drivable_lanes = + combineDrivableLanes(drivable_area_info1.drivable_lanes, drivable_area_info2.drivable_lanes); + + // obstacles + for (const auto & obstacle : drivable_area_info1.obstacles) { + combined_drivable_area_info.obstacles.push_back(obstacle); + } + for (const auto & obstacle : drivable_area_info2.obstacles) { + combined_drivable_area_info.obstacles.push_back(obstacle); + } + + // enable expanding hatched road markings + combined_drivable_area_info.enable_expanding_hatched_road_markings = + drivable_area_info1.enable_expanding_hatched_road_markings || + drivable_area_info2.enable_expanding_hatched_road_markings; + + return combined_drivable_area_info; +} + // NOTE: Assuming that path.right/left_bound is already created. void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles)