fix(behavior_velocity_run_out_module): fix slow_down jerk filter (autowarefoundation#10065)

Signed-off-by: tomoya.kimura <[email protected]>

Author: tkimura4

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp

@@ -183,11 +183,6 @@ bool RunOutModule::modifyPathVelocity(PathWithLaneId * path)
     }
   }
 
-  // apply max jerk limit if the ego can't stop with specified max jerk and acc
-  if (planner_param_.slow_down_limit.enable) {
-    applyMaxJerkLimit(current_pose, current_vel, current_acc, *path);
-  }
-
   publishDebugValue(extended_smoothed_path, filtered_obstacles, dynamic_obstacle, current_pose);
 
   // record time for collision check
@@ -745,7 +740,7 @@ std::optional<geometry_msgs::msg::Pose> RunOutModule::calcStopPoint(
 
 bool RunOutModule::insertStopPoint(
   const std::optional<geometry_msgs::msg::Pose> stop_point,
-  autoware_internal_planning_msgs::msg::PathWithLaneId & path)
+  autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double stop_point_velocity)
 {
   // no stop point
   if (!stop_point) {
@@ -769,7 +764,7 @@ bool RunOutModule::insertStopPoint(
   autoware_internal_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id;
   stop_point_with_lane_id = path.points.at(nearest_seg_idx);
   stop_point_with_lane_id.point.pose = *stop_point;
-  planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx);
+  planning_utils::insertVelocity(path, stop_point_with_lane_id, stop_point_velocity, insert_idx);
 
   planning_factor_interface_->add(
     path.points, planner_data_->current_odometry->pose, stop_point.value(), stop_point.value(),
@@ -855,7 +850,14 @@ bool RunOutModule::insertStoppingVelocity(
 {
   stopping_point =
     calcStopPoint(dynamic_obstacle, output_path, current_pose, current_vel, current_acc);
-  return insertStopPoint(stopping_point, output_path);
+
+  // apply max jerk limit if the ego can't stop with specified max jerk and acc
+  double stop_point_velocity = 0.0;
+  if (planner_param_.slow_down_limit.enable) {
+    stop_point_velocity = calcMaxJerkLimitedVelocity(
+      current_pose, current_vel, current_acc, output_path, *stopping_point);
+  }
+  return insertStopPoint(stopping_point, output_path, stop_point_velocity);
 }
 
 void RunOutModule::insertApproachingVelocity(
@@ -901,26 +903,19 @@ void RunOutModule::insertApproachingVelocity(
   planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx_stop);
 }
 
-void RunOutModule::applyMaxJerkLimit(
+double RunOutModule::calcMaxJerkLimitedVelocity(
   const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
-  PathWithLaneId & path) const
+  PathWithLaneId & path, const geometry_msgs::msg::Pose & stop_point) const
 {
-  const auto stop_point_idx = run_out_utils::findFirstStopPointIdx(path.points);
-  if (!stop_point_idx) {
-    return;
-  }
-
-  const auto stop_point = path.points.at(stop_point_idx.value()).point.pose.position;
-  const auto dist_to_stop_point =
-    autoware::motion_utils::calcSignedArcLength(path.points, current_pose.position, stop_point);
+  const auto dist_to_stop_point = autoware::motion_utils::calcSignedArcLength(
+    path.points, current_pose.position, stop_point.position);
 
   // calculate desired velocity with limited jerk
   const auto jerk_limited_vel = planning_utils::calcDecelerationVelocityFromDistanceToTarget(
     planner_param_.slow_down_limit.max_jerk, planner_param_.slow_down_limit.max_acc, current_acc,
     current_vel, dist_to_stop_point);
 
-  // overwrite velocity with limited velocity
-  run_out_utils::insertPathVelocityFromIndex(stop_point_idx.value(), jerk_limited_vel, path.points);
+  return jerk_limited_vel;
 }
 
 std::vector<DynamicObstacle> RunOutModule::excludeObstaclesCrossingEgoCutLine(

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp

@@ -127,7 +127,8 @@ class RunOutModule : public SceneModuleInterface
 
   bool insertStopPoint(
     const std::optional<geometry_msgs::msg::Pose> stop_point,
-    autoware_internal_planning_msgs::msg::PathWithLaneId & path);
+    autoware_internal_planning_msgs::msg::PathWithLaneId & path,
+    const double stop_point_velocity = 0.0);
 
   void insertVelocityForState(
     const std::optional<DynamicObstacle> & dynamic_obstacle, const PlannerData planner_data,
@@ -148,9 +149,9 @@ class RunOutModule : public SceneModuleInterface
     const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose,
     const float approaching_vel, const float approach_margin, PathWithLaneId & output_path);
 
-  void applyMaxJerkLimit(
+  double calcMaxJerkLimitedVelocity(
     const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
-    PathWithLaneId & path) const;
+    PathWithLaneId & path, const geometry_msgs::msg::Pose & stop_point) const;
 
   /**
    * @brief Creates a virtual line segment that is perpendicular to the ego vehicle and that passes