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] committed Dec 14, 2023
1 parent 85c2338 commit 51750f3
Show file tree
Hide file tree
Showing 3 changed files with 20 additions and 12 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
5 changes: 4 additions & 1 deletion ...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 @@ -188,7 +188,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
bool is_initialized_; //!< @brief flag to check the initial position is set
bool add_measurement_noise_; //!< @brief flag to add measurement noise

DeltaTime delta_time_; //!< @brief to calculate delta time
/* 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 @@ -79,6 +79,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");

using rclcpp::QoS;
Expand Down Expand Up @@ -288,6 +289,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 51750f3

Please sign in to comment.