feat(behavior_path_planner): combine drivable area info from multiple…

… modules (autowarefoundation#3647) * feat(behavior_path_planner): combine drivable area info from multiple modules Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tier4 · May 10, 2023 · f127f2e · f127f2e
1 parent 6781a5a
commit f127f2e
Show file tree

Hide file tree

Showing 6 changed files with 70 additions and 16 deletions.
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
@@ -391,6 +391,9 @@ std::vector<DrivableLanes> combineDrivableLanes(
   const std::vector<DrivableLanes> & original_drivable_lanes_vec,
   const std::vector<DrivableLanes> & new_drivable_lanes_vec);
 
+DrivableAreaInfo combineDrivableAreaInfo(
+  const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);
+
 void extractObstaclesFromDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & 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
@@ -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
@@ -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
@@ -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
@@ -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<PathWithLaneId>(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<PathWithLaneId>(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<PathWithLaneId>(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<PathWithLaneId>(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<PathWithLaneId>(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
@@ -2762,6 +2762,31 @@ std::vector<DrivableLanes> 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<DrivableAreaInfo::Obstacle> & obstacles)