change wz to steer

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp

@@ -43,9 +43,9 @@ class SimModelDelaySteerVel : public SimModelInterface
    * @param [in] wz_time_constant time constant for 1D model of angular-velocity dynamics
    */
   SimModelDelaySteerVel(
-    float64_t vx_lim, float64_t angel_lim, float64_t vx_rate_lim, float64_t wz_rate_lim,
-    float64_t dt, float64_t vx_delay, float64_t vx_time_constant, float64_t wz_delay,
-    float64_t wz_time_constant);
+    float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim,
+    float64_t wheelbase, float64_t dt, float64_t vx_delay, float64_t vx_time_constant,
+    float64_t steer_delay, float64_t steer_time_constant);
 
   /**
    * @brief destructor
@@ -61,27 +61,28 @@ class SimModelDelaySteerVel : public SimModelInterface
     Y,
     YAW,
     VX,
-    WZ,
+    STEER,
   };
   enum IDX_U
   {
     VX_DES = 0,
-    WZ_DES,
+    STEER_DES,
   };
 
   const float64_t vx_lim_;       //!< @brief velocity limit
   const float64_t vx_rate_lim_;  //!< @brief acceleration limit
-  const float64_t wz_lim_;       //!< @brief angular velocity limit
-  const float64_t wz_rate_lim_;  //!< @brief angular acceleration limit
+  const float64_t steer_lim_;       //!< @brief steering limit [rad]
+  const float64_t steer_rate_lim_;  //!< @brief steering angular velocity limit [rad/s]
+  const float64_t wheelbase_;       //!< @brief vehicle wheelbase length [m]
 
   std::deque<float64_t> vx_input_queue_;  //!< @brief buffer for velocity command
-  std::deque<float64_t> wz_input_queue_;  //!< @brief buffer for angular velocity command
+  std::deque<float64_t> steer_input_queue_;  //!< @brief buffer for angular velocity command
   const float64_t vx_delay_;              //!< @brief time delay for velocity command [s]
   const float64_t vx_time_constant_;
     //!< @brief time constant for 1D model of velocity dynamics
-  const float64_t wz_delay_;              //!< @brief time delay for angular-velocity command [s]
+  const float64_t steer_delay_;              //!< @brief time delay for angular-velocity command [s]
   const float64_t
-    wz_time_constant_;  //!< @brief time constant for 1D model of angular-velocity dynamics
+    steer_time_constant_;  //!< @brief time constant for 1D model of angular-velocity dynamics
 
   /**
    * @brief set queue buffer for input command
@@ -109,6 +110,16 @@ class SimModelDelaySteerVel : public SimModelInterface
    */
   float64_t getVx() override;
 
+  /**
+   * @brief get vehicle lateral velocity
+   */
+  float64_t getVy() override;
+
+  /**
+   * @brief get vehicle longiudinal acceleration
+   */
+  float64_t getAx() override;
+
   /**
    * @brief get vehicle angular-velocity wz
    */

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -194,7 +194,7 @@ void SimplePlanningSimulator::initialize_vehicle_model()
   } else if (vehicle_model_type_str == "DELAY_STEER_VEL") {
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL;
     vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerVel>(
-      vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, timer_sampling_time_ms_ / 1000.0,
+      vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
       vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant);
   } else if (vehicle_model_type_str == "DELAY_STEER_ACC") {
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC;

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp

@@ -17,38 +17,44 @@
 #include <algorithm>
 
 SimModelDelaySteerVel::SimModelDelaySteerVel(
-  float64_t vx_lim, float64_t wz_lim, float64_t vx_rate_lim, float64_t wz_rate_lim, float64_t dt,
-  float64_t vx_delay, float64_t vx_time_constant, float64_t wz_delay, float64_t wz_time_constant)
+  float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim,
+  float64_t wheelbase, float64_t dt, float64_t vx_delay, float64_t vx_time_constant,
+  float64_t steer_delay, float64_t steer_time_constant)
 : SimModelInterface(5 /* dim x */, 2 /* dim u */),
   MIN_TIME_CONSTANT(0.03),
   vx_lim_(vx_lim),
   vx_rate_lim_(vx_rate_lim),
