feat(obstacle_cruise_planner): add jerk and acc limits for slow-down

planning/obstacle_cruise_planner/config/default_common.param.yaml

@@ -7,6 +7,10 @@
       min_jerk: -1.0        # min jerk [m/sss]
       max_jerk: 1.0         # max jerk [m/sss]
 
+    slow_down:
+      min_acc: -1.0         # min slowdown deceleration [m/ss]
+      min_jerk: -1.0        # min slowdown jerk [m/sss]
+
     # constraints to be observed
     limit:
       min_acc: -2.5         # min deceleration limit [m/ss]

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -166,6 +166,8 @@ struct LongitudinalInfo
     limit_min_accel = node.declare_parameter<double>("limit.min_acc");
     limit_max_jerk = node.declare_parameter<double>("limit.max_jerk");
     limit_min_jerk = node.declare_parameter<double>("limit.min_jerk");
+    slow_down_min_accel = node.declare_parameter<double>("slow_down.min_acc");
+    slow_down_min_jerk = node.declare_parameter<double>("slow_down.min_jerk");
 
     idling_time = node.declare_parameter<double>("common.idling_time");
     min_ego_accel_for_rss = node.declare_parameter<double>("common.min_ego_accel_for_rss");
@@ -191,6 +193,9 @@ struct LongitudinalInfo
     tier4_autoware_utils::updateParam<double>(parameters, "limit.min_accel", limit_min_accel);
     tier4_autoware_utils::updateParam<double>(parameters, "limit.max_jerk", limit_max_jerk);
     tier4_autoware_utils::updateParam<double>(parameters, "limit.min_jerk", limit_min_jerk);
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "slow_down.min_accel", slow_down_min_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "slow_down.min_jerk", slow_down_min_jerk);
 
     tier4_autoware_utils::updateParam<double>(parameters, "common.idling_time", idling_time);
     tier4_autoware_utils::updateParam<double>(
@@ -214,6 +219,8 @@ struct LongitudinalInfo
   double min_accel;
   double max_jerk;
   double min_jerk;
+  double slow_down_min_jerk;
+  double slow_down_min_accel;
   double limit_max_accel;
   double limit_min_accel;
   double limit_max_jerk;

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -257,156 +257,156 @@
     "stop planning");
 
   // Get Closest Stop Obstacle
   const auto closest_stop_obstacle =
     obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj_points, stop_obstacles);
   if (!closest_stop_obstacle) {
     // delete marker
     const auto markers =
       motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
     tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
 
     prev_stop_distance_info_ = std::nullopt;
     return planner_data.traj_points;
   }
 
   // Get Closest Obstacle Stop Distance
   const double closest_obstacle_dist = motion_utils::calcSignedArcLength(
     planner_data.traj_points, 0, closest_stop_obstacle->collision_point);
 
   const auto ego_segment_idx =
     ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
   const auto negative_dist_to_ego = motion_utils::calcSignedArcLength(
     planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0);
   const double dist_to_ego = -negative_dist_to_ego;
 
   const double margin_from_obstacle =
     calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle);
 
   // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin
   // we set closest_obstacle_stop_distance to closest_behavior_stop_distance
   const double margin_from_obstacle_considering_behavior_module = [&]() {
     const size_t nearest_segment_idx =
       findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
     const auto closest_behavior_stop_idx =
       motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1);
 
     if (!closest_behavior_stop_idx) {
       return margin_from_obstacle;
     }
 
     const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength(
       planner_data.traj_points, planner_data.ego_pose.position, nearest_segment_idx,
       *closest_behavior_stop_idx);
 
     if (*closest_behavior_stop_idx == planner_data.traj_points.size() - 1) {
       // Closest behavior stop point is the end point
       const double closest_obstacle_stop_dist_from_ego =
         closest_obstacle_dist - dist_to_ego - longitudinal_info_.terminal_safe_distance_margin -
         abs_ego_offset;
       const double stop_dist_diff =
         closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
       if (stop_dist_diff < margin_from_obstacle) {
         // Use terminal margin (terminal_safe_distance_margin) for obstacle stop
         return longitudinal_info_.terminal_safe_distance_margin;
       }
     } else {
       const double closest_obstacle_stop_dist_from_ego =
         closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset;
       const double stop_dist_diff =
         closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
       if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) {
         // Use shorter margin (min_behavior_stop_margin) for obstacle stop
         return min_behavior_stop_margin_;
       }
     }
     return margin_from_obstacle;
   }();
 
   const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() {
     // Check feasibility to stop
     if (suppress_sudden_obstacle_stop_) {
       const double closest_obstacle_stop_dist =
         closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset;
 
       // Calculate feasible stop margin (Check the feasibility)
       const double feasible_stop_dist = calcMinimumDistanceToStop(
                                           planner_data.ego_vel, longitudinal_info_.limit_max_accel,
                                           longitudinal_info_.limit_min_accel) +
                                         dist_to_ego;
 
       if (closest_obstacle_stop_dist < feasible_stop_dist) {
         const auto feasible_margin_from_obstacle =
           margin_from_obstacle_considering_behavior_module -
           (feasible_stop_dist - closest_obstacle_stop_dist);
         return std::make_pair(feasible_margin_from_obstacle, true);
       }
     }
     return std::make_pair(margin_from_obstacle_considering_behavior_module, false);
   }();
 
   // Generate Output Trajectory
   const double zero_vel_dist = [&]() {
     const double current_zero_vel_dist =
       std::max(0.0, closest_obstacle_dist - abs_ego_offset - stop_margin_from_obstacle);
 
     // Hold previous stop distance if necessary
     if (
       std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold &&
       prev_stop_distance_info_) {
       // NOTE: We assume that the current trajectory's front point is ahead of the previous
       // trajectory's front point.
       const size_t traj_front_point_prev_seg_idx =
         motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
           prev_stop_distance_info_->first, planner_data.traj_points.front().pose);
       const double diff_dist_front_points = motion_utils::calcSignedArcLength(
         prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position,
         traj_front_point_prev_seg_idx);
 
       const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
       if (
         std::abs(prev_zero_vel_dist - current_zero_vel_dist) <
         longitudinal_info_.hold_stop_distance_threshold) {
         return prev_zero_vel_dist;
       }
     }
 
     return current_zero_vel_dist;
   }();
 
   // Insert stop point
   auto output_traj_points = planner_data.traj_points;
   const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points);
   if (zero_vel_idx) {
     // virtual wall marker for stop obstacle
     const auto markers = motion_utils::createStopVirtualWallMarker(
       output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0,
       abs_ego_offset, "", planner_data.is_driving_forward);
     tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
     debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle);
 
     // Publish Stop Reason
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
     const auto stop_reasons_msg =
       makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle);
     stop_reasons_pub_->publish(stop_reasons_msg);
     velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose));
 
     // Publish if ego vehicle collides with the obstacle with a limit acceleration
     const auto stop_speed_exceeded_msg =
       createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle);
     stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg);
 
     prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist);
   }
 
   stop_planning_debug_info_.set(
     StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE,
     closest_obstacle_dist - abs_ego_offset);  // TODO(murooka)
   stop_planning_debug_info_.set(
     StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity);
 
   stop_planning_debug_info_.set(
     StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, stop_margin_from_obstacle);
   stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0);
   stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0);
 
@@ -790,8 +790,8 @@
       }
       // TODO(murooka) Calculate more precisely. Final acceleration should be zero.
       const double min_feasible_slow_down_vel = calcDecelerationVelocityFromDistanceToTarget(
-        longitudinal_info_.min_jerk, longitudinal_info_.min_accel, planner_data.ego_acc,
-        planner_data.ego_vel, deceleration_dist);
+        longitudinal_info_.slow_down_min_jerk, longitudinal_info_.slow_down_min_accel,
+        planner_data.ego_acc, planner_data.ego_vel, deceleration_dist);
       return min_feasible_slow_down_vel;
     }();
     if (prev_output) {