fix(simple_sensor_sumulator): hard-copy source code of SimModelDelayS…

…teerAccGearedWoFallGuard from simple_planning_simulator from autoware.universe

simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp

@@ -37,14 +37,15 @@ class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface
    * @param [in] steer_delay time delay for steering command [s]
    * @param [in] steer_time_constant time constant for 1D model of steering dynamics
    * @param [in] steer_dead_band dead band for steering angle [rad]
+   * @param [in] steer_bias steering bias [rad]
    * @param [in] debug_acc_scaling_factor scaling factor for accel command
    * @param [in] debug_steer_scaling_factor scaling factor for steering command
    */
   SimModelDelaySteerAccGearedWoFallGuard(
     double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
     double dt, double acc_delay, double acc_time_constant, double steer_delay,
-    double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor,
-    double debug_steer_scaling_factor);
+    double steer_time_constant, double steer_dead_band, double steer_bias,
+    double debug_acc_scaling_factor, double debug_steer_scaling_factor);
 
   /**
    * @brief default destructor
@@ -60,8 +61,8 @@ class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface
     YAW,
     VX,
     STEER,
-    PEDAL_ACCX,
     ACCX,
+    PEDAL_ACCX,
   };
   enum IDX_U { PEDAL_ACCX_DES = 0, GEAR, SLOPE_ACCX, STEER_DES };
 
@@ -78,6 +79,7 @@ class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface
   const double steer_delay_;                 //!< @brief time delay for steering command [s]
   const double steer_time_constant_;         //!< @brief time constant for steering dynamics
   const double steer_dead_band_;             //!< @brief dead band for steering angle [rad]
+  const double steer_bias_;                  //!< @brief steering angle bias [rad]
   const double debug_acc_scaling_factor_;    //!< @brief scaling factor for accel command
   const double debug_steer_scaling_factor_;  //!< @brief scaling factor for steering command
 

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -121,8 +121,9 @@ auto EgoEntitySimulation::makeSimulationModel(
   const auto acceleration_map_path      = get_parameter("acceleration_map_path",       std::string(""));
   const auto debug_acc_scaling_factor   = get_parameter("debug_acc_scaling_factor",    1.0);
   const auto debug_steer_scaling_factor = get_parameter("debug_steer_scaling_factor",  1.0);
-  const auto steer_lim                  = get_parameter("steer_lim",                   parameters.axles.front_axle.max_steering);  // 1.0
+  const auto steer_bias                 = get_parameter("steer_bias",                  0.0);
   const auto steer_dead_band            = get_parameter("steer_dead_band",             0.0);
+  const auto steer_lim                  = get_parameter("steer_lim",                   parameters.axles.front_axle.max_steering);  // 1.0
   const auto steer_rate_lim             = get_parameter("steer_rate_lim",              5.0);
   const auto steer_time_constant        = get_parameter("steer_time_constant",         0.27);
   const auto steer_time_delay           = get_parameter("steer_time_delay",            0.24);
@@ -149,7 +150,7 @@ auto EgoEntitySimulation::makeSimulationModel(
     case VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD:
       return std::make_shared<SimModelDelaySteerAccGearedWoFallGuard>(
         vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheel_base, step_time, acc_time_delay,
-        acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band,
+        acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, steer_bias,
         debug_acc_scaling_factor, debug_steer_scaling_factor);
 
     case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:

simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp

@@ -19,8 +19,8 @@
 SimModelDelaySteerAccGearedWoFallGuard::SimModelDelaySteerAccGearedWoFallGuard(
   double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
   double dt, double acc_delay, double acc_time_constant, double steer_delay,
-  double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor,
-  double debug_steer_scaling_factor)
+  double steer_time_constant, double steer_dead_band, double steer_bias,
+  double debug_acc_scaling_factor, double debug_steer_scaling_factor)
 : SimModelInterface(7 /* dim x */, 4 /* dim u */),
   MIN_TIME_CONSTANT(0.03),
   vx_lim_(vx_lim),
@@ -33,6 +33,7 @@ SimModelDelaySteerAccGearedWoFallGuard::SimModelDelaySteerAccGearedWoFallGuard(
   steer_delay_(steer_delay),
   steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)),
   steer_dead_band_(steer_dead_band),
+  steer_bias_(steer_bias),
   debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)),
   debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0))
 {
@@ -56,7 +57,11 @@ double SimModelDelaySteerAccGearedWoFallGuard::getWz()
   return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_;
 }
 
-double SimModelDelaySteerAccGearedWoFallGuard::getSteer() { return state_(IDX::STEER); }
+double SimModelDelaySteerAccGearedWoFallGuard::getSteer()
+{
+  // return measured values with bias added to actual values
+  return state_(IDX::STEER) + steer_bias_;
+}
 
 void SimModelDelaySteerAccGearedWoFallGuard::update(const double & dt)
 {
@@ -113,7 +118,10 @@ Eigen::VectorXd SimModelDelaySteerAccGearedWoFallGuard::calcModel(
     sat(input(IDX_U::PEDAL_ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_;
   const double steer_des =
     sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_;
-  const double steer_diff = steer - steer_des;
+  // NOTE: `steer_des` is calculated by control from measured values. getSteer() also gets the
+  // measured value. The steer_rate used in the motion calculation is obtained from these
+  // differences.
+  const double steer_diff = getSteer() - steer_des;
   const double steer_diff_with_dead_band = std::invoke([&]() {
     if (steer_diff > steer_dead_band_) {
       return steer_diff - steer_dead_band_;
@@ -149,6 +157,8 @@ Eigen::VectorXd SimModelDelaySteerAccGearedWoFallGuard::calcModel(
         return pedal_acc + input(IDX_U::SLOPE_ACCX);
       } else if (vel < 0.0) {
         return -pedal_acc + input(IDX_U::SLOPE_ACCX);
+      } else if (-pedal_acc >= std::abs(input(IDX_U::SLOPE_ACCX))) {
+        return 0.0;
       } else {
         return input(IDX_U::SLOPE_ACCX);
       }