-  wz_lim_(wz_lim),
-  wz_rate_lim_(wz_rate_lim),
+  steer_lim_(steer_lim),
+  steer_rate_lim_(steer_rate_lim),
+  wheelbase_(wheelbase),
   vx_delay_(vx_delay),
   vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)),
-  wz_delay_(wz_delay),
-  wz_time_constant_(std::max(wz_time_constant, MIN_TIME_CONSTANT))
+  steer_delay_(steer_delay),
+  steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT))
 {
   initializeInputQueue(dt);
 }
 
 float64_t SimModelDelaySteerVel::getX() { return state_(IDX::X); }
 float64_t SimModelDelaySteerVel::getY() { return state_(IDX::Y); }
 float64_t SimModelDelaySteerVel::getYaw() { return state_(IDX::YAW); }
-float64_t SimModelDelaySteerVel::getVx() { return input_(IDX_U::VX_DES); }
-float64_t SimModelDelaySteerVel::getWz() { return state_(IDX::WZ); }
-float64_t SimModelDelaySteerVel::getSteer() { return 0.0; }
+float64_t SimModelDelaySteerVel::getVx() { return input_(IDX::VX); }
+float64_t SimModelDelaySteerVel::getVy() {return 0.0;}
+float64_t SimModelDelaySteerVel::getWz()
+{
+  return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_;
+}
+float64_t SimModelDelaySteerVel::getSteer() {return state_(IDX::STEER);}
 void SimModelDelaySteerVel::update(const float64_t & dt)
 {
   Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_);
 
   vx_input_queue_.push_back(input_(IDX_U::VX_DES));
   delayed_input(IDX_U::VX_DES) = vx_input_queue_.front();
   vx_input_queue_.pop_front();
-  wz_input_queue_.push_back(input_(IDX_U::WZ_DES));
-  delayed_input(IDX_U::WZ_DES) = wz_input_queue_.front();
-  wz_input_queue_.pop_front();
+  steer_input_queue_.push_back(input_(IDX_U::STEER_DES));
+  delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front();
+  steer_input_queue_.pop_front();
   // do not use deadzone_delta_steer (Steer IF does not exist in this model)
   updateRungeKutta(dt, delayed_input);
 }
@@ -59,9 +65,9 @@ void SimModelDelaySteerVel::initializeInputQueue(const float_t & dt)
   for (size_t i = 0; i < vx_input_queue_size; i++) {
     vx_input_queue_.push_back(0.0);
   }
-  size_t wz_input_queue_size = static_cast<size_t>(round(wz_delay_ / dt));
-  for (size_t i = 0; i < wz_input_queue_size; i++) {
-    wz_input_queue_.push_back(0.0);
+  size_t steer_input_queue_size = static_cast<size_t>(round(steer_delay_ / dt));
+  for (size_t i = 0; i < steer_input_queue_size; i++) {
+    steer_input_queue_.push_back(0.0);
   }
 }
 
@@ -72,23 +78,23 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel(
   auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);};
 
   const float_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_);
-  const float_t wz = sat(state(IDX::WZ), wz_lim_, -wz_lim_);
+  const float_t steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_);
   const float_t yaw = state(IDX::YAW);
   const float_t delay_input_vx = input(IDX_U::VX_DES);
-  const float_t delay_input_wz = input(IDX_U::WZ_DES);
+  const float_t delay_input_steer = input(IDX_U::STEER_DES);
   const float_t delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_);
-  const float_t delay_wz_des = sat(delay_input_wz, wz_lim_, -wz_lim_);
+  const float_t delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_);
   float_t vx_rate = -(vx - delay_vx_des) / vx_time_constant_;
-  float_t wz_rate = -(wz - delay_wz_des) / wz_time_constant_;
+  float_t steer_rate = -(steer - delay_steer_des) / steer_time_constant_;
   vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_);
-  wz_rate = sat(wz_rate, wz_rate_lim_, -wz_rate_lim_);
+  steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_);
 
   Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
   d_state(IDX::X) = vx * cos(yaw);
   d_state(IDX::Y) = vx * sin(yaw);
-  d_state(IDX::YAW) = wz;
+  d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_;
   d_state(IDX::VX) = vx_rate;
-  d_state(IDX::WZ) = wz_rate;
+  d_state(IDX::STEER) = steer_rate;
 
   return d_state;
 }