fix(simple_planning_simulator): fix steering bias model (#6240)

* fix(simple_planning_simulator): fix steering bias model

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove old implementation

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix initialize order

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix yawrate measurement

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove unused code

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add bias to steer rate

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add comments

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix getWz()

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp

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

---------

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: kosuke55 <kosuke.tnp@gmail.com>

Author: TakaHoribe

simulator/simple_planning_simulator/README.md

@@ -40,18 +40,17 @@ The purpose of this simulator is for the integration test of planning and contro
 
 ### Common Parameters
 
-| Name                   | Type   | Description                                                                                                                                | Default value        |
-| :--------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------------- |
-| simulated_frame_id     | string | set to the child_frame_id in output tf                                                                                                     | "base_link"          |
-| origin_frame_id        | string | set to the frame_id in output tf                                                                                                           | "odom"               |
-| initialize_source      | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `input/initialpose` topic is published. | "INITIAL_POSE_TOPIC" |
-| add_measurement_noise  | bool   | If true, the Gaussian noise is added to the simulated results.                                                                             | true                 |
-| pos_noise_stddev       | double | Standard deviation for position noise                                                                                                      | 0.01                 |
-| rpy_noise_stddev       | double | Standard deviation for Euler angle noise                                                                                                   | 0.0001               |
-| vel_noise_stddev       | double | Standard deviation for longitudinal velocity noise                                                                                         | 0.0                  |
-| angvel_noise_stddev    | double | Standard deviation for angular velocity noise                                                                                              | 0.0                  |
-| steer_noise_stddev     | double | Standard deviation for steering angle noise                                                                                                | 0.0001               |
-| measurement_steer_bias | double | Measurement bias for steering angle                                                                                                        | 0.0                  |
+| Name                  | Type   | Description                                                                                                                                | Default value        |
+| :-------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------------- |
+| simulated_frame_id    | string | set to the child_frame_id in output tf                                                                                                     | "base_link"          |
+| origin_frame_id       | string | set to the frame_id in output tf                                                                                                           | "odom"               |
+| initialize_source     | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `input/initialpose` topic is published. | "INITIAL_POSE_TOPIC" |
+| add_measurement_noise | bool   | If true, the Gaussian noise is added to the simulated results.                                                                             | true                 |
+| pos_noise_stddev      | double | Standard deviation for position noise                                                                                                      | 0.01                 |
+| rpy_noise_stddev      | double | Standard deviation for Euler angle noise                                                                                                   | 0.0001               |
+| vel_noise_stddev      | double | Standard deviation for longitudinal velocity noise                                                                                         | 0.0                  |
+| angvel_noise_stddev   | double | Standard deviation for angular velocity noise                                                                                              | 0.0                  |
+| steer_noise_stddev    | double | Standard deviation for steering angle noise                                                                                                | 0.0001               |
 
 ### Vehicle Model Parameters
 
@@ -82,6 +81,7 @@ The table below shows which models correspond to what parameters. The model name
 | vel_rate_lim               | double | limit of acceleration                                                                                       | x      | x      | x        | o      | o      | o        | o            | 7.0           | [m/ss]  |
 | steer_lim                  | double | limit of steering angle                                                                                     | x      | x      | x        | o      | o      | o        | o            | 1.0           | [rad]   |
 | steer_rate_lim             | double | limit of steering angle change rate                                                                         | x      | x      | x        | o      | o      | o        | o            | 5.0           | [rad/s] |
+| steer_bias                 | double | bias for steering angle                                                                                     | x      | x      | x        | o      | o      | o        | o            | 0.0           | [rad]   |
 | debug_acc_scaling_factor   | double | scaling factor for accel command                                                                            | x      | x      | x        | x      | o      | o        | x            | 1.0           | [-]     |
 | debug_steer_scaling_factor | double | scaling factor for steer command                                                                            | x      | x      | x        | x      | o      | o        | x            | 1.0           | [-]     |
 | acceleration_map_path      | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x      | x      | x        | x      | x      | x        | o            | -             | [-]     |

simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

@@ -195,9 +195,6 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   bool is_initialized_ = false;         //!< @brief flag to check the initial position is set
   bool add_measurement_noise_ = false;  //!< @brief flag to add measurement noise
 
-  /* measurement bias */
-  double measurement_steer_bias_ = 0.0;  //!< @brief measurement bias for steering measurement
-
   DeltaTime delta_time_{};  //!< @brief to calculate delta time
 
   MeasurementNoiseGenerator measurement_noise_{};  //!< @brief for measurement noise

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp

@@ -40,14 +40,15 @@ class SimModelDelaySteerAcc : 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
    */
   SimModelDelaySteerAcc(
     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
@@ -84,6 +85,7 @@ class SimModelDelaySteerAcc : 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
 

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp

@@ -40,14 +40,15 @@ class SimModelDelaySteerAccGeared : 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
    */
   SimModelDelaySteerAccGeared(
     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
@@ -84,6 +85,7 @@ class SimModelDelaySteerAccGeared : 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
 

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp

@@ -92,12 +92,13 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface
    * @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_bias steering bias [rad]
    * @param [in] path path to csv file for acceleration conversion map
    */
   SimModelDelaySteerMapAccGeared(
     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, std::string path);
+    double steer_time_constant, double steer_bias, std::string path);
 
   /**
    * @brief default destructor
@@ -135,6 +136,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface
   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_bias_;               //!< @brief steering angle bias [rad]
   const std::string path_;                //!< @brief conversion map path
 
   /**

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

@@ -43,11 +43,12 @@ class SimModelDelaySteerVel : 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]
    */
   SimModelDelaySteerVel(
     double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
     double dt, double vx_delay, double vx_time_constant, double steer_delay,
-    double steer_time_constant, double steer_dead_band);
+    double steer_time_constant, double steer_dead_band, double steer_bias);
 
   /**
    * @brief destructor
@@ -86,6 +87,7 @@ class SimModelDelaySteerVel : public SimModelInterface
   const double
     steer_time_constant_;  //!< @brief time constant for 1D model of angular-velocity dynamics
   const double steer_dead_band_;  //!< @brief dead band for steering angle [rad]
+  const double steer_bias_;       //!< @brief steering angle bias [rad]
 
   /**
    * @brief set queue buffer for input command

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -102,7 +102,6 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
   simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
   origin_frame_id_ = declare_parameter("origin_frame_id", "odom");
   add_measurement_noise_ = declare_parameter("add_measurement_noise", false);
-  measurement_steer_bias_ = declare_parameter("measurement_steer_bias", 0.0);
   simulate_motion_ = declare_parameter<bool>("initial_engage_state");
   enable_road_slope_simulation_ = declare_parameter("enable_road_slope_simulation", false);
 
@@ -232,6 +231,8 @@ void SimplePlanningSimulator::initialize_vehicle_model()
   const double steer_time_delay = declare_parameter("steer_time_delay", 0.24);
   const double steer_time_constant = declare_parameter("steer_time_constant", 0.27);
   const double steer_dead_band = declare_parameter("steer_dead_band", 0.0);
+  const double steer_bias = declare_parameter("steer_bias", 0.0);
+
   const double debug_acc_scaling_factor = declare_parameter("debug_acc_scaling_factor", 1.0);
   const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0);
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -250,19 +251,20 @@ void SimplePlanningSimulator::initialize_vehicle_model()
     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, steer_dead_band);
+      vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band,
+      steer_bias);
   } else if (vehicle_model_type_str == "DELAY_STEER_ACC") {
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC;
     vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAcc>(
       vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
       acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band,
-      debug_acc_scaling_factor, debug_steer_scaling_factor);
+      steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor);
   } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") {
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED;
     vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerAccGeared>(
       vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
       acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band,
-      debug_acc_scaling_factor, debug_steer_scaling_factor);
+      steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor);
   } else if (vehicle_model_type_str == "DELAY_STEER_MAP_ACC_GEARED") {
     vehicle_model_type_ = VehicleModelType::DELAY_STEER_MAP_ACC_GEARED;
     const std::string acceleration_map_path =
@@ -277,7 +279,7 @@ void SimplePlanningSimulator::initialize_vehicle_model()
     }
     vehicle_model_ptr_ = std::make_shared<SimModelDelaySteerMapAccGeared>(
       vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0,
-      acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant,
+      acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_bias,
       acceleration_map_path);
   } else {
     throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str);
@@ -383,9 +385,6 @@ void SimplePlanningSimulator::on_timer()
     add_measurement_noise(current_odometry_, current_velocity_, current_steer_);
   }
 
-  // add measurement bias
-  current_steer_.steering_tire_angle += measurement_steer_bias_;
-
   // add estimate covariance
   {
     using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp

@@ -19,8 +19,8 @@
 SimModelDelaySteerAcc::SimModelDelaySteerAcc(
   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(6 /* dim x */, 2 /* dim u */),
   MIN_TIME_CONSTANT(0.03),
   vx_lim_(vx_lim),
@@ -33,6 +33,7 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc(
   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))
 {
@@ -69,7 +70,7 @@ double SimModelDelaySteerAcc::getWz()
 }
 double SimModelDelaySteerAcc::getSteer()
 {
-  return state_(IDX::STEER);
+  return state_(IDX::STEER) + steer_bias_;
 }
 void SimModelDelaySteerAcc::update(const double & dt)
 {
@@ -111,7 +112,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel(
     sat(input(IDX_U::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;
+  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_;