From d51e69311f6eac80cc3197fdda111d30d3fe928b Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 31 Jan 2024 00:31:55 +0900 Subject: [PATCH 01/11] fix(simple_planning_simulator): fix steering bias model Signed-off-by: Takamasa Horibe --- simulator/simple_planning_simulator/README.md | 24 +++++++++---------- .../sim_model_delay_steer_acc.hpp | 6 +++-- .../sim_model_delay_steer_acc_geared.hpp | 6 +++-- .../sim_model_delay_steer_map_acc_geared.hpp | 4 +++- .../sim_model_delay_steer_vel.hpp | 4 +++- .../simple_planning_simulator_core.cpp | 12 ++++++---- .../sim_model_delay_steer_acc.cpp | 7 +++--- .../sim_model_delay_steer_acc_geared.cpp | 7 +++--- .../sim_model_delay_steer_map_acc_geared.cpp | 5 ++-- .../sim_model_delay_steer_vel.cpp | 7 +++--- 10 files changed, 48 insertions(+), 34 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index ed2b06ee1f44d..d2ed19708c5c5 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -40,18 +40,17 @@ 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 | -| measurement_steer_bias | double | Measurement bias for steering angle | 0.0 | +| 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 | ### Vehicle Model Parameters @@ -82,6 +81,7 @@ The table below shows which models correspond to what parameters. The model name | vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | 7.0 | [m/ss] | | steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | 1.0 | [rad] | | steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | 5.0 | [rad/s] | +| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | 0.0 | [rad] | | debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | 1.0 | [-] | | debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | 1.0 | [-] | | acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | - | [-] | diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index 92129541ff891..f43abd8572874 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -40,14 +40,15 @@ class SimModelDelaySteerAcc : public SimModelInterface * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics * @param [in] steer_dead_band dead band for steering angle [rad] + * @param [in] steer_bias steering bias [rad] * @param [in] debug_acc_scaling_factor scaling factor for accel command * @param [in] debug_steer_scaling_factor scaling factor for steering command */ SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, - double debug_steer_scaling_factor); + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor); /** * @brief default destructor @@ -84,6 +85,7 @@ class SimModelDelaySteerAcc : public SimModelInterface const double steer_delay_; //!< @brief time delay for steering command [s] const double steer_time_constant_; //!< @brief time constant for steering dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + const double steer_bias_; //!< @brief steering angle bias [rad] const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 376ee55f1aa5e..35b95bf4b1ae5 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -40,14 +40,15 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics * @param [in] steer_dead_band dead band for steering angle [rad] + * @param [in] steer_bias steering bias [rad] * @param [in] debug_acc_scaling_factor scaling factor for accel command * @param [in] debug_steer_scaling_factor scaling factor for steering command */ SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, - double debug_steer_scaling_factor); + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor); /** * @brief default destructor @@ -84,6 +85,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface const double steer_delay_; //!< @brief time delay for steering command [s] const double steer_time_constant_; //!< @brief time constant for steering dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + const double steer_bias_; //!< @brief steering angle bias [rad] const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index fecd3c2bc8be4..eaf86a6e21ff2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -92,11 +92,12 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface * @param [in] acc_time_constant time constant for 1D model of accel dynamics * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] steer_bias steering bias [rad] * @param [in] path path to csv file for acceleration conversion map */ SimModelDelaySteerMapAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double acc_delay, double acc_time_constant, double steer_delay, + double dt, double acc_delay, double acc_time_constant, double steer_delay, double steer_bias, double steer_time_constant, std::string path); /** @@ -135,6 +136,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface const double acc_time_constant_; //!< @brief time constant for accel dynamics const double steer_delay_; //!< @brief time delay for steering command [s] const double steer_time_constant_; //!< @brief time constant for steering dynamics + const double steer_bias_; //!< @brief steering angle bias [rad] const std::string path_; //!< @brief conversion map path /** diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp index 5c1ec55d522bf..61a9e8d0a5643 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp @@ -43,11 +43,12 @@ class SimModelDelaySteerVel : public SimModelInterface * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics * @param [in] steer_dead_band dead band for steering angle [rad] + * @param [in] steer_bias steering bias [rad] */ SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band); + double steer_time_constant, double steer_dead_band, double steer_bias); /** * @brief destructor @@ -86,6 +87,7 @@ class SimModelDelaySteerVel : public SimModelInterface const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + const double steer_bias_; //!< @brief steering angle bias [rad] /** * @brief set queue buffer for input command 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 37ec5b33014a7..19f4f9dfba12c 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,7 +102,6 @@ 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); @@ -232,6 +231,8 @@ void SimplePlanningSimulator::initialize_vehicle_model() const double steer_time_delay = declare_parameter("steer_time_delay", 0.24); const double steer_time_constant = declare_parameter("steer_time_constant", 0.27); const double steer_dead_band = declare_parameter("steer_dead_band", 0.0); + const double steer_bias = declare_parameter("steer_bias", 0.0); + const double debug_acc_scaling_factor = declare_parameter("debug_acc_scaling_factor", 1.0); const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -250,19 +251,20 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); + vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + steer_bias); } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, - debug_acc_scaling_factor, debug_steer_scaling_factor); + steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor); } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, - debug_acc_scaling_factor, debug_steer_scaling_factor); + steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor); } else if (vehicle_model_type_str == "DELAY_STEER_MAP_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_MAP_ACC_GEARED; const std::string acceleration_map_path = @@ -277,7 +279,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() } vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, + acc_time_delay, acc_time_constant, steer_time_delay, steer_bias, steer_time_constant, acceleration_map_path); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 670671165de60..8d84553a7c64d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -19,8 +19,8 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, - double debug_steer_scaling_factor) + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -33,6 +33,7 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias), debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { @@ -127,7 +128,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::YAW) = vel * std::tan(steer + steer_bias_) / wheelbase_; d_state(IDX::VX) = acc; d_state(IDX::STEER) = steer_rate; d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index d72b8bf116d77..24631d9e78d49 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -21,8 +21,8 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, - double debug_steer_scaling_factor) + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -35,6 +35,7 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias), debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { @@ -135,7 +136,7 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::YAW) = vel * std::tan(steer + steer_bias_) / wheelbase_; d_state(IDX::VX) = acc; d_state(IDX::STEER) = steer_rate; d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index b04a10667adcf..64a20b8192ec4 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -20,7 +20,7 @@ SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double acc_delay, double acc_time_constant, double steer_delay, + double dt, double acc_delay, double acc_time_constant, double steer_delay, double steer_bias, double steer_time_constant, std::string path) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), @@ -32,6 +32,7 @@ SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( acc_delay_(acc_delay), acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), + steer_bias_(steer_bias), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), path_(path) { @@ -119,7 +120,7 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::YAW) = vel * std::tan(steer + steer_bias_) / wheelbase_; d_state(IDX::VX) = acc; d_state(IDX::STEER) = steer_rate; const double converted_acc = diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 6d155ee921107..8f97277066681 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -19,7 +19,7 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band) + double steer_time_constant, double steer_dead_band, double steer_bias) : SimModelInterface(5 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -31,7 +31,8 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), - steer_dead_band_(steer_dead_band) + steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias) { initializeInputQueue(dt); } @@ -125,7 +126,7 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw); d_state(IDX::Y) = vx * sin(yaw); - d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + d_state(IDX::YAW) = vx * std::tan(steer + steer_bias_) / wheelbase_; d_state(IDX::VX) = vx_rate; d_state(IDX::STEER) = steer_rate; From d1fbfc785129568eb552c8b143112a67e8558ed6 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 31 Jan 2024 00:33:39 +0900 Subject: [PATCH 02/11] remove old implementation Signed-off-by: Takamasa Horibe --- .../simple_planning_simulator_core.hpp | 3 --- .../simple_planning_simulator_core.cpp | 3 --- 2 files changed, 6 deletions(-) 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 4c3a3aed24d90..58914d19552df 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,9 +195,6 @@ 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 19f4f9dfba12c..05d596b2e02e0 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 @@ -385,9 +385,6 @@ 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; From aed992d14734ffff7cc197e33965415f1fb95682 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 31 Jan 2024 00:43:57 +0900 Subject: [PATCH 03/11] fix initialize order Signed-off-by: Takamasa Horibe --- .../vehicle_model/sim_model_delay_steer_map_acc_geared.hpp | 4 ++-- .../simple_planning_simulator_core.cpp | 2 +- .../vehicle_model/sim_model_delay_steer_map_acc_geared.cpp | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index eaf86a6e21ff2..d7ca032aa6c48 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -97,8 +97,8 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface */ SimModelDelaySteerMapAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double acc_delay, double acc_time_constant, double steer_delay, double steer_bias, - double steer_time_constant, std::string path); + double dt, double acc_delay, double acc_time_constant, double steer_delay, + double steer_time_constant, double steer_bias, std::string path); /** * @brief default destructor 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 05d596b2e02e0..26be4841e2f2a 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 @@ -279,7 +279,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() } vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_bias, steer_time_constant, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_bias, acceleration_map_path); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 64a20b8192ec4..84be54589491f 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -20,8 +20,8 @@ SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double acc_delay, double acc_time_constant, double steer_delay, double steer_bias, - double steer_time_constant, std::string path) + double dt, double acc_delay, double acc_time_constant, double steer_delay, + double steer_time_constant, double steer_bias, std::string path) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -32,8 +32,8 @@ SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( acc_delay_(acc_delay), acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), - steer_bias_(steer_bias), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_bias_(steer_bias), path_(path) { initializeInputQueue(dt); From cf188f27da044d2531ce22de29e4a37658d15a5c Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 31 Jan 2024 01:12:35 +0900 Subject: [PATCH 04/11] fix yawrate measurement Signed-off-by: Takamasa Horibe --- .../vehicle_model/sim_model_delay_steer_acc.cpp | 2 +- .../vehicle_model/sim_model_delay_steer_acc_geared.cpp | 4 +++- .../vehicle_model/sim_model_delay_steer_map_acc_geared.cpp | 2 +- .../vehicle_model/sim_model_delay_steer_vel.cpp | 2 +- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 8d84553a7c64d..d2f3eb8f77ee6 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -66,7 +66,7 @@ double SimModelDelaySteerAcc::getAx() } double SimModelDelaySteerAcc::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_; } double SimModelDelaySteerAcc::getSteer() { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 24631d9e78d49..9059aa6332350 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -68,7 +68,7 @@ double SimModelDelaySteerAccGeared::getAx() } double SimModelDelaySteerAccGeared::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_; } double SimModelDelaySteerAccGeared::getSteer() { @@ -141,6 +141,8 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( d_state(IDX::STEER) = steer_rate; d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; + // std::cerr << "steer = " << steer << ", steer_bias_ = " << steer_bias_ << std::endl; + return d_state; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 84be54589491f..e01c39037a2d5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -66,7 +66,7 @@ double SimModelDelaySteerMapAccGeared::getAx() } double SimModelDelaySteerMapAccGeared::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_; } double SimModelDelaySteerMapAccGeared::getSteer() { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 8f97277066681..0de080cd5805f 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -63,7 +63,7 @@ double SimModelDelaySteerVel::getAx() } double SimModelDelaySteerVel::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_; } double SimModelDelaySteerVel::getSteer() { From 6018a2cce50632b94aaf826c80358b9f2c621de8 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 31 Jan 2024 01:25:20 +0900 Subject: [PATCH 05/11] remove unused code Signed-off-by: Takamasa Horibe --- .../vehicle_model/sim_model_delay_steer_acc_geared.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 9059aa6332350..60163740caae8 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -141,8 +141,6 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( d_state(IDX::STEER) = steer_rate; d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; - // std::cerr << "steer = " << steer << ", steer_bias_ = " << steer_bias_ << std::endl; - return d_state; } From e4dadc5b80fdc3266eec412535443bac680dead8 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 31 Jan 2024 15:36:03 +0900 Subject: [PATCH 06/11] add bias to steer rate Signed-off-by: kosuke55 --- .../vehicle_model/sim_model_delay_steer_acc.cpp | 6 +++--- .../vehicle_model/sim_model_delay_steer_acc_geared.cpp | 6 +++--- .../vehicle_model/sim_model_delay_steer_map_acc_geared.cpp | 6 +++--- .../vehicle_model/sim_model_delay_steer_vel.cpp | 6 +++--- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index d2f3eb8f77ee6..222fc0edfebe4 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -70,7 +70,7 @@ double SimModelDelaySteerAcc::getWz() } double SimModelDelaySteerAcc::getSteer() { - return state_(IDX::STEER); + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerAcc::update(const double & dt) { @@ -112,7 +112,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_; const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_; - const double steer_diff = steer - steer_des; + const double steer_diff = getSteer() - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { return steer_diff - steer_dead_band_; @@ -128,7 +128,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer + steer_bias_) / wheelbase_; + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; d_state(IDX::VX) = acc; d_state(IDX::STEER) = steer_rate; d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 60163740caae8..fa6849ae9e6b7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -72,7 +72,7 @@ double SimModelDelaySteerAccGeared::getWz() } double SimModelDelaySteerAccGeared::getSteer() { - return state_(IDX::STEER); + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerAccGeared::update(const double & dt) { @@ -120,7 +120,7 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_; const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_; - const double steer_diff = steer - steer_des; + const double steer_diff = getSteer() - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { return steer_diff - steer_dead_band_; @@ -136,7 +136,7 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer + steer_bias_) / wheelbase_; + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; d_state(IDX::VX) = acc; d_state(IDX::STEER) = steer_rate; d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index e01c39037a2d5..2df8ddf0e015d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -70,7 +70,7 @@ double SimModelDelaySteerMapAccGeared::getWz() } double SimModelDelaySteerMapAccGeared::getSteer() { - return state_(IDX::STEER); + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerMapAccGeared::update(const double & dt) { @@ -114,13 +114,13 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( const double steer = state(IDX::STEER); const double acc_des = std::clamp(input(IDX_U::ACCX_DES), -vx_rate_lim_, vx_rate_lim_); const double steer_des = std::clamp(input(IDX_U::STEER_DES), -steer_lim_, steer_lim_); - double steer_rate = -(steer - steer_des) / steer_time_constant_; + double steer_rate = -(getSteer() - steer_des) / steer_time_constant_; steer_rate = std::clamp(steer_rate, -steer_rate_lim_, steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer + steer_bias_) / wheelbase_; + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; d_state(IDX::VX) = acc; d_state(IDX::STEER) = steer_rate; const double converted_acc = diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 0de080cd5805f..87d7af140bb74 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -67,7 +67,7 @@ double SimModelDelaySteerVel::getWz() } double SimModelDelaySteerVel::getSteer() { - return state_(IDX::STEER); + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerVel::update(const double & dt) { @@ -110,7 +110,7 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - const double steer_diff = steer - delay_steer_des; + const double steer_diff = getSteer() - delay_steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { return steer_diff - steer_dead_band_; @@ -126,7 +126,7 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw); d_state(IDX::Y) = vx * sin(yaw); - d_state(IDX::YAW) = vx * std::tan(steer + steer_bias_) / wheelbase_; + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; d_state(IDX::VX) = vx_rate; d_state(IDX::STEER) = steer_rate; From 30ea518121d848926715a1b4849d852f58467b16 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 31 Jan 2024 15:50:33 +0900 Subject: [PATCH 07/11] add comments Signed-off-by: kosuke55 --- .../vehicle_model/sim_model_delay_steer_acc_geared.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index fa6849ae9e6b7..f3dd6e0050033 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -72,6 +72,7 @@ double SimModelDelaySteerAccGeared::getWz() } double SimModelDelaySteerAccGeared::getSteer() { + // return measured values with bias added to actual values return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerAccGeared::update(const double & dt) @@ -120,6 +121,9 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_; const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_; + // NOTE: `steer_des` is calculated by control from measured values. getSteer() also gets the + // measured value. The steer_rate used in the motion calculation is obtained from these + // differences. const double steer_diff = getSteer() - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { From c625256a3193ccad9ad625f7f0bfdb2d84bc2455 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 5 Feb 2024 13:05:57 +0900 Subject: [PATCH 08/11] fix getWz() Signed-off-by: kosuke55 --- .../vehicle_model/sim_model_delay_steer_acc.cpp | 3 ++- .../vehicle_model/sim_model_delay_steer_acc_geared.cpp | 3 ++- .../vehicle_model/sim_model_delay_steer_map_acc_geared.cpp | 3 ++- .../vehicle_model/sim_model_delay_steer_vel.cpp | 3 ++- 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 222fc0edfebe4..f28ca50547f96 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -66,7 +66,8 @@ double SimModelDelaySteerAcc::getAx() } double SimModelDelaySteerAcc::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + ; } double SimModelDelaySteerAcc::getSteer() { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index f3dd6e0050033..10e6a97d703cd 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -68,7 +68,8 @@ double SimModelDelaySteerAccGeared::getAx() } double SimModelDelaySteerAccGeared::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + ; } double SimModelDelaySteerAccGeared::getSteer() { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 2df8ddf0e015d..56610b0b2a067 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -66,7 +66,8 @@ double SimModelDelaySteerMapAccGeared::getAx() } double SimModelDelaySteerMapAccGeared::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + ; } double SimModelDelaySteerMapAccGeared::getSteer() { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 87d7af140bb74..68674d631e6a5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -63,7 +63,8 @@ double SimModelDelaySteerVel::getAx() } double SimModelDelaySteerVel::getWz() { - return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_; + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + ; } double SimModelDelaySteerVel::getSteer() { From a0259fc6a954b9b8d3951f9a315d19404ef872bf Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 5 Feb 2024 23:51:31 +0900 Subject: [PATCH 09/11] Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp --- .../vehicle_model/sim_model_delay_steer_acc.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index f28ca50547f96..6a9b5c65d4760 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -67,7 +67,6 @@ double SimModelDelaySteerAcc::getAx() double SimModelDelaySteerAcc::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; - ; } double SimModelDelaySteerAcc::getSteer() { From b3e9f7117ea8627e734591f6fe9aa5efa418e2bc Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 5 Feb 2024 23:51:38 +0900 Subject: [PATCH 10/11] Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp --- .../vehicle_model/sim_model_delay_steer_map_acc_geared.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 56610b0b2a067..72273f0b21ec2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -67,7 +67,6 @@ double SimModelDelaySteerMapAccGeared::getAx() double SimModelDelaySteerMapAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; - ; } double SimModelDelaySteerMapAccGeared::getSteer() { From 81010550e33b643eafcaf4b75a445a3aead48bcd Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 5 Feb 2024 23:51:45 +0900 Subject: [PATCH 11/11] Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp --- .../vehicle_model/sim_model_delay_steer_vel.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 68674d631e6a5..1ee08fad566f5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -64,7 +64,6 @@ double SimModelDelaySteerVel::getAx() double SimModelDelaySteerVel::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; - ; } double SimModelDelaySteerVel::getSteer() {