po

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -76,6 +76,14 @@ struct DrivableLanes
   lanelet::ConstLanelets middle_lanes;
 };
 
+struct DrivableAreaInfo
+{
+  std::vector<DrivableLanes> drivable_lanes;
+  std::vector<tier4_autoware_utils::Polygon2d> obstacle_polys;
+  double left_drivable_margin;   // temporary only for pull over/out and sideshift
+  double right_drivable_margin;  // temporary only for pull over/out and sideshift
+};
+
 struct TurnSignalInfo
 {
   TurnSignalInfo()
@@ -108,8 +116,9 @@ struct BehaviorModuleOutput
 
   std::optional<PoseWithUuidStamped> modified_goal{};
 
-  // drivable lanes
-  std::vector<DrivableLanes> drivable_lanes;
+  // drivable area info to create drivable area
+  // NOTE: Drivable area in the path is generated at last from drivable_area_info.
+  DrivableAreaInfo drivable_area_info;
 };
 
 struct CandidateOutput

planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp

@@ -85,7 +85,7 @@ class SideShiftModule : public SceneModuleInterface
 #endif
 
   // non-const methods
-  void adjustDrivableArea(ShiftedPath * path) const;
+  void adjustDrivableArea(ShiftedPath * path, DrivableAreaInfo & out) const;
 
   ShiftLine calcShiftLine() const;
 

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp

@@ -89,11 +89,9 @@ void setStartData(
 Polygon2d createEnvelopePolygon(
   const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer);
 
-void generateDrivableArea(
-  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
-  const std::shared_ptr<const PlannerData> planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters, const ObjectDataArray & objects,
-  const double vehicle_length, const bool enable_bound_clipping, const bool disable_path_update);
+std::vector<tier4_autoware_utils::Polygon2d> generateObstaclePolygonsForDrivableArea(
+  const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
+  const double vehicle_width);
 
 double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);
 

planning/behavior_path_planner/src/behavior_tree_manager.cpp

@@ -108,6 +108,14 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr<PlannerData>
     m->publishRTCStatus();
     m->publishSteeringFactor();
   });
+
+  // generate drivable area
+  if (output.path) {
+    utils::generateDrivableArea(
+      *output.path, output.drivable_area_info.drivable_lanes, data->parameters.vehicle_length,
+      data);
+  }
+
   return output;
 }
 

planning/behavior_path_planner/src/planner_manager.cpp

@@ -47,56 +47,74 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
   std::for_each(
     manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); });
 
