feat(behavior_path_planner): pull_over decrease hz (autowarefoundatio…

…n#1819)

* feat(behavior_path_planner): pull_over decrease hz

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* generate drivable area at last in pull_out

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp

@@ -52,6 +52,7 @@ struct PullOutStatus
   PathWithLaneId backward_path;
   lanelet::ConstLanelets current_lanes;
   lanelet::ConstLanelets pull_out_lanes;
+  lanelet::ConstLanelets lanes;
   std::vector<uint64_t> lane_follow_lane_ids;
   std::vector<uint64_t> pull_out_lane_ids;
   bool is_safe = false;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp

@@ -24,14 +24,14 @@ namespace behavior_path_planner
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 struct ShiftParkingPath
 {
-  PathWithLaneId path;
-  PathWithLaneId straight_path;
-  ShiftedPath shifted_path;
-  ShiftPoint shift_point;
+  PathWithLaneId path{};
+  PathWithLaneId straight_path{};
+  ShiftedPath shifted_path{};
+  ShiftPoint shift_point{};
   double acceleration{0.0};
   double preparation_length{0.0};
   double pull_over_length{0.0};
-  bool is_safe;
+  bool is_safe{false};
 };
 }  // namespace behavior_path_planner
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PATH_HPP_

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

@@ -168,6 +168,9 @@ BehaviorModuleOutput PullOutModule::plan()
   } else {
     path = status_.backward_path;
   }
+  path.drivable_area = util::generateDrivableArea(
+    path, status_.lanes, planner_data_->parameters.drivable_area_resolution,
+    planner_data_->parameters.vehicle_length, planner_data_);
 
   output.path = std::make_shared<PathWithLaneId>(path);
   output.turn_signal_info =
