Merge pull request autowarefoundation#673 from tier4/fix/start_goal_p…

…lanner

Fix/start goal planner

planning/behavior_path_planner/config/start_planner/start_planner.param.yaml

@@ -8,12 +8,13 @@
       collision_check_distance_from_end: 1.0
       # shift pull out
       enable_shift_pull_out: true
-      shift_pull_out_velocity: 2.0
-      pull_out_sampling_num: 4
       minimum_shift_pull_out_distance: 0.0
-      maximum_lateral_jerk: 2.0
-      minimum_lateral_jerk: 0.1
       deceleration_interval: 15.0
+      lateral_jerk: 0.5
+      lateral_acceleration_sampling_num: 3
+      minimum_lateral_acc: 0.15
+      maximum_lateral_acc: 0.5
+      maximum_curvature: 0.07
       # geometric pull out
       enable_geometric_pull_out: true
       divide_pull_out_path: false

planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md

@@ -2,7 +2,17 @@
 
 ## Purpose / Role
 
-Pull out from the shoulder lane without colliding with objects.
+The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and implementing safety checks against dynamic obstacles. (Note: The feature of safety checks against dynamic obstacles is currently a work in progress.)
+This module is activated when a new route is received.
+
+Use cases are as follows
+
+- start smoothly from the current ego position to centerline.
+  - ![case1](../image/start_from_road_lane.drawio.svg)
+- pull out from the side of the road lane to centerline.
+  - ![case2](../image/start_from_road_side.drawio.svg)
+- pull out from the shoulder lane to the road lane centerline.
+  - ![case3](../image/start_from_road_shoulder.drawio.svg)
 
 ## Design
 
@@ -60,31 +70,37 @@ PullOutPath --o PullOutPlannerBase
 | collision_check_margin                                      | [m]   | double | Obstacle collision check margin                                      | 1.0           |
 | collision_check_distance_from_end                           | [m]   | double | collision check distance from end point. currently only for pull out | 15.0          |
 
-## **Safe check with obstacles in shoulder lane**
+## Safety check with static obstacles
 
 1. Calculate ego-vehicle's footprint on pull out path between from current position to pull out end point. (Illustrated by blue frame)
-2. Calculate object's polygon which is located in shoulder lane
+2. Calculate object's polygon
 3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path
 
 ![pull_out_collision_check](../image/pull_out_collision_check.drawio.svg)
 
-### **Path Generation**
+## Safety check with dynamic obstacles
+
+WIP
+
+## **Path Generation**
 
 There are two path generation methods.
 
-#### **shift pull out**
+### **shift pull out**
+
+This is the most basic method of starting path planning and is used on road lanes and shoulder lanes when there is no particular obstruction.
 
 Pull out distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values, and the one that generates a safe path is selected.
 
-- Generate the shoulder lane centerline and shift it to the current position.
+- Generate the road lane centerline and shift it to the current position.
 - In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials)
 - Combine this path with center line of road lane
 
 ![shift_pull_out](../image/shift_pull_out.drawio.svg)
 
 [shift pull out video](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4)
 
-##### parameters for shift pull out
+#### parameters for shift pull out
 
 | Name                            | Unit   | Type   | Description                                                                                                          | Default value |
 | :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
@@ -94,8 +110,11 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral
 | maximum_lateral_jerk            | [m/s3] | double | maximum lateral jerk                                                                                                 | 2.0           |
 | minimum_lateral_jerk            | [m/s3] | double | minimum lateral jerk                                                                                                 | 0.1           |
 | minimum_shift_pull_out_distance | [m]    | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0           |
+| maximum_curvature               | [m]    | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value.             |
+
+| 0.07 |
 
-#### **geometric pull out**
+### **geometric pull out**
 
 Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot.
 See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm.
@@ -104,7 +123,7 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474
 
 [geometric pull out video](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4)
 
-##### parameters for geometric pull out
+#### parameters for geometric pull out
 
 | Name                        | Unit  | Type   | Description                                                                                                                                                | Default value |
 | :-------------------------- | :---- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
@@ -115,15 +134,15 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474
 | lane_departure_margin       | [m]   | double | margin of deviation to lane right                                                                                                                          | 0.2           |
 | pull_out_max_steer_angle    | [rad] | double | maximum steer angle for path generation                                                                                                                    | 0.26          |
 
