feat(vehicle_cmd_gate)!: add steer and steer_rate filter

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -15,6 +15,8 @@
     nominal:
       vel_lim: 25.0
       reference_speed_points: [20.0, 30.0]
+      steer_lim: [1.0, 0.8]
+      steer_rate_lim: [1.0, 0.8]
       lon_acc_lim: [5.0, 4.0]
       lon_jerk_lim: [5.0, 4.0]
       lat_acc_lim: [5.0, 4.0]
@@ -23,6 +25,8 @@
     on_transition:
       vel_lim: 50.0
       reference_speed_points: [20.0, 30.0]
+      steer_lim: [1.0, 0.8]
+      steer_rate_lim: [1.0, 0.8]
       lon_acc_lim: [1.0, 0.9]
       lon_jerk_lim: [0.5, 0.4]
       lat_acc_lim: [2.0, 1.8]

control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

@@ -29,7 +29,8 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam &
   const auto s = p.reference_speed_points.size();
   if (
     p.lon_acc_lim.size() != s || p.lon_jerk_lim.size() != s || p.lat_acc_lim.size() != s ||
-    p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s) {
+    p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s || p.steer_lim.size() != s ||
+    p.steer_rate_lim.size() != s) {
     std::cerr << "VehicleCmdFilter::setParam() There is a size mismatch in the parameter. "
                  "Parameter initialization failed."
               << std::endl;
@@ -39,7 +40,18 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam &
   param_ = p;
   return true;
 }
