feat: Add delay steer vel in psim

simulator/simple_planning_simulator/CMakeLists.txt

@@ -16,6 +16,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp
+  src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
   src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
 )

simulator/simple_planning_simulator/design/simple_planning_simulator-design.md

@@ -60,6 +60,7 @@ The purpose of this simulator is for the integration test of planning and contro
  - `IDEAL_STEER_VEL`
  - `IDEAL_STEER_ACC`
  - `IDEAL_STEER_ACC_GEARED`
+ - `DELAY_STEER_VEL`
  - `DELAY_STEER_ACC`
  - `DELAY_STEER_ACC_GEARED`
 
@@ -68,16 +69,18 @@ The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves base
 The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V).
 
 
-|Name|Type|Description|I_ST_V|I_ST_A|I_ST_A_G|D_ST_A|D_ST_A_G|Default value| unit |
-|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---|
-|acc_time_delay         | double | dead time for the acceleration input                     | x | x | x | o | o | 0.1 | [s] |
-|steer_time_delay       | double | dead time for the steering input                         | x | x | x | o | o | 0.24| [s] |
-|acc_time_constant      | double | time constant of the 1st-order acceleration dynamics     | x | x | x | o | o | 0.1 | [s] |
-|steer_time_constant    | double | time constant of the 1st-order steering dynamics         | x | x | x | o | o | 0.27| [s] |
-|vel_lim                | double | limit of velocity                                        | x | x | x | o | o | 50.0| [m/s] |
-|vel_rate_lim           | double | limit of acceleration                                    | x | x | x | o | o | 7.0 | [m/ss] |
-|steer_lim              | double | limit of steering angle                                  | x | x | x | o | o | 1.0 | [rad] |
-|steer_rate_lim         | double | limit of steering angle change rate                      | x | x | x | o | o | 5.0 | [rad/s] |
+|Name|Type|Description|I_ST_V|I_ST_A|I_ST_A_G|D_ST_V|D_ST_A|D_ST_A_G|Default value| unit |
+|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---|
+|acc_time_delay         | double | dead time for the acceleration input                     | x | x | x | x | o | o | 0.1 | [s] |
+|steer_time_delay       | double | dead time for the steering input                         | x | x | x | o | o | o | 0.24| [s] |
+|vel_time_delay         | double | dead time for the velocity input                         | x | x | x | o | x | x | 0.25| [s] |
+|acc_time_constant      | double | time constant of the 1st-order acceleration dynamics     | x | x | x | x | o | o | 0.1 | [s] |
+|steer_time_constant    | double | time constant of the 1st-order steering dynamics         | x | x | x | o | o | o | 0.27| [s] |
+|vel_time_constant      | double | time constant of the 1st-order velocity dynamics         | x | x | x | o | x | x | 0.5 | [s] |
+|vel_lim                | double | limit of velocity                                        | x | x | x | o | o | o | 50.0| [m/s] |
+|vel_rate_lim           | double | limit of acceleration                                    | x | x | x | o | o | o | 7.0 | [m/ss] |
+|steer_lim              | double | limit of steering angle                                  | x | x | x | o | o | o | 1.0 | [rad] |
+|steer_rate_lim         | double | limit of steering angle change rate                      | x | x | x | o | o | o | 5.0 | [rad/s] |
 <!-- |deadzone_delta_steer   | double | dead zone for the steering dynamics                      | x | x | x | o | o | 0.0 | [rad] | -->
 
 

simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

@@ -189,6 +189,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
     DELAY_STEER_ACC = 2,
     DELAY_STEER_ACC_GEARED = 3,
     IDEAL_STEER_VEL = 4,
+    DELAY_STEER_VEL = 5
   } vehicle_model_type_;  //!< @brief vehicle model type to decide the model dynamics
   std::shared_ptr<SimModelInterface> vehicle_model_ptr_;  //!< @brief vehicle model pointer
 

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp

@@ -21,6 +21,7 @@
 #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp"
 #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp"
 #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp"
+#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp"
 
 
 #endif  // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_

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

