use previous stop path when is has been generated

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Sep 2, 2022 · 257bb02 · 257bb02
1 parent e1908d5
commit 257bb02
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 19 deletions.
diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
@@ -108,12 +108,14 @@ enum PathType {
 struct PUllOverStatus
 {
   PathWithLaneId path;
+  std::shared_ptr<PathWithLaneId> prev_stop_path = nullptr;
   lanelet::ConstLanelets current_lanes;
   lanelet::ConstLanelets pull_over_lanes;
   lanelet::ConstLanelets lanes;  // current + pull_over
   bool has_decided_path = false;
   int path_type = PathType::NONE;
   bool is_safe = false;
+  bool prev_is_safe = false;
   bool has_decided_velocity = false;
   bool has_requested_approval_ = false;
 };
@@ -187,7 +189,7 @@ class PullOverModule : public SceneModuleInterface
   std::unique_ptr<rclcpp::Time> last_approved_time_;
 
   PathWithLaneId getReferencePath() const;
-  PathWithLaneId getStopPath() const;
+  PathWithLaneId generateStopPath() const;
   lanelet::ConstLanelets getPullOverLanes() const;
   std::pair<bool, bool> getSafePath(ShiftParkingPath & safe_path) const;
   Pose getRefinedGoal() const;

diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
@@ -570,8 +570,18 @@ BehaviorModuleOutput PullOverModule::plan()
   }
 
   BehaviorModuleOutput output;
-  output.path = status_.is_safe ? std::make_shared<PathWithLaneId>(status_.path)
-                                : std::make_shared<PathWithLaneId>(getStopPath());
+
+  // safe: use pull over path
+  if (status_.is_safe) {
+    output.path = std::make_shared<PathWithLaneId>(status_.path);
+  } else if (status_.prev_is_safe || status_.prev_stop_path == nullptr) {
+    // safe -> not_safe or no prev_stop_path: generate new stop_path
+    output.path = std::make_shared<PathWithLaneId>(generateStopPath());
+    status_.prev_stop_path = output.path;
+  } else {  // not_safe -> not_safe: use previout stop path
+    output.path = status_.prev_stop_path;
+  }
+  status_.prev_is_safe = status_.is_safe;
 
   // set hazard and turn signal
   if (status_.has_decided_path) {
@@ -696,7 +706,7 @@ PathWithLaneId PullOverModule::getReferencePath() const
 
   // stop pose is behind current pose
   if (s_forward < s_backward) {
-    return getStopPath();
+    return generateStopPath();
   }
 
   PathWithLaneId reference_path =
@@ -721,32 +731,23 @@ PathWithLaneId PullOverModule::getReferencePath() const
   return reference_path;
 }
 
-PathWithLaneId PullOverModule::getStopPath() const
+PathWithLaneId PullOverModule::generateStopPath() const
 {
   const auto & route_handler = planner_data_->route_handler;
   const auto current_pose = planner_data_->self_pose->pose;
   const auto common_parameters = planner_data_->parameters;
 
   const double current_vel = planner_data_->self_odometry->twist.twist.linear.x;
-  auto reference_path = util::getCenterLinePath(
+  auto stop_path = util::getCenterLinePath(
     *route_handler, status_.current_lanes, current_pose, common_parameters.backward_path_length,
     common_parameters.forward_path_length, common_parameters);
 
-  // stop immediately if current_vel is less than min_vel
-  constexpr double min_vel = 1.0;
-  if (current_vel < min_vel) {
-    for (auto & point : reference_path.points) {
-      point.point.longitudinal_velocity_mps = 0.0;
-    }
-    return reference_path;
-  }
-
   const double current_to_stop_distance =
     std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2;
   const double arclength_current_pose =
     lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length;
 
-  for (auto & point : reference_path.points) {
+  for (auto & point : stop_path.points) {
     const double arclength =
       lanelet::utils::getArcCoordinates(status_.current_lanes, point.point.pose).length;
     const double arclength_current_to_point = arclength - arclength_current_pose;
@@ -762,11 +763,11 @@ PathWithLaneId PullOverModule::getStopPath() const
     }
   }
 
-  reference_path.drivable_area = util::generateDrivableArea(
-    reference_path, status_.current_lanes, common_parameters.drivable_area_resolution,
+  stop_path.drivable_area = util::generateDrivableArea(
+    stop_path, status_.current_lanes, common_parameters.drivable_area_resolution,
     common_parameters.vehicle_length, planner_data_);
 
-  return reference_path;
+  return stop_path;
 }
 
 lanelet::ConstLanelets PullOverModule::getPullOverLanes() const