fix(behavior_path_planner) fix stop path for pull_over

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Sep 2, 2022 · e1908d5 · e1908d5
1 parent fd0d776
commit e1908d5
Showing 1 changed file with 22 additions and 15 deletions.
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
@@ -723,32 +723,39 @@ PathWithLaneId PullOverModule::getReferencePath() const
 
 PathWithLaneId PullOverModule::getStopPath() 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 reference_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 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 =
+    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));