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

…utowarefoundation#5810) Add jerk and acc limits for slow-down Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp> Signed-off-by: karishma <karishma@interpl.ai>
Interplai · Dec 19, 2023 · c6a915a · c6a915a
1 parent 2e6139e
commit c6a915a
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 2 deletions.
diff --git a/planning/obstacle_cruise_planner/config/default_common.param.yaml b/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]

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
@@ -172,6 +172,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");
@@ -197,6 +199,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>(
@@ -220,6 +225,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;

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -763,8 +763,8 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints(
       }
       // 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) {