fix(behavior_path_planner): fix short drivable area bug (autowarefoun…

…dation#1475)

* fix(behavior_path_planner): fix short drivable area bug

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add comment

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -193,8 +193,8 @@ cv::Point toCVPoint(
   const Point & geom_point, const double width_m, const double height_m, const double resolution);
 
 OccupancyGrid generateDrivableArea(
-  const lanelet::ConstLanelets & lanes, const double resolution, const double vehicle_length,
-  const std::shared_ptr<const PlannerData> planner_data);
+  const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution,
+  const double vehicle_length, const std::shared_ptr<const PlannerData> planner_data);
 
 lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets(
   const std::shared_ptr<const PlannerData> & planner_data);

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

@@ -1891,7 +1891,8 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c
   {
     const auto & p = planner_data_->parameters;
     shifted_path->path.drivable_area = util::generateDrivableArea(
-      extended_lanelets, p.drivable_area_resolution, p.vehicle_length, planner_data_);
+      shifted_path->path, extended_lanelets, p.drivable_area_resolution, p.vehicle_length,
+      planner_data_);
   }
 }
 

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

@@ -152,7 +152,7 @@ BehaviorModuleOutput LaneChangeModule::plan()
 
     const double resolution = common_parameters.drivable_area_resolution;
     path.drivable_area = util::generateDrivableArea(
-      lanes, resolution, common_parameters.vehicle_length, planner_data_);
+      path, lanes, resolution, common_parameters.vehicle_length, planner_data_);
   }
 
   if (isAbortConditionSatisfied()) {
@@ -288,8 +288,8 @@ PathWithLaneId LaneChangeModule::getReferencePath() const
     lane_change_buffer);
 
   reference_path.drivable_area = util::generateDrivableArea(
-    current_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length,
-    planner_data_);
+    reference_path, current_lanes, common_parameters.drivable_area_resolution,
+    common_parameters.vehicle_length, planner_data_);
 
   return reference_path;
 }

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -142,7 +142,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
   }
 
   reference_path.drivable_area = util::generateDrivableArea(
-    current_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_);
+    reference_path, current_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_);
 
   return reference_path;
 }

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

@@ -232,7 +232,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
     lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end());
     const double resolution = common_parameters.drivable_area_resolution;
     candidate_path.drivable_area = util::generateDrivableArea(
-      lanes, resolution, common_parameters.vehicle_length, planner_data_);
+      candidate_path, lanes, resolution, common_parameters.vehicle_length, planner_data_);
 
     updateRTCStatus(candidate.distance_to_path_change);
   }
@@ -309,7 +309,8 @@ void PullOutModule::updatePullOutStatus()
 
     const double resolution = common_parameters.drivable_area_resolution;
     status_.pull_out_path.path.drivable_area = util::generateDrivableArea(
-      lanes, resolution, common_parameters.vehicle_length, planner_data_);
+      status_.pull_out_path.path, lanes, resolution, common_parameters.vehicle_length,
+      planner_data_);
   }
 
   const auto arclength_start =
@@ -348,8 +349,8 @@ PathWithLaneId PullOutModule::getReferencePath() const
     parameters_.deceleration_interval, goal_pose);
 
   reference_path.drivable_area = util::generateDrivableArea(
-    pull_out_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length,
-    planner_data_);
+    reference_path, pull_out_lanes, common_parameters.drivable_area_resolution,
+    common_parameters.vehicle_length, planner_data_);
   return reference_path;
 }
 

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

@@ -648,8 +648,8 @@ bool PullOverModule::planShiftPath()
   std::tie(found_valid_path, found_safe_path) = getSafePath(shift_parking_path_);
 
   shift_parking_path_.path.drivable_area = util::generateDrivableArea(
-    status_.lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length,
-    planner_data_);
+    shift_parking_path_.path, status_.lanes, common_parameters.drivable_area_resolution,
+    common_parameters.vehicle_length, planner_data_);
 
   shift_parking_path_.path.header = planner_data_->route_handler->getRouteHeader();
   return found_safe_path;
@@ -694,7 +694,7 @@ PathWithLaneId PullOverModule::getReferencePath() const
   }
 
   reference_path.drivable_area = util::generateDrivableArea(
-    status_.current_lanes, common_parameters.drivable_area_resolution,
+    reference_path, status_.current_lanes, common_parameters.drivable_area_resolution,
     common_parameters.vehicle_length, planner_data_);
 
   return reference_path;
@@ -735,7 +735,7 @@ PathWithLaneId PullOverModule::getStopPath()
   }
 
   reference_path.drivable_area = util::generateDrivableArea(
-    status_.current_lanes, common_parameters.drivable_area_resolution,
+    reference_path, status_.current_lanes, common_parameters.drivable_area_resolution,
     common_parameters.vehicle_length, planner_data_);
 
   return reference_path;

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -425,7 +425,7 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const
   {
     const auto & p = planner_data_->parameters;
     path->path.drivable_area = util::generateDrivableArea(
-      extended_lanelets, p.drivable_area_resolution, p.vehicle_length, planner_data_);
+      path->path, extended_lanelets, p.drivable_area_resolution, p.vehicle_length, planner_data_);
   }
 }
 

planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp

@@ -149,7 +149,7 @@ void GeometricParallelParking::generateStraightPath(const Pose start_pose)
 
   const auto common_params = planner_data_->parameters;
   path.drivable_area = util::generateDrivableArea(
-    current_lanes, common_params.drivable_area_resolution, common_params.vehicle_length,
+    path, current_lanes, common_params.drivable_area_resolution, common_params.vehicle_length,
     planner_data_);
 
   path.points.back().point.longitudinal_velocity_mps = 0;
@@ -255,12 +255,14 @@ bool GeometricParallelParking::planOneTraial(
 
   path_turn_left.header = planner_data_->route_handler->getRouteHeader();
   path_turn_left.drivable_area = util::generateDrivableArea(
-    lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_);
+    path_turn_left, lanes, common_params.drivable_area_resolution, common_params.vehicle_length,
+    planner_data_);
   paths_.push_back(path_turn_left);
 
   path_turn_right.header = planner_data_->route_handler->getRouteHeader();
   path_turn_right.drivable_area = util::generateDrivableArea(
-    lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_);
+    path_turn_right, lanes, common_params.drivable_area_resolution, common_params.vehicle_length,
+    planner_data_);
   paths_.push_back(path_turn_right);
 
   Cr_.pose = Cr;

planning/behavior_path_planner/src/utilities.cpp

@@ -65,11 +65,47 @@ bool sumLengthFromTwoPoints(
   return is_end;
 }
 
