feat(simple_planning_simulator): add mesurent_steer_bias

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Dec 14, 2023 · 35f0b57 · 35f0b57
1 parent 7477e9a
commit 35f0b57
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 0 deletions.
diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md
@@ -51,6 +51,7 @@ The purpose of this simulator is for the integration test of planning and contro
 | 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                  |
 
 ### Vehicle Model Parameters
 

diff --git a/...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -195,6 +195,9 @@ 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

diff --git a/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -102,6 +102,7 @@ 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);
 
@@ -381,6 +382,9 @@ 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;