Merge pull request #1 from tier4/beta/v0.5.0-emergency-jerk

Change of allowable jerk during emergency
tier4 · Jan 12, 2023 · 97057fb · 97057fb
2 parents b76a79a + c92ad0e
commit 97057fb
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 3 deletions.
diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp
@@ -179,6 +179,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
   // jerk limit
   float64_t m_max_jerk;
   float64_t m_min_jerk;
+  float64_t smooth_stop_acc;
+  float64_t emergency_max_jerk;
 
   // slope compensation
   bool8_t m_use_traj_for_pitch;

diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp
@@ -145,6 +145,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
     m_smooth_stop.setParams(
       max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel,
       min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist);
+    smooth_stop_acc = min_strong_acc;
   }
 
   // parameters for stop state
@@ -170,6 +171,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
   // parameters for jerk limit
   m_max_jerk = node_->declare_parameter<float64_t>("max_jerk");  // [m/s^3]
   m_min_jerk = node_->declare_parameter<float64_t>("min_jerk");  // [m/s^3]
+  emergency_max_jerk = node_->declare_parameter<float64_t>("emergency_max_jerk");  // [m/s^3]
 
   // parameters for slope compensation
   m_use_traj_for_pitch = node_->declare_parameter<bool8_t>("use_trajectory_for_pitch_calculation");
@@ -342,6 +344,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
   // jerk limit
   update_param("max_jerk", m_max_jerk);
   update_param("min_jerk", m_min_jerk);
+  update_param("emergency_max_jerk", emergency_max_jerk);
 
   // slope compensation
   update_param("max_pitch_rad", m_max_pitch_rad);
@@ -604,8 +607,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
     // This acceleration is without slope compensation
     const auto & p = m_stopped_state_params;
     raw_ctrl_cmd.vel = p.vel;
-    raw_ctrl_cmd.acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter(
-      p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk);
+    raw_ctrl_cmd.acc = 0.0;
 
     RCLCPP_DEBUG(
       node_->get_logger(), "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc);
@@ -740,8 +742,14 @@ float64_t PidLongitudinalController::calcFilteredAcc(
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered);
 
   // This jerk filter must be applied after slope compensation
+  double max_jerk;
+  if (raw_acc > smooth_stop_acc){
+    max_jerk = m_max_jerk;
+  }else{
+    max_jerk = emergency_max_jerk;
+  }
   const float64_t acc_jerk_filtered = trajectory_follower::longitudinal_utils::applyDiffLimitFilter(
-    acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk);
+    acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, max_jerk, m_min_jerk);
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered);
 
   return acc_jerk_filtered;

diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml
@@ -66,6 +66,7 @@
     # jerk limit
     max_jerk: 2.0
     min_jerk: -5.0
+    emergency_max_jerk: 1.0
 
     # pitch
     use_trajectory_for_pitch_calculation: false