feat: add new vehicle model

simulation/simple_sensor_simulator/CMakeLists.txt

@@ -48,6 +48,7 @@ ament_auto_add_library(simple_sensor_simulator_component SHARED
   src/vehicle_simulation/ego_entity_simulation.cpp
   src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc.cpp
   src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp
+  src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp
   src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
   src/vehicle_simulation/vehicle_model/sim_model_delay_steer_vel.cpp
   src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc.cpp

simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp

@@ -30,6 +30,7 @@ namespace vehicle_simulation
 enum class VehicleModelType {
   DELAY_STEER_ACC,
   DELAY_STEER_ACC_GEARED,
+  DELAY_STEER_ACC_GEARED_WO_FALL_GUARD,
   DELAY_STEER_MAP_ACC_GEARED,
   DELAY_STEER_VEL,
   IDEAL_STEER_ACC,

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

@@ -17,6 +17,7 @@
 
 #include <simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc.hpp>
 #include <simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp>
+#include <simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp>
 #include <simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp>
 #include <simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_vel.hpp>
 #include <simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc.hpp>

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

@@ -0,0 +1,152 @@
+// Copyright 2025 The Autoware Foundation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_  // NOLINT
+#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_  // NOLINT
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+#include <deque>
+#include <iostream>
+#include <queue>
+#include <simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp>
+
+namespace autoware::simulator::simple_planning_simulator
+{
+
+class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface
+{
+public:
+  /**
+   * @param [in] vx_lim velocity limit [m/s]
+   * @param [in] steer_lim steering limit [rad]
+   * @param [in] vx_rate_lim acceleration limit [m/ss]
+   * @param [in] steer_rate_lim steering angular velocity limit [rad/ss]
+   * @param [in] wheelbase vehicle wheelbase length [m]
+   * @param [in] dt delta time information to set input buffer for delay
+   * @param [in] acc_delay time delay for accel command [s]
+   * @param [in] acc_time_constant time constant for 1D model of accel dynamics
+   * @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 steer_bias,
+    double debug_acc_scaling_factor, double debug_steer_scaling_factor);
+
+  /**
+   * @brief default destructor
+   */
+  ~SimModelDelaySteerAccGearedWoFallGuard() = default;
+
+private:
+  const double MIN_TIME_CONSTANT;  //!< @brief minimum time constant
+
+  enum IDX {
+    X = 0,
+    Y,
+    YAW,
+    VX,
+    STEER,
+    ACCX,
+    PEDAL_ACCX,
+  };
+  enum IDX_U { PEDAL_ACCX_DES = 0, GEAR, SLOPE_ACCX, STEER_DES };
+
+  const double vx_lim_;          //!< @brief velocity limit [m/s]
+  const double vx_rate_lim_;     //!< @brief acceleration limit [m/ss]
+  const double steer_lim_;       //!< @brief steering limit [rad]
+  const double steer_rate_lim_;  //!< @brief steering angular velocity limit [rad/s]
+  const double wheelbase_;       //!< @brief vehicle wheelbase length [m]
+
+  std::deque<double> acc_input_queue_;       //!< @brief buffer for accel command
+  std::deque<double> steer_input_queue_;     //!< @brief buffer for steering command
+  const double acc_delay_;                   //!< @brief time delay for accel command [s]
+  const double acc_time_constant_;           //!< @brief time constant for accel dynamics
+  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
+
+  /**
+   * @brief set queue buffer for input command
+   * @param [in] dt delta time
+   */
+  void initializeInputQueue(const double & dt);
+
+  /**
+   * @brief get vehicle position x
+   */
+  double getX() override;
+
+  /**
+   * @brief get vehicle position y
+   */
+  double getY() override;
+
+  /**
+   * @brief get vehicle angle yaw
+   */
+  double getYaw() override;
+
+  /**
+   * @brief get vehicle velocity vx
+   */
+  double getVx() override;
+
+  /**
+   * @brief get vehicle lateral velocity
+   */
+  double getVy() override;
+
+  /**
+   * @brief get vehicle longitudinal acceleration
+   */
+  double getAx() override;
+
+  /**
+   * @brief get vehicle angular-velocity wz
+   */
+  double getWz() override;
+
+  /**
+   * @brief get vehicle steering angle
+   */
+  double getSteer() override;
+
+  /**
+   * @brief update vehicle states
+   * @param [in] dt delta time [s]
+   */
+  void update(const double & dt) override;
+
+  /**
+   * @brief calculate derivative of states with time delay steering model
+   * @param [in] state current model state
+   * @param [in] input input vector to model
+   */
+  Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override;
+};
+
+}  // namespace autoware::simulator::simple_planning_simulator
+
+// NOLINTNEXTLINE
+#endif  // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -66,6 +66,7 @@ auto toString(const VehicleModelType datum) -> std::string
   switch (datum) {
     BOILERPLATE(DELAY_STEER_ACC);
     BOILERPLATE(DELAY_STEER_ACC_GEARED);
+    BOILERPLATE(DELAY_STEER_ACC_GEARED_WO_FALL_GUARD);
     BOILERPLATE(DELAY_STEER_MAP_ACC_GEARED);
     BOILERPLATE(DELAY_STEER_VEL);
     BOILERPLATE(IDEAL_STEER_ACC);
@@ -86,6 +87,8 @@ auto EgoEntitySimulation::getVehicleModelType() -> VehicleModelType
   static const std::unordered_map<std::string, VehicleModelType> table{
     {"DELAY_STEER_ACC", VehicleModelType::DELAY_STEER_ACC},
     {"DELAY_STEER_ACC_GEARED", VehicleModelType::DELAY_STEER_ACC_GEARED},
+    {"DELAY_STEER_ACC_GEARED_WO_FALL_GUARD",
+     VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD},
     {"DELAY_STEER_MAP_ACC_GEARED", VehicleModelType::DELAY_STEER_MAP_ACC_GEARED},
     {"DELAY_STEER_VEL", VehicleModelType::DELAY_STEER_VEL},
     {"IDEAL_STEER_ACC", VehicleModelType::IDEAL_STEER_ACC},
@@ -120,8 +123,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);
@@ -145,6 +149,13 @@ auto EgoEntitySimulation::makeSimulationModel(
         acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band,
         debug_acc_scaling_factor, debug_steer_scaling_factor);
 
+    case VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD:
+      return std::make_shared<
+        autoware::simulator::simple_planning_simulator::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, steer_bias,
+        debug_acc_scaling_factor, debug_steer_scaling_factor);
+
     case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:
       if (!std::filesystem::exists(acceleration_map_path)) {
         throw std::runtime_error(
@@ -200,6 +211,10 @@ void EgoEntitySimulation::requestSpeedChange(double value)
       v << 0, 0, 0, value, 0, 0;
       break;
 
+    case VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD:
+      v << 0, 0, 0, value, 0, 0, 0;
+      break;
+
     case VehicleModelType::IDEAL_STEER_ACC:
     case VehicleModelType::IDEAL_STEER_ACC_GEARED:
       v << 0, 0, 0, value;
@@ -257,6 +272,7 @@ auto EgoEntitySimulation::overwrite(
     switch (auto state = Eigen::VectorXd(vehicle_model_ptr_->getDimX()); vehicle_model_type_) {
       case VehicleModelType::DELAY_STEER_ACC:
       case VehicleModelType::DELAY_STEER_ACC_GEARED:
+      case VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD:
       case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:
         state(5) = status.action_status.accel.linear.x;
         [[fallthrough]];
@@ -322,6 +338,13 @@ void EgoEntitySimulation::update(
         input(1) = tire_angle;
         break;
 
+      case VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD:
+        input(0) = acceleration;
+        input(1) = autoware->getGearCommand().command;
+        input(2) = acceleration_by_slope;
+        input(3) = tire_angle;
+        break;
+
       case VehicleModelType::DELAY_STEER_VEL:
       case VehicleModelType::IDEAL_STEER_VEL:
         input(0) = speed;