fix(behavior_path_planner): fix stop path for pull_over (autowarefoun…

…dation#1742) * fix(behavior_path_planner): fix stop path for pull_over Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * use previous stop path when is has been generated Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
boyali · Oct 19, 2022 · dfafcde · dfafcde
1 parent 4685a1c
commit dfafcde
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 24 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 previous 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,45 +731,43 @@ PathWithLaneId PullOverModule::getReferencePath() const
   return reference_path;
 }
 
-PathWithLaneId PullOverModule::getStopPath() const
+PathWithLaneId PullOverModule::generateStopPath() const
 {
-  PathWithLaneId reference_path;
-
   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 stop_path = util::getCenterLinePath(
+    *route_handler, status_.current_lanes, current_pose, common_parameters.backward_path_length,
+    common_parameters.forward_path_length, common_parameters);
+
   const double current_to_stop_distance =
     std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2;
-  const auto arclength_current_pose =
+  const double arclength_current_pose =
     lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length;
 
-  reference_path = util::getCenterLinePath(
-    *route_handler, status_.current_lanes, current_pose, common_parameters.backward_path_length,
-    common_parameters.forward_path_length, common_parameters);
-
-  for (auto & point : reference_path.points) {
-    const auto arclength =
+  for (auto & point : stop_path.points) {
+    const double arclength =
       lanelet::utils::getArcCoordinates(status_.current_lanes, point.point.pose).length;
-    const auto arclength_current_to_point = arclength - arclength_current_pose;
-    if (0 < arclength_current_to_point) {
-      point.point.longitudinal_velocity_mps = std::min(
-        point.point.longitudinal_velocity_mps,
-        static_cast<float>(std::max(
-          0.0, current_vel * (current_to_stop_distance - arclength_current_to_point) /
-                 current_to_stop_distance)));
+    const double arclength_current_to_point = arclength - arclength_current_pose;
+    if (0.0 < arclength_current_to_point) {
+      point.point.longitudinal_velocity_mps = std::clamp(
+        static_cast<float>(
+          current_vel * (current_to_stop_distance - arclength_current_to_point) /
+          current_to_stop_distance),
+        0.0f, point.point.longitudinal_velocity_mps);
     } else {
       point.point.longitudinal_velocity_mps =
         std::min(point.point.longitudinal_velocity_mps, static_cast<float>(current_vel));
     }
   }
 
-  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