Skip to content

Commit

Permalink
feat(simple_planning_simulator): add mesurent_steer_bias (#5868)
Browse files Browse the repository at this point in the history 
* feat(simple_planning_simulator): add mesurent_steer_bias

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

* style(pre-commit): autofix

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@kosuke55 @pre-commit-ci
kosuke55 and pre-commit-ci[bot] authored Dec 14, 2023
1 parent 3d40702 commit d4d632a
Show file tree
Hide file tree
Showing 3 changed files with 19 additions and 11 deletions.
23 changes: 12 additions & 11 deletions simulator/simple_planning_simulator/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
3 changes: 3 additions & 0 deletions ...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 4 additions & 0 deletions ...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit d4d632a

Please sign in to comment.