fix(behavior_path_planner): add drivable area information (autowarefo…

…undation#3660) (autowarefoundation#421)

* fix(behavior_path_planner): add drivable area information



* fix(behavior_path_planner): fix typo



* fix(behavior_path_planner): fix cmake



* fix(behavior_path_planner): cosmetic change



---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -297,9 +297,7 @@ PathWithLaneId refinePathForGoal(
 
 bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id);
 
-PathWithLaneId createGoalAroundPath(
-  const std::shared_ptr<RouteHandler> & route_handler,
-  const std::optional<PoseWithUuidStamped> & modified_goal);
+BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr<const PlannerData> & planner_data);
 
 bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes);
 

planning/behavior_path_planner/src/behavior_tree_manager.cpp

@@ -97,11 +97,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr<PlannerData>
     !is_any_module_running &&
     utils::isEgoOutOfRoute(
       data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler)) {
-    BehaviorModuleOutput output{};
-    const auto output_path =
-      utils::createGoalAroundPath(data->route_handler, data->prev_modified_goal);
-    output.path = std::make_shared<PathWithLaneId>(output_path);
-    output.reference_path = std::make_shared<PathWithLaneId>(output_path);
+    BehaviorModuleOutput output = utils::createGoalAroundPath(data);
     return output;
   }
 

planning/behavior_path_planner/src/planner_manager.cpp

@@ -62,11 +62,8 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
       data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler);
 
     if (!is_any_module_running && is_out_of_route) {
-      BehaviorModuleOutput output{};
-      const auto output_path =
-        utils::createGoalAroundPath(data->route_handler, data->prev_modified_goal);
-      output.path = std::make_shared<PathWithLaneId>(output_path);
-      output.reference_path = std::make_shared<PathWithLaneId>(output_path);
+      BehaviorModuleOutput output = utils::createGoalAroundPath(data);
+      generateCombinedDrivableArea(output, data);
       return output;
     }
 
@@ -129,6 +126,11 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
 void PlannerManager::generateCombinedDrivableArea(
   BehaviorModuleOutput & output, const std::shared_ptr<PlannerData> & data) const
 {
+  if (output.path->points.empty()) {
+    RCLCPP_ERROR_STREAM(logger_, "[generateCombinedDrivableArea] Output path is empty!");
+    return;
+  }
+
   const auto & di = output.drivable_area_info;
   constexpr double epsilon = 1e-3;
 

planning/behavior_path_planner/src/utils/utils.cpp

@@ -936,10 +936,14 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal
   return false;
 }
 
-PathWithLaneId createGoalAroundPath(
-  const std::shared_ptr<RouteHandler> & route_handler,
-  const std::optional<PoseWithUuidStamped> & modified_goal)
+BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr<const PlannerData> & planner_data)
 {
+  BehaviorModuleOutput output;
+
+  const auto & p = planner_data->parameters;
+  const auto & route_handler = planner_data->route_handler;
+  const auto & modified_goal = planner_data->prev_modified_goal;
+
   const Pose goal_pose = modified_goal ? modified_goal->pose : route_handler->getGoalPose();
   const auto shoulder_lanes = route_handler->getShoulderLanelets();
 
@@ -951,16 +955,34 @@ PathWithLaneId createGoalAroundPath(
     return !route_handler->getGoalLanelet(&goal_lane);
   });
   if (is_failed_getting_lanelet) {
-    PathWithLaneId path{};
-    return path;
+    return output;
   }
 
   constexpr double backward_length = 1.0;
   const auto arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, goal_pose);
   const double s_start = std::max(arc_coord.length - backward_length, 0.0);
   const double s_end = arc_coord.length;
 
-  return route_handler->getCenterLinePath({goal_lane}, s_start, s_end);
+  auto reference_path = route_handler->getCenterLinePath({goal_lane}, s_start, s_end);
+
+  const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler);
+  const auto drivable_lanes = generateDrivableLanes(drivable_lanelets);
+
+  const auto & dp = planner_data->drivable_area_expansion_parameters;
+
+  const auto shorten_lanes = cutOverlappedLanes(reference_path, drivable_lanes);
+  const auto expanded_lanes = expandLanelets(
+    shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
+    dp.drivable_area_types_to_skip);
+
+  // for old architecture
+  generateDrivableArea(reference_path, expanded_lanes, false, p.vehicle_length, planner_data);
+
+  output.path = std::make_shared<PathWithLaneId>(reference_path);
+  output.reference_path = std::make_shared<PathWithLaneId>(reference_path);
+  output.drivable_area_info.drivable_lanes = drivable_lanes;
+
+  return output;
 }
 
 bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes)
@@ -1531,7 +1553,7 @@ void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound
         }
         monotonic_bound.push_back(original_bound.at(b_idx));
 
-        // caculate bound pose and its laterally offset pose.
+        // calculate bound pose and its laterally offset pose.
         const auto bound_pose = [&]() {
           geometry_msgs::msg::Pose pose;
           pose.position = original_bound.at(b_idx);