@@ -220,9 +223,14 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
 {
   BehaviorModuleOutput output;
   const auto current_lanes = getCurrentLanes();
-  const auto shoulder_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
+  const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
+  auto lanes = current_lanes;
+  lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
 
-  const auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
+  auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
+  candidate_path.drivable_area = util::generateDrivableArea(
+    candidate_path, lanes, planner_data_->parameters.drivable_area_resolution,
+    planner_data_->parameters.vehicle_length, planner_data_);
   auto stop_path = candidate_path;
   for (auto & p : stop_path.points) {
     p.point.longitudinal_velocity_mps = 0.0;
@@ -327,7 +335,6 @@ void PullOutModule::planWithPriorityOnShortBackDistance(
 void PullOutModule::updatePullOutStatus()
 {
   const auto & route_handler = planner_data_->route_handler;
-  const auto & common_parameters = planner_data_->parameters;
   const auto & current_pose = planner_data_->self_pose->pose;
   const auto & goal_pose = planner_data_->route_handler->getGoalPose();
 
@@ -338,6 +345,10 @@ void PullOutModule::updatePullOutStatus()
   const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
   status_.pull_out_lanes = pull_out_lanes;
 
+  // combine road and shoulder lanes
+  status_.lanes = current_lanes;
+  status_.lanes.insert(status_.lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
+
   // search pull out start candidates backward
   std::vector<Pose> start_pose_candidates;
   {
@@ -372,9 +383,6 @@ void PullOutModule::updatePullOutStatus()
     status_.backward_path = pull_out_utils::getBackwardPath(
       *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,
       parameters_.backward_velocity);
-    status_.backward_path.drivable_area = util::generateDrivableArea(
-      status_.backward_path, pull_out_lanes, common_parameters.drivable_area_resolution,
-      common_parameters.vehicle_length, planner_data_);
   }
 
   // Update status
@@ -405,14 +413,13 @@ lanelet::ConstLanelets PullOutModule::getCurrentLanes() const
 std::vector<Pose> PullOutModule::searchBackedPoses()
 {
   const auto current_pose = planner_data_->self_pose->pose;
-  const auto current_lanes = getCurrentLanes();
-  const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
 
   // get backward shoulder path
-  const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose);
+  const auto arc_position_pose =
+    lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose);
   const double check_distance = parameters_.max_back_distance + 30.0;  // buffer
   auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath(
-    pull_out_lanes, arc_position_pose.length - check_distance,
+    status_.pull_out_lanes, arc_position_pose.length - check_distance,
     arc_position_pose.length + check_distance);
 
   // lateral shift to current_pose

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

@@ -624,9 +624,14 @@ BehaviorModuleOutput PullOverModule::plan()
         -calcMinimumShiftPathDistance(), parameters_.deceleration_interval);
     }
   }
+  // generate drivable area
+  {
+    const auto p = planner_data_->parameters;
+    status_.path.drivable_area = util::generateDrivableArea(
+      status_.path, status_.lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_);
+  }
 
   BehaviorModuleOutput output;
-
   // safe: use pull over path
   if (status_.is_safe) {
     output.path = std::make_shared<PathWithLaneId>(status_.path);
@@ -731,10 +736,6 @@ bool PullOverModule::planShiftPath()
     return found_safe_path;
   }
 
-  shift_parking_path_.path.drivable_area = util::generateDrivableArea(
-    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;
 }

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

@@ -99,11 +99,21 @@ std::vector<ShiftParkingPath> getShiftParkingPaths(
   const double jerk_resolution =
     std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_over_sampling_num;
 
-  double distance_to_shoulder_lane_boundary =
+  const double distance_to_shoulder_lane_boundary =
     util::getDistanceToShoulderBoundary(route_handler.getShoulderLanelets(), current_pose);
-  double offset_from_current_pose =
+  const double offset_from_current_pose =
     distance_to_shoulder_lane_boundary + common_parameter.vehicle_width / 2 + margin;
 
+  // shift end point in shoulder lane
+  PathPointWithLaneId shift_end_point;
+  {
+    const auto arc_position_goal = lanelet::utils::getArcCoordinates(target_lanelets, goal_pose);
+    const double s_start = arc_position_goal.length - after_pull_over_straight_distance;
+    const double s_end = s_start + std::numeric_limits<double>::epsilon();
+    const auto path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end, true);
+    shift_end_point = path.points.front();
+  }
+
   for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk;
        lateral_jerk += jerk_resolution) {
     PathShifter path_shifter;
@@ -124,15 +134,6 @@ std::vector<ShiftParkingPath> getShiftParkingPaths(
                           pull_over_distance - arc_position_pose.length;
     }
 
-    // shift end point in shoulder lane
-    const auto shift_end_point = [&]() {
-      const auto arc_position_goal = lanelet::utils::getArcCoordinates(target_lanelets, goal_pose);
-      const double s_start = arc_position_goal.length - after_pull_over_straight_distance;
-      const double s_end = s_start + std::numeric_limits<double>::epsilon();
-      const auto path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end, true);
-      return path.points.front();
-    }();
-
     PathWithLaneId road_lane_reference_path;
     {
       const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, current_pose);

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

@@ -347,11 +347,6 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start
   auto path = planner_data_->route_handler->getCenterLinePath(
     current_lanes, current_arc_position.length, start_arc_position.length, true);
   path.header = planner_data_->route_handler->getRouteHeader();
-
-  path.drivable_area = util::generateDrivableArea(
-    path, current_lanes, planner_data_->parameters.drivable_area_resolution,
-    planner_data_->parameters.vehicle_length, planner_data_);
-
   path.points.back().point.longitudinal_velocity_mps = 0;
 
   return path;
@@ -429,9 +424,6 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
     p.lane_ids.push_back(closest_shoulder_lanelet.id());
   }
   path_turn_left.header = planner_data_->route_handler->getRouteHeader();
-  path_turn_left.drivable_area = util::generateDrivableArea(
-    path_turn_left, lanes, common_params.drivable_area_resolution, common_params.vehicle_length,
-    planner_data_);
 
   PathWithLaneId path_turn_right = generateArcPath(
     Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward);
@@ -440,9 +432,6 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
     p.lane_ids.push_back(closest_shoulder_lanelet.id());
   }
   path_turn_right.header = planner_data_->route_handler->getRouteHeader();
-  path_turn_right.drivable_area = util::generateDrivableArea(
-    path_turn_right, lanes, common_params.drivable_area_resolution, common_params.vehicle_length,
-    planner_data_);
 
   // Need to add straight path to last right_turning for parking in parallel
   if (std::abs(end_pose_offset) > 0) {