-  while (rclcpp::ok()) {
-    /**
-     * STEP1: get approved modules' output
-     */
-    const auto approved_modules_output = runApprovedModules(data);
+  auto result_output = [&]() {
+    while (rclcpp::ok()) {
+      /**
+       * STEP1: get approved modules' output
+       */
+      const auto approved_modules_output = runApprovedModules(data);
+
+      /**
+       * STEP2: check modules that need to be launched
+       */
+      const auto request_modules = getRequestModules(approved_modules_output);
+
+      /**
+       * STEP3: if there is no module that need to be launched, return approved modules' output
+       */
+      if (request_modules.empty()) {
+        processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
+        return approved_modules_output;
+      }
 
-    /**
-     * STEP2: check modules that need to be launched
-     */
-    const auto request_modules = getRequestModules(approved_modules_output);
+      /**
+       * STEP4: if there is module that should be launched, execute the module
+       */
+      const auto [highest_priority_module, candidate_modules_output] =
+        runRequestModules(request_modules, data, approved_modules_output);
+      if (!highest_priority_module) {
+        processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
+        return approved_modules_output;
+      }
 
-    /**
-     * STEP3: if there is no module that need to be launched, return approved modules' output
-     */
-    if (request_modules.empty()) {
-      processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-      return approved_modules_output;
-    }
+      /**
+       * STEP5: if the candidate module's modification is NOT approved yet, return the result.
+       * NOTE: the result is output of the candidate module, but the output path don't contains path
+       * shape modification that needs approval. On the other hand, it could include velocity
+       * profile modification.
+       */
+      if (highest_priority_module->isWaitingApproval()) {
+        processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
+        return candidate_modules_output;
+      }
 
-    /**
-     * STEP4: if there is module that should be launched, execute the module
-     */
-    const auto [highest_priority_module, candidate_modules_output] =
-      runRequestModules(request_modules, data, approved_modules_output);
-    if (!highest_priority_module) {
-      processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-      return approved_modules_output;
+      /**
+       * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_
+       */
+      addApprovedModule(highest_priority_module);
+      clearCandidateModules();
     }
+    return BehaviorModuleOutput{};
+  }();
 
-    /**
-     * STEP5: if the candidate module's modification is NOT approved yet, return the result.
-     * NOTE: the result is output of the candidate module, but the output path don't contains path
-     * shape modification that needs approval. On the other hand, it could include velocity profile
-     * modification.
-     */
-    if (highest_priority_module->isWaitingApproval()) {
-      processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-      return candidate_modules_output;
-    }
+  const auto shorten_lanes =
+    utils::cutOverlappedLanes(*result_output.path, result_output.drivable_area_info.drivable_lanes);
+  const auto extended_lanes = utils::expandLanelets(
+    shorten_lanes, result_output.drivable_area_info.left_drivable_margin,
+    result_output.drivable_area_info.right_drivable_margin, {"road_border"});
 
-    /**
-     * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_
-     */
-    addApprovedModule(highest_priority_module);
-    clearCandidateModules();
-  }
+  std::cerr << result_output.drivable_area_info.drivable_lanes.size() << " " << shorten_lanes.size()
+            << " " << extended_lanes.size() << std::endl;
 
-  processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
+  // TODO(murooka)
+  // parameters_->drivable_area_types_to_skip);
+  utils::generateDrivableArea(
+    *result_output.path, extended_lanes, data->parameters.vehicle_length, data);
 
-  return {};
+  std::cerr << result_output.path->right_bound.size() << " "
+            << result_output.path->left_bound.size() << std::endl;
+
+  return result_output;
 }
 
 std::vector<SceneModulePtr> PlannerManager::getRequestModules(
@@ -358,12 +376,16 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   std::unordered_map<std::string, BehaviorModuleOutput> results;
 
   BehaviorModuleOutput output = getReferencePath(data);
+  // TODO(murooka) is this correct?
+  const auto current_lanes = utils::getCurrentLanes(data);
+  output.drivable_area_info.drivable_lanes = utils::generateDrivableLanes(current_lanes);
   results.emplace("root", output);
 
   /**
    * execute all approved modules.
    */
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
+    std::cerr << m->name() << std::endl;
     output = run(m, data, output);
     results.emplace(m->name(), output);
   });
@@ -428,6 +450,18 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   // convert reverse iterator -> iterator
   const auto success_itr = std::prev(not_success_itr).base() - 1;
 
