diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index b441f9f903d0d..ed2b06ee1f44d 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -40,17 +40,18 @@ 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 | +| 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 | ### 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;