-### **backward pull out start point search**
+## **backward pull out start point search**
 
 If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`).
 
 ![pull_out_after_back](../image/pull_out_after_back.drawio.svg)
 
 [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4)
 
-#### **parameters for backward pull out start point search**
+### **parameters for backward pull out start point search**
 
 | Name                          | Unit | Type   | Description                                                                                                                                                          | Default value  |
 | :---------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- |

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -134,7 +134,6 @@ class GoalPlannerModule : public SceneModuleInterface
   // goal searcher
   std::shared_ptr<GoalSearcherBase> goal_searcher_;
   std::optional<GoalCandidate> modified_goal_pose_;
-  std::optional<size_t> prev_goal_id_;
   Pose refined_goal_pose_;
   GoalCandidates goal_candidates_;
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -155,6 +155,12 @@ class StartPlannerModule : public SceneModuleInterface
   bool isStopped();
   bool hasFinishedCurrentPath();
 
+  // check if the goal is located behind the ego in the same route segment.
+  bool IsGoalBehindOfEgoInSameRouteSegment() const;
+
+  // generate BehaviorPathOutput with stopping path.
+  BehaviorModuleOutput generateStopOutput() const;
+
   void setDebugData() const;
 
 // temporary for old architecture

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp

@@ -41,7 +41,7 @@ class ShiftPullOut : public PullOutPlannerBase
 
   std::vector<PullOutPath> calcPullOutPaths(
     const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose,
+    const Pose & start_pose, const Pose & goal_pose,
     const BehaviorPathPlannerParameters & common_parameter,
     const behavior_path_planner::StartPlannerParameters & parameter);
 
@@ -53,6 +53,13 @@ class ShiftPullOut : public PullOutPlannerBase
     const PathWithLaneId & path, const double target_after_arc_length, const double dr);
 
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
+
+private:
+  // Calculate longitudinal distance based on the acceleration limit, curvature limit, and the
+  // minimum distance requirement.
+  double calcPullOutLongitudinalDistance(
+    const double lon_acc, const double shift_time, const double shift_length,
+    const double max_curvature, const double min_distance) const;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp

@@ -35,11 +35,12 @@ struct StartPlannerParameters
   double collision_check_distance_from_end;
   // shift pull out
   bool enable_shift_pull_out;
-  double shift_pull_out_velocity;
-  int pull_out_sampling_num;
   double minimum_shift_pull_out_distance;
-  double maximum_lateral_jerk;
-  double minimum_lateral_jerk;
+  int lateral_acceleration_sampling_num;
+  double lateral_jerk;
+  double maximum_lateral_acc;
+  double minimum_lateral_acc;
+  double maximum_curvature;  // maximum curvature considered in the path generation
   double deceleration_interval;
   // geometric pull out
   bool enable_geometric_pull_out;

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

@@ -43,7 +43,8 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa
 PathWithLaneId getBackwardPath(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & current_pose, const Pose & backed_pose, const double velocity);
-lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr<const PlannerData> & planner_data);
+lanelet::ConstLanelets getPullOutLanes(
+  const std::shared_ptr<const PlannerData> & planner_data, const double backward_length);
 Pose getBackedPose(
   const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance);
 }  // namespace behavior_path_planner::start_planner_utils

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1067,12 +1067,14 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam()
     declare_parameter<double>(ns + "collision_check_distance_from_end");
   // shift pull out
   p.enable_shift_pull_out = declare_parameter<bool>(ns + "enable_shift_pull_out");
-  p.shift_pull_out_velocity = declare_parameter<double>(ns + "shift_pull_out_velocity");
-  p.pull_out_sampling_num = declare_parameter<int>(ns + "pull_out_sampling_num");
   p.minimum_shift_pull_out_distance =
     declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
-  p.maximum_lateral_jerk = declare_parameter<double>(ns + "maximum_lateral_jerk");
-  p.minimum_lateral_jerk = declare_parameter<double>(ns + "minimum_lateral_jerk");
+  p.lateral_acceleration_sampling_num =
+    declare_parameter<int>(ns + "lateral_acceleration_sampling_num");
+  p.lateral_jerk = declare_parameter<double>(ns + "lateral_jerk");
+  p.maximum_lateral_acc = declare_parameter<double>(ns + "maximum_lateral_acc");
+  p.minimum_lateral_acc = declare_parameter<double>(ns + "minimum_lateral_acc");
+  p.maximum_curvature = declare_parameter<double>(ns + "maximum_curvature");
   p.deceleration_interval = declare_parameter<double>(ns + "deceleration_interval");
   // geometric pull out
   p.enable_geometric_pull_out = declare_parameter<bool>(ns + "enable_geometric_pull_out");
@@ -1096,10 +1098,10 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam()
   p.ignore_distance_from_lane_end = declare_parameter<double>(ns + "ignore_distance_from_lane_end");
 
   // validation of parameters
-  if (p.pull_out_sampling_num < 1) {
+  if (p.lateral_acceleration_sampling_num < 1) {
     RCLCPP_FATAL_STREAM(
-      get_logger(), "pull_out_sampling_num must be positive integer. Given parameter: "
-                      << p.pull_out_sampling_num << std::endl
+      get_logger(), "lateral_acceleration_sampling_num must be positive integer. Given parameter: "
+                      << p.lateral_acceleration_sampling_num << std::endl
                       << "Terminating the program...");
     exit(EXIT_FAILURE);
   }
@@ -1292,7 +1294,13 @@ void BehaviorPathPlannerNode::run()
   lk_manager.unlock();  // release bt_manager_
 #endif
 
-  if (output.modified_goal) {
+  // publish modified goal only when it is updated
+  if (
+    output.modified_goal &&
+    /* has changed modified goal */ (
+      !planner_data_->prev_modified_goal || tier4_autoware_utils::calcDistance2d(
+                                              planner_data_->prev_modified_goal->pose.position,
+                                              output.modified_goal->pose.position) > 0.01)) {
     PoseWithUuidStamped modified_goal = *(output.modified_goal);
     modified_goal.header.stamp = path->header.stamp;
     planner_data_->prev_modified_goal = modified_goal;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -141,17 +141,18 @@ void PlannerManager::generateCombinedDrivableArea(
   const auto & di = output.drivable_area_info;
   constexpr double epsilon = 1e-3;
 
+  const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points);
+  const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true;
+
   if (epsilon < std::abs(di.drivable_margin)) {
     // for single free space pull over
-    const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points);
-    const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true;
-
     utils::generateDrivableArea(
       *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward);
   } else if (di.is_already_expanded) {
     // for single side shift
     utils::generateDrivableArea(
-      *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data);
+      *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data,
+      is_driving_forward);
   } else {
     const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes);
 
@@ -163,7 +164,7 @@ void PlannerManager::generateCombinedDrivableArea(
     // for other modules where multiple modules may be launched
     utils::generateDrivableArea(
       *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings,
-      data->parameters.vehicle_length, data);
+      data->parameters.vehicle_length, data, is_driving_forward);
   }
 
   // extract obstacles from drivable area

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -130,7 +130,6 @@ void GoalPlannerModule::resetStatus()
   pull_over_path_candidates_.clear();
   closest_start_pose_.reset();
   goal_candidates_.clear();
-  prev_goal_id_.reset();
 }
 
 // This function is needed for waiting for planner_data_
@@ -296,7 +295,7 @@ bool GoalPlannerModule::isExecutionRequested() const
       return lanelet::utils::isInLanelet(goal_pose, current_lane);
     });
 
-  // check that goal is in current neghibor shoulder lane
+  // check that goal is in current neighbor shoulder lane
   const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() {
     lanelet::ConstLanelet neighbor_shoulder_lane{};
     for (const auto & lane : current_lanes) {
@@ -633,7 +632,6 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output)
   setDrivableAreaInfo(output);
 
   setModifiedGoal(output);
-  prev_goal_id_ = modified_goal_pose_->id;
 
   // set hazard and turn signal
   if (status_.has_decided_path) {
@@ -689,11 +687,8 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 
 void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const
 {
-  // set the modified goal only when it is updated
   const auto & route_handler = planner_data_->route_handler;
-  const bool has_changed_goal =
-    modified_goal_pose_ && (!prev_goal_id_ || *prev_goal_id_ != modified_goal_pose_->id);
-  if (status_.is_safe && has_changed_goal) {
+  if (status_.is_safe) {
     PoseWithUuidStamped modified_goal{};
     modified_goal.uuid = route_handler->getRouteUuid();
     modified_goal.pose = modified_goal_pose_->goal_pose;