-std::array<double, 4> getLaneletScope(
-  const lanelet::ConstLanelets & lanes, const size_t nearest_lane_idx,
+std::array<double, 4> getPathScope(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const std::shared_ptr<route_handler::RouteHandler> route_handler,
   const geometry_msgs::msg::Pose & current_pose, const double forward_lane_length,
   const double backward_lane_length, const double lane_margin)
 {
+  // extract lanes from path_with_lane_id
+  lanelet::ConstLanelets path_lanes = [&]() {
+    // extract "unique" lane ids from path_with_lane_id
+    std::vector<size_t> path_lane_ids;
+    for (const auto & path_point : path.points) {
+      for (const size_t lane_id : path_point.lane_ids) {
+        if (std::find(path_lane_ids.begin(), path_lane_ids.end(), lane_id) == path_lane_ids.end()) {
+          path_lane_ids.push_back(lane_id);
+        }
+      }
+    }
+
+    // get lanes according to lane ids
+    lanelet::ConstLanelets path_lanes;
+    for (const auto path_lane_id : path_lane_ids) {
+      const auto & lane = route_handler->getLaneletsFromId(path_lane_id);
+      path_lanes.push_back(lane);
+    }
+
+    return path_lanes;
+  }();
+
+  // claculate nearest lane idx
+  const int nearest_lane_idx = [&]() -> int {
+    lanelet::ConstLanelet closest_lanelet;
+    if (lanelet::utils::query::getClosestLanelet(path_lanes, current_pose, &closest_lanelet)) {
+      for (size_t i = 0; i < path_lanes.size(); ++i) {
+        if (path_lanes.at(i).id() == closest_lanelet.id()) {
+          return i;
+        }
+      }
+    }
+    return 0;
+  }();
+
   // define functions to get right/left bounds as a vector
   const auto get_bound_funcs =
     std::vector<std::function<lanelet::ConstLineString2d(const lanelet::ConstLanelet & lane)>>{
@@ -88,7 +124,7 @@ std::array<double, 4> getLaneletScope(
 
   for (const auto & get_bound_func : get_bound_funcs) {
     // search nearest point index to current pose
-    const auto & nearest_bound = get_bound_func(lanes.at(nearest_lane_idx));
+    const auto & nearest_bound = get_bound_func(path_lanes.at(nearest_lane_idx));
     if (nearest_bound.empty()) {
       continue;
     }
@@ -108,7 +144,7 @@ std::array<double, 4> getLaneletScope(
       motion_utils::calcSignedArcLength(points, current_pose.position, nearest_segment_idx);
     double sum_length = std::min(forward_offset_length, 0.0);
     size_t current_lane_idx = nearest_lane_idx;
-    auto current_lane = lanes.at(current_lane_idx);
+    auto current_lane = path_lanes.at(current_lane_idx);
     size_t current_point_idx = nearest_segment_idx;
     while (true) {
       const auto & bound = get_bound_func(current_lane);
@@ -130,12 +166,12 @@ std::array<double, 4> getLaneletScope(
         drivable_area_utils::updateMinMaxPosition(
           previous_bound[previous_point_idx].basicPoint(), min_x, min_y, max_x, max_y);
 
-        if (current_lane_idx == lanes.size() - 1) {
+        if (current_lane_idx == path_lanes.size() - 1) {
           break;
         }
 
         current_lane_idx += 1;
-        current_lane = lanes.at(current_lane_idx);
+        current_lane = path_lanes.at(current_lane_idx);
         current_point_idx = 0;
         const auto & current_bound = get_bound_func(current_lane);
 
@@ -156,7 +192,7 @@ std::array<double, 4> getLaneletScope(
       motion_utils::calcSignedArcLength(points, nearest_segment_idx + 1, current_pose.position);
     sum_length = std::min(backward_offset_length, 0.0);
     current_lane_idx = nearest_lane_idx;
-    current_lane = lanes.at(current_lane_idx);
+    current_lane = path_lanes.at(current_lane_idx);
     while (true) {
       const auto & bound = get_bound_func(current_lane);
       if (current_point_idx != 0) {
@@ -182,7 +218,7 @@ std::array<double, 4> getLaneletScope(
         }
 
         current_lane_idx -= 1;
-        current_lane = lanes.at(current_lane_idx);
+        current_lane = path_lanes.at(current_lane_idx);
         const auto & current_bound = get_bound_func(current_lane);
         current_point_idx = current_bound.size() - 1;
 
@@ -1029,40 +1065,32 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal
 }
 
 // input lanes must be in sequence
+// NOTE: lanes in the path argument is used to calculate the size of the drivable area to cover
+// designated forward and backward length by getPathScope function.
+//       lanes argument is used to determinte (= draw) the drivable area.
+//       This is because lanes argument has multiple parallel lanes which makes hard to calculate
+//       the size of the drivable area
 OccupancyGrid generateDrivableArea(
-  const lanelet::ConstLanelets & lanes, const double resolution, const double vehicle_length,
-  const std::shared_ptr<const PlannerData> planner_data)
+  const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution,
+  const double vehicle_length, const std::shared_ptr<const PlannerData> planner_data)
 {
   const auto & params = planner_data->parameters;
   const auto route_handler = planner_data->route_handler;
   const auto current_pose = planner_data->self_pose;
 
-  // search closest lanelet to current pose from given lanelets
-  const int nearest_lane_idx = [&]() -> int {
-    lanelet::ConstLanelet closest_lanelet;
-    if (lanelet::utils::query::getClosestLanelet(lanes, current_pose->pose, &closest_lanelet)) {
-      for (size_t i = 0; i < lanes.size(); ++i) {
-        if (lanes.at(i).id() == closest_lanelet.id()) {
-          return i;
-        }
-      }
-    }
-    return 0;
-  }();
-
-  // calculate min/max x and y
-  const auto lanelet_scope = drivable_area_utils::getLaneletScope(
-    lanes, nearest_lane_idx, current_pose->pose, params.drivable_lane_forward_length,
+  // calculate min/max x and y from lanes in path argument (not from lanes argument)
+  const auto path_scope = drivable_area_utils::getPathScope(
+    path, route_handler, current_pose->pose, params.drivable_lane_forward_length,
     params.drivable_lane_backward_length, params.drivable_lane_margin);
 
   const double min_x =
-    drivable_area_utils::quantize(lanelet_scope.at(0) - params.drivable_area_margin, resolution);
+    drivable_area_utils::quantize(path_scope.at(0) - params.drivable_area_margin, resolution);
   const double min_y =
-    drivable_area_utils::quantize(lanelet_scope.at(1) - params.drivable_area_margin, resolution);
+    drivable_area_utils::quantize(path_scope.at(1) - params.drivable_area_margin, resolution);
   const double max_x =
-    drivable_area_utils::quantize(lanelet_scope.at(2) + params.drivable_area_margin, resolution);
+    drivable_area_utils::quantize(path_scope.at(2) + params.drivable_area_margin, resolution);
   const double max_y =
-    drivable_area_utils::quantize(lanelet_scope.at(3) + params.drivable_area_margin, resolution);
+    drivable_area_utils::quantize(path_scope.at(3) + params.drivable_area_margin, resolution);
 
   const double width = max_x - min_x;
   const double height = max_y - min_y;
@@ -1584,7 +1612,7 @@ std::shared_ptr<PathWithLaneId> generateCenterLinePath(
   centerline_path->header = route_handler->getRouteHeader();
 
   centerline_path->drivable_area = util::generateDrivableArea(
-    lanelet_sequence, p.drivable_area_resolution, p.vehicle_length, planner_data);
+    *centerline_path, lanelet_sequence, p.drivable_area_resolution, p.vehicle_length, planner_data);
 
   return centerline_path;
 }