diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index b441f9f903d0d..985d4e8cc2495 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -51,6 +51,8 @@ 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 | +| 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/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 38f689c124439..d096130e65f05 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_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/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 9a0ea6a6c3659..8de5be9d71503 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_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("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;