From c1aa8a7c07c7d466b99de70601f87f34dc40e77d Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 9 Nov 2023 18:31:46 +0900 Subject: [PATCH 1/2] feat(simple_planning_simulator): add acceleration and steer command scaling factor Signed-off-by: kosuke55 --- simulator/simple_planning_simulator/README.md | 28 ++++++++++--------- .../sim_model_delay_steer_acc.hpp | 7 ++++- .../sim_model_delay_steer_acc_geared.hpp | 7 ++++- ...mple_planning_simulator_default.param.yaml | 2 ++ .../simple_planning_simulator_core.cpp | 8 ++++-- .../sim_model_delay_steer_acc.cpp | 13 ++++++--- .../sim_model_delay_steer_acc_geared.cpp | 14 ++++++---- 7 files changed, 53 insertions(+), 26 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 75f77489daada..bb66725c589db 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -67,19 +67,21 @@ The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves base The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). -| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | -| :------------------ | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | -| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] | -| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | +| :------------------- | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | +| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | +| acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | 1.0 | [-] | +| steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | 1.0 | [-] | 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 289c607a18d90..5b95969f6898c 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,11 +40,14 @@ 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] acc_scaling_factor scaling factor for accel command + * @param [in] 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 steer_time_constant, double steer_dead_band, double acc_scaling_factor, + double steer_scaling_factor); /** * @brief default destructor @@ -81,6 +84,8 @@ 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 acc_scaling_factor_; //!< @brief scaling factor for accel command + const double steer_scaling_factor_; //!< @brief scaling factor for steering command /** * @brief set queue buffer for input 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 3bda878f8cd76..9664d4d953fe1 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,11 +40,14 @@ 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] acc_scaling_factor scaling factor for accel command + * @param [in] 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 steer_time_constant, double steer_dead_band, double acc_scaling_factor, + double steer_scaling_factor); /** * @brief default destructor @@ -81,6 +84,8 @@ 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 acc_scaling_factor_; //!< @brief scaling factor for accel command + const double steer_scaling_factor_; //!< @brief scaling factor for steering command /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml index dbd56bf9e9bff..c24b23a1119ed 100644 --- a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -15,6 +15,8 @@ steer_time_delay: 0.1 steer_time_constant: 0.1 steer_dead_band: 0.0 + acc_scaling_factor: 1.0 + steer_scaling_factor: 1.0 x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate 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 f2629a0586045..8fafd24fe5ed2 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 @@ -230,6 +230,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 acc_scaling_factor = declare_parameter("acc_scaling_factor", 1.0); + const double steer_scaling_factor = declare_parameter("steer_scaling_factor", 1.0); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; @@ -251,12 +253,14 @@ void SimplePlanningSimulator::initialize_vehicle_model() 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); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + acc_scaling_factor, 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); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + acc_scaling_factor, steer_scaling_factor); } 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 964157cdeb64c..368a57d2f6019 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,7 +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 steer_time_constant, double steer_dead_band, double acc_scaling_factor, + double steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -31,7 +32,9 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( acc_time_constant_(std::max(acc_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), + acc_scaling_factor_(std::max(acc_scaling_factor, 0.0)), + steer_scaling_factor_(std::max(steer_scaling_factor, 0.0)) { initializeInputQueue(dt); } @@ -104,8 +107,10 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); const double yaw = state(IDX::YAW); const double steer = state(IDX::STEER); - const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); - const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + const double acc_des = + sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * acc_scaling_factor_; + const double steer_des = + sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * steer_scaling_factor_; const double steer_diff = steer - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { 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 76669c9490fc6..206e140ede00a 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,7 +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 steer_time_constant, double steer_dead_band, double acc_scaling_factor, + double steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -33,8 +34,9 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( acc_time_constant_(std::max(acc_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), + acc_scaling_factor_(std::max(acc_scaling_factor, 0.0)), + steer_scaling_factor_(std::max(steer_scaling_factor, 0.0)) { initializeInputQueue(dt); } @@ -113,8 +115,10 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); const double yaw = state(IDX::YAW); const double steer = state(IDX::STEER); - const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); - const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + const double acc_des = + sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * acc_scaling_factor_; + const double steer_des = + sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * steer_scaling_factor_; const double steer_diff = steer - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { From 7880ea62fc0f33edff5850fa686c81922292603e Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 9 Nov 2023 19:02:53 +0900 Subject: [PATCH 2/2] update params as debug Signed-off-by: kosuke55 --- simulator/simple_planning_simulator/README.md | 30 +++++++++---------- .../sim_model_delay_steer_acc.hpp | 26 ++++++++-------- .../sim_model_delay_steer_acc_geared.hpp | 26 ++++++++-------- ...mple_planning_simulator_default.param.yaml | 2 -- .../simple_planning_simulator_core.cpp | 8 ++--- .../sim_model_delay_steer_acc.cpp | 12 ++++---- .../sim_model_delay_steer_acc_geared.cpp | 12 ++++---- 7 files changed, 57 insertions(+), 59 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index bb66725c589db..0c5441c1ad9c8 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -67,21 +67,21 @@ The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves base The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). -| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | -| :------------------- | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | -| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] | -| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | -| acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | 1.0 | [-] | -| steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | 1.0 | [-] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | +| :------------------------- | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | +| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | +| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | 1.0 | [-] | +| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | 1.0 | [-] | 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 5b95969f6898c..92129541ff891 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,14 @@ 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] acc_scaling_factor scaling factor for accel command - * @param [in] steer_scaling_factor scaling factor for steering command + * @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 acc_scaling_factor, - double steer_scaling_factor); + double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, + double debug_steer_scaling_factor); /** * @brief default destructor @@ -77,15 +77,15 @@ class SimModelDelaySteerAcc : public SimModelInterface const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] const double wheelbase_; //!< @brief vehicle wheelbase length [m] - std::deque acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double acc_delay_; //!< @brief time delay for accel command [s] - 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_dead_band_; //!< @brief dead band for steering angle [rad] - const double acc_scaling_factor_; //!< @brief scaling factor for accel command - const double steer_scaling_factor_; //!< @brief scaling factor for steering command + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + 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_dead_band_; //!< @brief dead band for steering angle [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 /** * @brief set queue buffer for input 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 9664d4d953fe1..376ee55f1aa5e 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,14 @@ 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] acc_scaling_factor scaling factor for accel command - * @param [in] steer_scaling_factor scaling factor for steering command + * @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 acc_scaling_factor, - double steer_scaling_factor); + double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, + double debug_steer_scaling_factor); /** * @brief default destructor @@ -77,15 +77,15 @@ class SimModelDelaySteerAccGeared : public SimModelInterface const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] const double wheelbase_; //!< @brief vehicle wheelbase length [m] - std::deque acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double acc_delay_; //!< @brief time delay for accel command [s] - 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_dead_band_; //!< @brief dead band for steering angle [rad] - const double acc_scaling_factor_; //!< @brief scaling factor for accel command - const double steer_scaling_factor_; //!< @brief scaling factor for steering command + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + 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_dead_band_; //!< @brief dead band for steering angle [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 /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml index c24b23a1119ed..dbd56bf9e9bff 100644 --- a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -15,8 +15,6 @@ steer_time_delay: 0.1 steer_time_constant: 0.1 steer_dead_band: 0.0 - acc_scaling_factor: 1.0 - steer_scaling_factor: 1.0 x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate 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 8fafd24fe5ed2..07a5d34f63c39 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 @@ -230,8 +230,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 acc_scaling_factor = declare_parameter("acc_scaling_factor", 1.0); - const double steer_scaling_factor = declare_parameter("steer_scaling_factor", 1.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(); const double wheelbase = vehicle_info.wheel_base_m; @@ -254,13 +254,13 @@ 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, steer_dead_band, - acc_scaling_factor, steer_scaling_factor); + 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, - acc_scaling_factor, steer_scaling_factor); + debug_acc_scaling_factor, debug_steer_scaling_factor); } 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 368a57d2f6019..670671165de60 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 acc_scaling_factor, - double steer_scaling_factor) + double steer_time_constant, double steer_dead_band, 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,8 +33,8 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band), - acc_scaling_factor_(std::max(acc_scaling_factor, 0.0)), - steer_scaling_factor_(std::max(steer_scaling_factor, 0.0)) + debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), + debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { initializeInputQueue(dt); } @@ -108,9 +108,9 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( const double yaw = state(IDX::YAW); const double steer = state(IDX::STEER); const double acc_des = - sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * acc_scaling_factor_; + 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_) * steer_scaling_factor_; + 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_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { 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 206e140ede00a..d72b8bf116d77 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 acc_scaling_factor, - double steer_scaling_factor) + double steer_time_constant, double steer_dead_band, 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,8 +35,8 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band), - acc_scaling_factor_(std::max(acc_scaling_factor, 0.0)), - steer_scaling_factor_(std::max(steer_scaling_factor, 0.0)) + debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), + debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { initializeInputQueue(dt); } @@ -116,9 +116,9 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( const double yaw = state(IDX::YAW); const double steer = state(IDX::STEER); const double acc_des = - sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * acc_scaling_factor_; + 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_) * steer_scaling_factor_; + 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_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) {