@@ -0,0 +1,148 @@
+// Copyright 2021 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 SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_
+#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_
+
+#include <deque>
+#include <iostream>
+#include <queue>
+
+#include "eigen3/Eigen/Core"
+#include "eigen3/Eigen/LU"
+
+#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+/**
+ * @class SimModelDelaySteerVel
+ * @brief calculate delay steering dynamics
+ */
+class SimModelDelaySteerVel : public SimModelInterface
+{
+public:
+  /**
+   * @brief constructor
+   * @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] vx_delay time delay for velocity command [s]
+   * @param [in] vx_time_constant time constant for 1D model of velocity dynamics
+   * @param [in] steer_delay time delay for steering command [s]
+   * @param [in] steer_time_constant time constant for 1D model of steering dynamics
+   */
+  SimModelDelaySteerVel(
+    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
+   */
+  ~SimModelDelaySteerVel() = default;
+
+private:
+  const float64_t MIN_TIME_CONSTANT;  //!< @brief minimum time constant
+
+  enum IDX
+  {
+    X = 0,
+    Y,
+    YAW,
+    VX,
+    STEER,
+  };
+  enum IDX_U
+  {
+    VX_DES = 0,
+    STEER_DES,
+  };
+
+  const float64_t vx_lim_;       //!< @brief velocity limit
+  const float64_t vx_rate_lim_;  //!< @brief 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> 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 steer_delay_;              //!< @brief time delay for angular-velocity command [s]
+  const float64_t
+    steer_time_constant_;  //!< @brief time constant for 1D model of angular-velocity dynamics
+
+  /**
+   * @brief set queue buffer for input command
+   * @param [in] dt delta time
+   */
+  void initializeInputQueue(const float64_t & dt);
+
+  /**
+   * @brief get vehicle position x
+   */
+  float64_t getX() override;
+
+  /**
+   * @brief get vehicle position y
+   */
+  float64_t getY() override;
+
+  /**
+   * @brief get vehicle angle yaw
+   */
+  float64_t getYaw() override;
+
+  /**
+   * @brief get vehicle longitudinal velocity
+   */
+  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
+   */
+  float64_t getWz() override;
+
+  /**
+   * @brief get vehicle steering angle
+   */
+  float64_t getSteer() override;
+
+  /**
+   * @brief update vehicle states
+   * @param [in] dt delta time [s]
+   */
+  void update(const float64_t & dt) override;
+
+  /**
+   * @brief calculate derivative of states with 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;
+};
+
+#endif  // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -175,6 +175,8 @@ void SimplePlanningSimulator::initialize_vehicle_model()
   const float64_t steer_rate_lim = declare_parameter("steer_rate_lim", 5.0);
   const float64_t acc_time_delay = declare_parameter("acc_time_delay", 0.1);
   const float64_t acc_time_constant = declare_parameter("acc_time_constant", 0.1);
+  const float64_t vel_time_delay = declare_parameter("vel_time_delay", 0.25);
+  const float64_t vel_time_constant = declare_parameter("vel_time_constant", 0.5);
   const float64_t steer_time_delay = declare_parameter("steer_time_delay", 0.24);
   const float64_t steer_time_constant = declare_parameter("steer_time_constant", 0.27);
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -189,6 +191,11 @@ void SimplePlanningSimulator::initialize_vehicle_model()
   } else if (vehicle_model_type_str == "IDEAL_STEER_ACC_GEARED") {
     vehicle_model_type_ = VehicleModelType::IDEAL_STEER_ACC_GEARED;
     vehicle_model_ptr_ = std::make_shared<SimModelIdealSteerAccGeared>(wheelbase);
+  } 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, 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;
     vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAcc>(

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

@@ -0,0 +1,101 @@
+// Copyright 2021 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.
+
+#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp"
+
+#include <algorithm>
+
+SimModelDelaySteerVel::SimModelDelaySteerVel(
+  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),
+  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)),
+  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::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();
+  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);
+}
+
+void SimModelDelaySteerVel::initializeInputQueue(const float64_t & dt)
+{
+  size_t vx_input_queue_size = static_cast<size_t>(round(vx_delay_ / dt));
+  for (size_t i = 0; i < vx_input_queue_size; i++) {
+    vx_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);
+  }
+}
+
+
+Eigen::VectorXd SimModelDelaySteerVel::calcModel(
+  const Eigen::VectorXd & state, const Eigen::VectorXd & input)
+{
+  auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);};
+
+  const float64_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_);
+  const float64_t steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_);
+  const float64_t yaw = state(IDX::YAW);
+  const float64_t delay_input_vx = input(IDX_U::VX_DES);
+  const float64_t delay_input_steer = input(IDX_U::STEER_DES);
+  const float64_t delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_);
+  const float64_t delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_);
+  float64_t vx_rate = -(vx - delay_vx_des) / vx_time_constant_;
+  float64_t steer_rate = -(steer - delay_steer_des) / steer_time_constant_;
+  vx_rate = sat(vx_rate, vx_rate_lim_, -vx_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) = vx * std::tan(steer) / wheelbase_;
+  d_state(IDX::VX) = vx_rate;
+  d_state(IDX::STEER) = steer_rate;
+
+  return d_state;
+}
+

simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp

@@ -42,6 +42,7 @@ const std::vector<std::string> vehicle_model_type_vec = { // NOLINT
   "IDEAL_STEER_VEL",
   "IDEAL_STEER_ACC",
   "IDEAL_STEER_ACC_GEARED",
+  "DELAY_STEER_VEL",
   "DELAY_STEER_ACC",
   "DELAY_STEER_ACC_GEARED",
 };