+  utils::generateDrivableArea(
+    *approved_modules_output.path, approved_modules_output.drivable_area_info.drivable_lanes,
+    data->parameters.vehicle_length, data);
+  // TODO(murooka) expandLanelets
+  // TODO(murooka) remove obstacle from drivable area
+
+  //   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
+  //   const std::shared_ptr<const PlannerData> planner_data,
+  //   const std::shared_ptr<AvoidanceParameters> & parameters, const ObjectDataArray & objects,
+  //   const double vehicle_length, const bool enable_bound_clipping, const bool
+  //   disable_path_update)
+
   /**
    * there is no succeeded module. return.
    */

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2371,8 +2371,6 @@ ObjectDataArray AvoidanceModule::getAdjacentLaneObjects(
 // two lanes since which way to avoid is not obvious
 void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const
 {
-  auto & path = *output.path;
-
   const auto has_same_lane =
     [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) {
       if (lanes.empty()) return false;
@@ -2383,14 +2381,15 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
   const auto & route_handler = planner_data_->route_handler;
   const auto & current_lanes = avoidance_data_.current_lanelets;
   const auto & enable_opposite = parameters_->enable_avoidance_over_opposite_direction;
+  std::vector<DrivableLanes> drivable_lanes;
 
   for (const auto & current_lane : current_lanes) {
     DrivableLanes current_drivable_lanes;
     current_drivable_lanes.left_lane = current_lane;
     current_drivable_lanes.right_lane = current_lane;
 
     if (!parameters_->enable_avoidance_over_same_direction) {
-      output.drivable_lanes.push_back(current_drivable_lanes);
+      drivable_lanes.push_back(current_drivable_lanes);
       continue;
     }
 
@@ -2511,14 +2510,21 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
       current_drivable_lanes.middle_lanes.push_back(current_lane);
     }
 
-    output.drivable_lanes.push_back(current_drivable_lanes);
+    drivable_lanes.push_back(current_drivable_lanes);
   }
 
-  {
-    const auto & p = planner_data_->parameters;
-    generateDrivableArea(
-      path, output.drivable_lanes, planner_data_, parameters_, avoidance_data_.target_objects,
-      p.vehicle_length, parameters_->enable_bound_clipping, parameters_->disable_path_update);
+  {  // for old architecture
+    utils::generateDrivableArea(
+      *output.path, drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+  }
+
+  {  // for new architecture
+    // generate drivable lanes
+    output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
+      getPreviousModuleOutput().drivable_area_info.drivable_lanes, drivable_lanes);
+    // generate obstacle polygons
+    output.drivable_area_info.obstacle_polys = generateObstaclePolygonsForDrivableArea(
+      avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
   }
 }
 

planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp

@@ -620,6 +620,7 @@ std_msgs::msg::Header ExternalRequestLaneChangeModule::getRouteHeader() const
 {
   return planner_data_->route_handler->getRouteHeader();
 }
+
 void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(BehaviorModuleOutput & output)
 {
   const auto & common_parameters = planner_data_->parameters;
@@ -631,8 +632,14 @@ void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(BehaviorModul
     drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
     dp.drivable_area_types_to_skip);
 
+  // for new architecture
+  output.drivable_area_info.drivable_lanes = output.drivable_area_info.drivable_lanes =
+    utils::combineDrivableLanes(
+      getPreviousModuleOutput().drivable_area_info.drivable_lanes, expanded_lanes);
+
+  // for old architecture
   utils::generateDrivableArea(
-    output.path, expanded_lanes, common_parameters.vehicle_length, planner_data_);
+    *output.path, expanded_lanes, common_parameters.vehicle_length, planner_data_);
 }
 
 bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -116,7 +116,9 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   resetPathCandidate();
   resetPathReference();
 
-  module_type_->setPreviousDrivableLanes(getPreviousModuleOutput().drivable_lanes);
+  module_type_->setPreviousDrivableLanes(
+    getPreviousModuleOutput().drivable_area_info.drivable_lanes);
+
   auto output = module_type_->generatePlannedPath();
 
   if (!module_type_->isValidPath()) {

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -201,6 +201,8 @@ BehaviorModuleOutput PullOutModule::plan()
   const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes);
   utils::generateDrivableArea(
     path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+  output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
+    getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes);
 
   output.path = std::make_shared<PathWithLaneId>(path);
   output.reference_path = getPreviousModuleOutput().reference_path;
@@ -320,6 +322,9 @@ 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);
+
   output.path = std::make_shared<PathWithLaneId>(stop_path);
   output.reference_path = getPreviousModuleOutput().reference_path;
   output.turn_signal_info = calcTurnSignalInfo();
@@ -522,8 +527,11 @@ PathWithLaneId PullOutModule::generateStopPath() const
 
   // generate drivable area
   const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes);
+
+  // generate drivable area info
   utils::generateDrivableArea(
     path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+  // TODO(murooka) use output
 
   return path;
 }

planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

@@ -625,6 +625,10 @@ BehaviorModuleOutput PullOverModule::plan()
   }
 
   BehaviorModuleOutput output;
+  // TODO(murooka)
+  // output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
+  //   getPreviousModuleOutput().drivable_area_info.drivable_lanes, expanded_lanes);
+
   if (status_.is_safe) {
     // safe: use pull over path
     status_.stop_pose.reset();