-
+void VehicleCmdFilter::setSteerLim(LimitArray v)
+{
+  auto tmp = param_;
+  tmp.steer_lim = v;
+  setParameterWithValidation(tmp);
+}
+void VehicleCmdFilter::setSteerRateLim(LimitArray v)
+{
+  auto tmp = param_;
+  tmp.steer_rate_lim = v;
+  setParameterWithValidation(tmp);
+}
 void VehicleCmdFilter::setLonAccLim(LimitArray v)
 {
   auto tmp = param_;
@@ -151,12 +163,36 @@ void VehicleCmdFilter::limitActualSteerDiff(
 
 void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const
 {
-  // TODO(Horibe): parametrize the max steering angle.
-  // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration
-  // calculation does not support bigger steering value than PI/2 due to tan/atan calculation.
-  constexpr float steer_limit = M_PI_2;
+  const float steer_limit = getSteerLim();
+
   input.lateral.steering_tire_angle =
     std::clamp(input.lateral.steering_tire_angle, -steer_limit, steer_limit);
+
+  // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration
+  // calculation does not support bigger steering value than PI/2 due to tan/atan calculation.
+  if (std::abs(input.lateral.steering_tire_angle) > M_PI_2f) {
+    std::cerr << "[vehicle_Cmd_gate] limitLateralSteer(): steering limit is set to pi/2 since the "
+                 "current filtering logic can not handle the steering larger than pi/2. Please "
+                 "check the steering angle limit."
+              << std::endl;
+
+    std::clamp(input.lateral.steering_tire_angle, -M_PI_2f, M_PI_2f);
+  }
+}
+
+void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCommand & input) const
+{
+  const float steer_rate_limit = getSteerRateLim();
+
+  // for steering angle rate
+  input.lateral.steering_tire_rotation_rate =
+    std::clamp(input.lateral.steering_tire_rotation_rate, -steer_rate_limit, steer_rate_limit);
+
+  // for steering angle
+  const float steer_diff_limit = steer_rate_limit * dt;
+  float ds = input.lateral.steering_tire_angle - prev_cmd_.lateral.steering_tire_angle;
+  ds = std::clamp(ds, -steer_diff_limit, steer_diff_limit);
+  input.lateral.steering_tire_angle = prev_cmd_.lateral.steering_tire_angle + ds;
 }
 
 void VehicleCmdFilter::filterAll(
@@ -165,6 +201,7 @@ void VehicleCmdFilter::filterAll(
 {
   const auto cmd_orig = cmd;
   limitLateralSteer(cmd);
+  limitLateralSteerRate(dt, cmd);
   limitLongitudinalWithJerk(dt, cmd);
   limitLongitudinalWithAcc(dt, cmd);
   limitLongitudinalWithVel(cmd);
@@ -267,6 +304,14 @@ double VehicleCmdFilter::getLatJerkLim() const
 {
   return interpolateFromSpeed(param_.lat_jerk_lim);
 }
+double VehicleCmdFilter::getSteerLim() const
+{
+  return interpolateFromSpeed(param_.steer_lim);
+}
+double VehicleCmdFilter::getSteerRateLim() const
+{
+  return interpolateFromSpeed(param_.steer_rate_lim);
+}
 double VehicleCmdFilter::getSteerDiffLim() const
 {
   return interpolateFromSpeed(param_.actual_steer_diff_lim);

control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp

@@ -37,6 +37,8 @@ struct VehicleCmdFilterParam
   LimitArray lon_jerk_lim;
   LimitArray lat_acc_lim;
   LimitArray lat_jerk_lim;
+  LimitArray steer_lim;
+  LimitArray steer_rate_lim;
   LimitArray actual_steer_diff_lim;
 };
 class VehicleCmdFilter
@@ -47,6 +49,8 @@ class VehicleCmdFilter
 
   void setWheelBase(double v) { param_.wheel_base = v; }
   void setVelLim(double v) { param_.vel_lim = v; }
+  void setSteerLim(LimitArray v);
+  void setSteerRateLim(LimitArray v);
   void setLonAccLim(LimitArray v);
   void setLonJerkLim(LimitArray v);
   void setLatAccLim(LimitArray v);
@@ -64,6 +68,7 @@ class VehicleCmdFilter
   void limitActualSteerDiff(
     const double current_steer_angle, AckermannControlCommand & input) const;
   void limitLateralSteer(AckermannControlCommand & input) const;
+  void limitLateralSteerRate(const double dt, AckermannControlCommand & input) const;
   void filterAll(
     const double dt, const double current_steer_angle, AckermannControlCommand & input,
     IsFilterActivated & is_activated) const;
@@ -90,6 +95,8 @@ class VehicleCmdFilter
   double getLonJerkLim() const;
   double getLatAccLim() const;
   double getLatJerkLim() const;
+  double getSteerLim() const;
+  double getSteerRateLim() const;
   double getSteerDiffLim() const;
 };
 }  // namespace vehicle_cmd_gate

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -169,6 +169,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     p.vel_lim = declare_parameter<double>("nominal.vel_lim");
     p.reference_speed_points =
       declare_parameter<std::vector<double>>("nominal.reference_speed_points");
+    p.steer_lim = declare_parameter<std::vector<double>>("nominal.steer_lim");
+    p.steer_rate_lim = declare_parameter<std::vector<double>>("nominal.steer_rate_lim");
     p.lon_acc_lim = declare_parameter<std::vector<double>>("nominal.lon_acc_lim");
     p.lon_jerk_lim = declare_parameter<std::vector<double>>("nominal.lon_jerk_lim");
     p.lat_acc_lim = declare_parameter<std::vector<double>>("nominal.lat_acc_lim");
@@ -184,6 +186,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     p.vel_lim = declare_parameter<double>("on_transition.vel_lim");
     p.reference_speed_points =
       declare_parameter<std::vector<double>>("on_transition.reference_speed_points");
+    p.steer_lim = declare_parameter<std::vector<double>>("on_transition.steer_lim");
+    p.steer_rate_lim = declare_parameter<std::vector<double>>("on_transition.steer_rate_lim");
     p.lon_acc_lim = declare_parameter<std::vector<double>>("on_transition.lon_acc_lim");
     p.lon_jerk_lim = declare_parameter<std::vector<double>>("on_transition.lon_jerk_lim");
     p.lat_acc_lim = declare_parameter<std::vector<double>>("on_transition.lat_acc_lim");

control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp

@@ -45,6 +45,8 @@ void print_values(int i, const T1 & name, const T2 & a, const T3 &... b)
 
 // global params
 const std::vector<double> reference_speed_points = {5., 10., 15., 20.};
+const std::vector<double> steer_lim = {0.5, 0.3, 0.2, 0.1};
+const std::vector<double> steer_rate_lim = {0.5, 0.3, 0.2, 0.1};
 const std::vector<double> lon_acc_lim = {1.5, 1.0, 0.8, 0.6};
 const std::vector<double> lon_jerk_lim = {1.4, 0.9, 0.7, 0.5};
 const std::vector<double> lat_acc_lim = {2.0, 1.6, 1.2, 0.8};
@@ -336,6 +338,8 @@ std::shared_ptr<VehicleCmdGate> generateNode()
   node_options.append_parameter_override("wheel_base", wheelbase);
   override("nominal.reference_speed_points", reference_speed_points);
   override("nominal.reference_speed_points", reference_speed_points);
+  override("nominal.steer_lim", steer_lim);
+  override("nominal.steer_rate_lim", steer_rate_lim);
   override("nominal.lon_acc_lim", lon_acc_lim);
   override("nominal.lon_jerk_lim", lon_jerk_lim);
   override("nominal.lat_acc_lim", lat_acc_lim);

control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

@@ -31,12 +31,15 @@ constexpr double NOMINAL_INTERVAL = 1.0;
 
 void setFilterParams(
   vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a,
-  LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, const double wheelbase)
+  LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, LimitArray steer_lim,
+  LimitArray steer_rate_lim, const double wheelbase)
 {
   vehicle_cmd_gate::VehicleCmdFilterParam p;
   p.vel_lim = v;
   p.wheel_base = wheelbase;
   p.reference_speed_points = speed_points;
+  p.steer_lim = steer_lim;
+  p.steer_rate_lim = steer_rate_lim;
   p.lat_acc_lim = lat_a;
   p.lat_jerk_lim = lat_j;
   p.lon_acc_lim = a;
@@ -97,16 +100,17 @@ double calcLatJerk(
 
 void test_1d_limit(
   double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM,
-  double STEER_DIFF, const AckermannControlCommand & prev_cmd,
-  const AckermannControlCommand & raw_cmd)
+  double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM,
+  const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd)
 {
   const double WHEELBASE = 3.0;
   const double DT = 0.1;  // [s]
 
   vehicle_cmd_gate::VehicleCmdFilter filter;
   filter.setCurrentSpeed(ego_v);
   setFilterParams(
-    filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE);
+    filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, {STEER_LIM},
+    {STEER_RATE_LIM}, WHEELBASE);
   filter.setPrevCmd(prev_cmd);
 
   // velocity filter
@@ -233,6 +237,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilter)
   const std::vector<double> lat_a_arr = {0.01, 1.0, 100.0};
   const std::vector<double> lat_j_arr = {0.01, 1.0, 100.0};
   const std::vector<double> steer_diff_arr = {0.01, 1.0, 100.0};
+  const std::vector<double> steer_lim_arr = {0.01, 1.0, 100.0};
+  const std::vector<double> steer_rate_lim_arr = {0.01, 1.0, 100.0};
   const std::vector<double> ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0};
 
   const std::vector<AckermannControlCommand> prev_cmd_arr = {
@@ -249,8 +255,13 @@ TEST(VehicleCmdFilter, VehicleCmdFilter)
             for (const auto & prev_cmd : prev_cmd_arr) {
               for (const auto & raw_cmd : raw_cmd_arr) {
                 for (const auto & steer_diff : steer_diff_arr) {
-                  for (const auto & ego_v : ego_v_arr) {
-                    test_1d_limit(ego_v, v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd);
+                  for (const auto & steer : steer_lim_arr) {
+                    for (const auto & steer_rate : steer_rate_lim_arr) {
+                      for (const auto & ego_v : ego_v_arr) {
+                        test_1d_limit(
+                          ego_v, v, a, j, la, lj, steer_diff, steer, steer_rate, prev_cmd, raw_cmd);
+                      }
+                    }
                   }
                 }
               }
@@ -271,6 +282,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
   p.wheel_base = WHEELBASE;
   p.vel_lim = 20.0;
   p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
+  p.steer_lim = std::vector<double>{0.1, 0.2, 0.3};
+  p.steer_rate_lim = std::vector<double>{0.2, 0.1, 0.05};
   p.lon_acc_lim = std::vector<double>{0.3, 0.4, 0.5};
   p.lon_jerk_lim = std::vector<double>{0.4, 0.4, 0.7};
   p.lat_acc_lim = std::vector<double>{0.1, 0.2, 0.3};
@@ -293,7 +306,16 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
   const auto set_speed_and_reset_prev = [&](const auto & current_vel) {
     filter.setCurrentSpeed(current_vel);
   };
-
+  const auto _limitSteer = [&](const auto & in) {
+    auto out = in;
+    filter.limitLateralSteer(out);
+    return out;
+  };
+  const auto _limitSteerRate = [&](const auto & in) {
+    auto out = in;
+    filter.limitLateralSteerRate(DT, out);
+    return out;
+  };
   const auto _limitLongitudinalWithVel = [&](const auto & in) {
     auto out = in;
     filter.limitLongitudinalWithVel(out);
@@ -334,6 +356,77 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
     EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep);
   }
 
+  // steer angle lim
+  // p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
+  // p.steer_lim = std::vector<double>{0.1, 0.2, 0.3};
+  {
+    set_speed_and_reset_prev(0.0);
+    EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.1, ep);
+
+    set_speed_and_reset_prev(2.0);
+    EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.1, ep);
+
+    set_speed_and_reset_prev(3.0);
+    EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.15, ep);
+
+    set_speed_and_reset_prev(5.0);
+    EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.2 + 0.1 / 6.0, ep);
+
+    set_speed_and_reset_prev(8.0);
+    EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.2 + 0.1 * 4.0 / 6.0, ep);
+
+    set_speed_and_reset_prev(10.0);
+    EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.3, ep);
+
+    set_speed_and_reset_prev(15.0);
+    EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.3, ep);
+  }
+
+  // steer angle rate lim
+  // p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
+  // p.steer_rate_lim = std::vector<double>{0.2, 0.1, 0.05};
+  {
+    const auto calcSteerRateFromAngle = [&](const auto & cmd) {
+      return (cmd.steering_tire_angle - 0.0) / DT;
+    };
+    autoware_auto_control_msgs::msg::AckermannLateralCommand filtered;
+
+    set_speed_and_reset_prev(0.0);
+    filtered = _limitSteerRate(orig_cmd).lateral;
+    EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.2, ep);
+    EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.2, ep);
+
+    set_speed_and_reset_prev(2.0);
+    filtered = _limitSteerRate(orig_cmd).lateral;
+    EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.2, ep);
+    EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.2, ep);
+
+    set_speed_and_reset_prev(3.0);
+    filtered = _limitSteerRate(orig_cmd).lateral;
+    EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.15, ep);
+    EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.15, ep);
+
+    set_speed_and_reset_prev(5.0);
+    filtered = _limitSteerRate(orig_cmd).lateral;
+    EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.1 - 0.05 * 1.0 / 6.0, ep);
+    EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.1 - 0.05 * 1.0 / 6.0, ep);
+
+    set_speed_and_reset_prev(8.0);
+    filtered = _limitSteerRate(orig_cmd).lateral;
+    EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.1 - 0.05 * 4.0 / 6.0, ep);
+    EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.1 - 0.05 * 4.0 / 6.0, ep);
+
+    set_speed_and_reset_prev(10.0);
+    filtered = _limitSteerRate(orig_cmd).lateral;
+    EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.05, ep);
+    EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.05, ep);
+
+    set_speed_and_reset_prev(15.0);
+    filtered = _limitSteerRate(orig_cmd).lateral;
+    EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.05, ep);
+    EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.05, ep);
+  }
+
   // longitudinal acc lim
   {
     set_speed_and_reset_prev(0.0);