From c103e05f351536904ad20f4036272001bcc8e7ee Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Thu, 27 Jun 2024 17:24:07 +0900 Subject: [PATCH 1/3] feat(simple_planning_simulator): add new vehicle model with falling down (#7651) * add new vehicle model Signed-off-by: Yuki Takagi --- .../simple_planning_simulator/CMakeLists.txt | 1 + simulator/simple_planning_simulator/README.md | 41 ++-- .../simple_planning_simulator_core.hpp | 3 +- .../vehicle_model/sim_model.hpp | 1 + ...l_delay_steer_acc_geared_wo_fall_guard.hpp | 149 ++++++++++++++ .../simple_planning_simulator_core.cpp | 31 ++- ...l_delay_steer_acc_geared_wo_fall_guard.cpp | 183 ++++++++++++++++++ .../test/test_simple_planning_simulator.cpp | 1 + 8 files changed, 380 insertions(+), 30 deletions(-) create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 331d5e819df77..376553aa62917 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -19,6 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp + src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp src/simple_planning_simulator/utils/csv_loader.cpp ) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 4902832f836a9..c3d02da64b882 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -62,6 +62,7 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_VEL` - `DELAY_STEER_ACC` - `DELAY_STEER_ACC_GEARED` +- `DELAY_STEER_ACC_GEARED_WO_FALL_GUARD` - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. - `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learning_based_vehicle_model). @@ -69,26 +70,26 @@ 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 | D_ST_M_ACC_G | L_S_V | Default value | unit | -| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :---- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | x | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | x | 0.24 | [s] | -| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | x | 0.25 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | x | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | x | 0.27 | [s] | -| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | x | x | 0.0 | [rad] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | x | 0.5 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | x | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | x | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | x | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | x | 5.0 | [rad/s] | -| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | x | 0.0 | [rad] | -| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | x | 1.0 | [-] | -| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | 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 | x | - | [-] | -| model_module_paths | string | path to a python module where the model is implemented | x | x | x | x | x | x | x | o | - | [-] | -| model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | o | - | [-] | -| model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | o | - | [-] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_A_G_WO_FG | D_ST_M_ACC_G | L_S_V | Default value | unit | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------- | :----------- | :---- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | o | x | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | o | x | 0.24 | [s] | +| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | x | x | 0.25 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | o | x | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | o | x | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | o | x | x | 0.0 | [rad] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | x | x | 0.5 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | o | x | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | o | x | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | o | x | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | o | x | 5.0 | [rad/s] | +| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | o | x | 0.0 | [rad] | +| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | o | x | x | 1.0 | [-] | +| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | o | x | 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 | x | o | x | - | [-] | +| model_module_paths | string | path to a python module where the model is implemented | x | x | x | x | x | x | x | x | o | - | [-] | +| model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | x | o | - | [-] | +| model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | x | o | - | [-] | _Note:_ Parameters `model_module_paths`, `model_param_paths`, and `model_class_names` need to have the same length. 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 18a3a3bae501a..86e4346770f0c 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 @@ -205,7 +205,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node IDEAL_STEER_VEL = 4, DELAY_STEER_VEL = 5, DELAY_STEER_MAP_ACC_GEARED = 6, - LEARNED_STEER_VEL = 7 + LEARNED_STEER_VEL = 7, + DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8 } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index ced7349f28914..2d38ef31498b6 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -17,6 +17,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp new file mode 100644 index 0000000000000..d7a6a8284d704 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp @@ -0,0 +1,149 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include + +#include +#include +#include + +class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface +{ +public: + /** + * @param [in] vx_lim velocity limit [m/s] + * @param [in] steer_lim steering limit [rad] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] + * @param [in] wheelbase vehicle wheelbase length [m] + * @param [in] dt delta time information to set input buffer for delay + * @param [in] acc_delay time delay for accel command [s] + * @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_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 + */ + SimModelDelaySteerAccGearedWoFallGuard( + 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_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor); + + /** + * @brief default destructor + */ + ~SimModelDelaySteerAccGearedWoFallGuard() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + PEDAL_ACCX, + }; + enum IDX_U { PEDAL_ACCX_DES = 0, GEAR, SLOPE_ACCX, STEER_DES }; + + const double vx_lim_; //!< @brief velocity limit [m/s] + const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const double steer_lim_; //!< @brief steering limit [rad] + 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 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 + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + double getVy() override; + + /** + * @brief get vehicle longitudinal acceleration + */ + double getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time delay steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; +}; + +// clang-format off +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT +// clang-format on 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 71c89fcf4df42..7d09aabdc9abd 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 @@ -269,6 +269,12 @@ void SimplePlanningSimulator::initialize_vehicle_model() 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, steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor); + } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED_WO_FALL_GUARD") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD; + 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, + 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 = @@ -471,7 +477,7 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by { const auto steer = cmd.lateral.steering_tire_angle; const auto vel = cmd.longitudinal.velocity; - const auto accel = cmd.longitudinal.acceleration; + const auto acc_by_cmd = cmd.longitudinal.acceleration; using autoware_vehicle_msgs::msg::GearCommand; Eigen::VectorXd input(vehicle_model_ptr_->getDimU()); @@ -479,12 +485,15 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by // TODO(Watanabe): The definition of the sign of acceleration in REVERSE mode is different // between .auto and proposal.iv, and will be discussed later. - float acc = accel + acc_by_slope; - if (gear == GearCommand::NONE) { - acc = 0.0; - } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { - acc = -accel - acc_by_slope; - } + const float combined_acc = [&] { + if (gear == GearCommand::NONE) { + return 0.0; + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { + return -acc_by_cmd + acc_by_slope; + } else { + return acc_by_cmd + acc_by_slope; + } + }(); if ( vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL || @@ -494,12 +503,15 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { - input << acc, steer; + input << combined_acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED || vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) { - input << acc, steer; + input << combined_acc, steer; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD) { + input << acc_by_cmd, gear, acc_by_slope, steer; } vehicle_model_ptr_->setInput(input); } @@ -599,6 +611,7 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD || vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) { state << x, y, yaw, vx, steer, accx; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp new file mode 100644 index 0000000000000..c699d704724f5 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp @@ -0,0 +1,183 @@ +// Copyright 2024 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp" + +#include "autoware_vehicle_msgs/msg/gear_command.hpp" + +#include + +SimModelDelaySteerAccGearedWoFallGuard::SimModelDelaySteerAccGearedWoFallGuard( + 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_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor) +: SimModelInterface(6 /* dim x */, 4 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), + acc_delay_(acc_delay), + 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_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)) +{ + initializeInputQueue(dt); +} + +double SimModelDelaySteerAccGearedWoFallGuard::getX() +{ + return state_(IDX::X); +} +double SimModelDelaySteerAccGearedWoFallGuard::getY() +{ + return state_(IDX::Y); +} +double SimModelDelaySteerAccGearedWoFallGuard::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelDelaySteerAccGearedWoFallGuard::getVx() +{ + return state_(IDX::VX); +} +double SimModelDelaySteerAccGearedWoFallGuard::getVy() +{ + return 0.0; +} +double SimModelDelaySteerAccGearedWoFallGuard::getAx() +{ + return state_(IDX::PEDAL_ACCX); +} +double SimModelDelaySteerAccGearedWoFallGuard::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + ; +} +double SimModelDelaySteerAccGearedWoFallGuard::getSteer() +{ + // return measured values with bias added to actual values + return state_(IDX::STEER) + steer_bias_; +} +void SimModelDelaySteerAccGearedWoFallGuard::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::PEDAL_ACCX_DES)); + delayed_input(IDX_U::PEDAL_ACCX_DES) = acc_input_queue_.front(); + acc_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); + delayed_input(IDX_U::GEAR) = input_(IDX_U::GEAR); + delayed_input(IDX_U::SLOPE_ACCX) = input_(IDX_U::SLOPE_ACCX); + + const auto prev_state = state_; + updateEuler(dt, delayed_input); + // we cannot use updateRungeKutta() because the differentiability or the continuity condition is + // not satisfied, but we can use Runge-Kutta method with code reconstruction. + + // take velocity limit explicitly + state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); + + if ( + prev_state(IDX::VX) * state_(IDX::VX) <= 0.0 && + -state_(IDX::PEDAL_ACCX) >= std::abs(delayed_input(IDX_U::SLOPE_ACCX))) { + // stop condition is satisfied + state_(IDX::VX) = 0.0; + } +} + +void SimModelDelaySteerAccGearedWoFallGuard::initializeInputQueue(const double & dt) +{ + size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); + acc_input_queue_.resize(acc_input_queue_size); + std::fill(acc_input_queue_.begin(), acc_input_queue_.end(), 0.0); + + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + steer_input_queue_.resize(steer_input_queue_size); + std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); +} + +Eigen::VectorXd SimModelDelaySteerAccGearedWoFallGuard::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; + + const double vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const double pedal_acc = sat(state(IDX::PEDAL_ACCX), vx_rate_lim_, -vx_rate_lim_); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + const double pedal_acc_des = + sat(input(IDX_U::PEDAL_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_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, 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) / wheelbase_; + d_state(IDX::VX) = [&] { + if (pedal_acc >= 0.0) { + using autoware_vehicle_msgs::msg::GearCommand; + const auto gear = input(IDX_U::GEAR); + if (gear == GearCommand::NONE || gear == GearCommand::PARK) { + return 0.0; + } else if (gear == GearCommand::NEUTRAL) { + return input(IDX_U::SLOPE_ACCX); + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { + return -pedal_acc + input(IDX_U::SLOPE_ACCX); + } else { + return pedal_acc + input(IDX_U::SLOPE_ACCX); + } + } else { + constexpr double brake_dead_band = 1e-3; + if (vel > brake_dead_band) { + return pedal_acc + input(IDX_U::SLOPE_ACCX); + } else if (vel < -brake_dead_band) { + return -pedal_acc + input(IDX_U::SLOPE_ACCX); + } else if (-pedal_acc >= std::abs(input(IDX_U::SLOPE_ACCX))) { + return 0.0; + } else { + return input(IDX_U::SLOPE_ACCX); + } + } + }(); + d_state(IDX::STEER) = steer_rate; + d_state(IDX::PEDAL_ACCX) = -(pedal_acc - pedal_acc_des) / acc_time_constant_; + + return d_state; +} diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index dc49bf17a11fc..c61237f83e5bd 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -271,6 +271,7 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) const std::string VEHICLE_MODEL_LIST[] = { // NOLINT "IDEAL_STEER_VEL", "IDEAL_STEER_ACC", "IDEAL_STEER_ACC_GEARED", "DELAY_STEER_VEL", "DELAY_STEER_ACC", "DELAY_STEER_ACC_GEARED", + "DELAY_STEER_ACC_GEARED_WO_FALL_GUARD" }; // clang-format on From 7771ef6e7fe7faf3509b5e3e2ce4a866008ff469 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 22 Jul 2024 13:54:18 +0900 Subject: [PATCH 2/3] refactor(vehicle_cmd_gate)!: delete rate limit skipping function for vehicle departure (#7720) * delete a fucntion block. More appropriate function can be achieved by the parameter settings. * add notation to readme --------- Signed-off-by: Yuki Takagi --- control/autoware_vehicle_cmd_gate/README.md | 4 ++++ .../config/vehicle_cmd_gate.param.yaml | 16 ++++++++-------- .../src/vehicle_cmd_gate.cpp | 18 ------------------ 3 files changed, 12 insertions(+), 26 deletions(-) diff --git a/control/autoware_vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md index 2d6f5c5f949af..e46db3c06cfeb 100644 --- a/control/autoware_vehicle_cmd_gate/README.md +++ b/control/autoware_vehicle_cmd_gate/README.md @@ -78,6 +78,10 @@ The limitation values are calculated based on the 1D interpolation of the limita Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published. +Notation 2: If you use vehicles in which the driving force is controlled by the accelerator/brake pedal, the jerk limit, denoting the pedal rate limit, must be sufficiently relaxed at low speeds. +Otherwise, quick pedal changes at start/stop will not be possible, resulting in slow starts and creep down on hills. +This functionality for starting/stopping was embedded in the source code but was removed because it was complex and could be achieved by parameters. + ## Assumptions / Known limits The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 54c87f45b6a96..74affea696893 100644 --- a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -16,14 +16,14 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - reference_speed_points: [20.0, 30.0] - steer_lim: [1.0, 0.8] - steer_rate_lim: [1.0, 0.8] - lon_acc_lim: [5.0, 4.0] - lon_jerk_lim: [5.0, 4.0] - lat_acc_lim: [5.0, 4.0] - lat_jerk_lim: [7.0, 6.0] - actual_steer_diff_lim: [1.0, 0.8] + reference_speed_points: [0.1, 0.3, 20.0, 30.0] + steer_lim: [1.0, 1.0, 1.0, 0.8] + steer_rate_lim: [1.0, 1.0, 1.0, 0.8] + lon_acc_lim: [5.0, 5.0, 5.0, 4.0] + lon_jerk_lim: [80.0, 5.0, 5.0, 4.0] # The first element is required for quick pedal changes when stopping and starting. + lat_acc_lim: [5.0, 5.0, 5.0, 4.0] + lat_jerk_lim: [7.0, 7.0, 7.0, 6.0] + actual_steer_diff_lim: [1.0, 1.0, 1.0, 0.8] on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 87e79f59bc356..dd6e2f0f54aea 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -607,7 +607,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_); - const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0; filter_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); filter_on_transition_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); @@ -618,23 +617,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) if (mode.is_in_transition) { filter_on_transition_.filterAll(dt, current_steer_, out, is_filter_activated); } else { - // When ego is stopped and the input command is not stopping, - // use the higher of actual vehicle longitudinal state - // and previous longitudinal command for the filtering - // this is to prevent the jerk limits being applied and causing - // a delay when restarting the vehicle. - - if (ego_is_stopped && !input_cmd_is_stopping) { - auto prev_cmd = filter_.getPrevCmd(); - prev_cmd.longitudinal.acceleration = - std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration); - // consider reverse driving - prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) > - std::fabs(current_status_cmd.longitudinal.velocity) - ? prev_cmd.longitudinal.velocity - : current_status_cmd.longitudinal.velocity; - filter_.setPrevCmd(prev_cmd); - } filter_.filterAll(dt, current_steer_, out, is_filter_activated); } From 5a2974590cc731e7738b622a2d5208d5b083fa99 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 24 Jul 2024 13:53:27 +0900 Subject: [PATCH 3/3] feat(pid_longitudinal_controller): re-organize diff limit structure and fix state change condition (#7718) change diff limit structure change stopped condition define a new param Signed-off-by: Yuki Takagi --- .../pid_longitudinal_controller.hpp | 4 +- ...ongitudinal_controller_defaults.param.yaml | 6 +- .../src/pid_longitudinal_controller.cpp | 164 ++++++++++-------- .../param/longitudinal/pid.param.yaml | 6 +- 4 files changed, 95 insertions(+), 85 deletions(-) diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 39c75964dbc72..61c730eb257e7 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -183,7 +183,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro { double vel; double acc; - double jerk; }; StoppedStateParams m_stopped_state_params; @@ -203,6 +202,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // jerk limit double m_max_jerk; double m_min_jerk; + double m_max_acc_cmd_diff; // slope compensation enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE }; @@ -291,7 +291,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @brief calculate control command in emergency state * @param [in] dt time between previous and current one */ - Motion calcEmergencyCtrlCmd(const double dt) const; + Motion calcEmergencyCtrlCmd(const double dt); /** * @brief update control state according to the current situation diff --git a/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml index ad6217663fbae..5027c94afe7c1 100644 --- a/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -53,12 +53,11 @@ # stopped state stopped_vel: 0.0 - stopped_acc: -3.4 - stopped_jerk: -5.0 + stopped_acc: -3.4 # denotes pedal position # emergency state emergency_vel: 0.0 - emergency_acc: -5.0 + emergency_acc: -5.0 # denotes acceleration emergency_jerk: -3.0 # acceleration limit @@ -68,6 +67,7 @@ # jerk limit max_jerk: 2.0 min_jerk: -5.0 + max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1] # slope compensation lpf_pitch_gain: 0.95 diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 09f4a90e14898..20f8dfe642cc6 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -150,9 +150,8 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters for stop state { auto & p = m_stopped_state_params; - p.vel = node.declare_parameter("stopped_vel"); // [m/s] - p.acc = node.declare_parameter("stopped_acc"); // [m/s^2] - p.jerk = node.declare_parameter("stopped_jerk"); // [m/s^3] + p.vel = node.declare_parameter("stopped_vel"); // [m/s] + p.acc = node.declare_parameter("stopped_acc"); // [m/s^2] } // parameters for emergency state @@ -168,8 +167,9 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) m_min_acc = node.declare_parameter("min_acc"); // [m/s^2] // parameters for jerk limit - m_max_jerk = node.declare_parameter("max_jerk"); // [m/s^3] - m_min_jerk = node.declare_parameter("min_jerk"); // [m/s^3] + m_max_jerk = node.declare_parameter("max_jerk"); // [m/s^3] + m_min_jerk = node.declare_parameter("min_jerk"); // [m/s^3] + m_max_acc_cmd_diff = node.declare_parameter("max_acc_cmd_diff"); // [m/s^3] // parameters for slope compensation m_adaptive_trajectory_velocity_th = @@ -353,7 +353,6 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac auto & p = m_stopped_state_params; update_param("stopped_vel", p.vel); update_param("stopped_acc", p.acc); - update_param("stopped_jerk", p.jerk); } // emergency state @@ -370,6 +369,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // jerk limit update_param("max_jerk", m_max_jerk); update_param("min_jerk", m_min_jerk); + update_param("max_acc_cmd_diff", m_max_acc_cmd_diff); // slope compensation update_param("max_pitch_rad", m_max_pitch_rad); @@ -565,26 +565,30 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData return control_data; } -PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd( - const double dt) const +PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd(const double dt) { // These accelerations are without slope compensation const auto & p = m_emergency_state_params; - const double vel = - longitudinal_utils::applyDiffLimitFilter(p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); - const double acc = - longitudinal_utils::applyDiffLimitFilter(p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); + Motion raw_ctrl_cmd{p.vel, p.acc}; + + raw_ctrl_cmd.vel = + longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); + raw_ctrl_cmd.acc = std::clamp(raw_ctrl_cmd.acc, m_min_acc, m_max_acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, raw_ctrl_cmd.acc); + raw_ctrl_cmd.acc = + longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc); RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); + logger_, *clock_, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, + raw_ctrl_cmd.acc); - return Motion{vel, acc}; + return raw_ctrl_cmd; } void PidLongitudinalController::updateControlState(const ControlData & control_data) { const double current_vel = control_data.current_motion.vel; - const double current_acc = control_data.current_motion.acc; const double stop_dist = control_data.stop_dist; // flags for state transition @@ -599,8 +603,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; - const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && - std::abs(current_acc) < p.stopped_state_entry_acc; + const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel; + // Case where the ego slips in the opposite direction of the gear due to e.g. a slope is also // considered as a stop const bool is_not_running = [&]() { @@ -701,7 +705,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); // prevent the car from taking a long time to start to move - m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); + m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_raw_ctrl_cmd.acc); return changeState(ControlState::DRIVE); } return; @@ -747,8 +751,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); - // prevent the car from taking a long time to start to move - m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); return changeState(ControlState::DRIVE); } @@ -780,55 +782,85 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const size_t target_idx = control_data.target_idx; // velocity and acceleration command - Motion raw_ctrl_cmd{ + Motion ctrl_cmd_as_pedal_pos{ control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps, control_data.interpolated_traj.points.at(target_idx).acceleration_mps2}; - if (m_control_state == ControlState::DRIVE) { - raw_ctrl_cmd.vel = - control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; - raw_ctrl_cmd.acc = applyVelocityFeedback(control_data); - raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx); + if (m_control_state == ControlState::STOPPED) { + const auto & p = m_stopped_state_params; + ctrl_cmd_as_pedal_pos.vel = p.vel; + ctrl_cmd_as_pedal_pos.acc = p.acc; - RCLCPP_DEBUG( - logger_, - "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " - "feedback_ctrl_cmd.ac: %3.3f", - raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, control_data.current_motion.vel, - control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps, - raw_ctrl_cmd.acc); - } else if (m_control_state == ControlState::STOPPING) { - raw_ctrl_cmd.acc = m_smooth_stop.calculate( - control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, - m_vel_hist, m_delay_compensation_time); - raw_ctrl_cmd.vel = m_stopped_state_params.vel; + m_prev_raw_ctrl_cmd.vel = 0.0; + m_prev_raw_ctrl_cmd.acc = 0.0; + + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, ctrl_cmd_as_pedal_pos.acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, ctrl_cmd_as_pedal_pos.acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, ctrl_cmd_as_pedal_pos.acc); RCLCPP_DEBUG( - logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, - raw_ctrl_cmd.acc); - } else if (m_control_state == ControlState::STOPPED) { - // This acceleration is without slope compensation - const auto & p = m_stopped_state_params; - raw_ctrl_cmd.vel = p.vel; - raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter( - p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); + logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", ctrl_cmd_as_pedal_pos.vel, + ctrl_cmd_as_pedal_pos.acc); + } else { + Motion raw_ctrl_cmd{ + control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps, + control_data.interpolated_traj.points.at(target_idx).acceleration_mps2}; + if (m_control_state == ControlState::EMERGENCY) { + raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); + } else { + if (m_control_state == ControlState::DRIVE) { + raw_ctrl_cmd.vel = control_data.interpolated_traj.points.at(control_data.target_idx) + .longitudinal_velocity_mps; + raw_ctrl_cmd.acc = applyVelocityFeedback(control_data); + raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx); + + RCLCPP_DEBUG( + logger_, + "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " + "feedback_ctrl_cmd.ac: %3.3f", + raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, control_data.current_motion.vel, + control_data.interpolated_traj.points.at(control_data.target_idx) + .longitudinal_velocity_mps, + raw_ctrl_cmd.acc); + } else if (m_control_state == ControlState::STOPPING) { + raw_ctrl_cmd.acc = m_smooth_stop.calculate( + control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, + m_vel_hist, m_delay_compensation_time); + raw_ctrl_cmd.vel = m_stopped_state_params.vel; + + RCLCPP_DEBUG( + logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, + raw_ctrl_cmd.acc); + } + raw_ctrl_cmd.acc = std::clamp(raw_ctrl_cmd.acc, m_min_acc, m_max_acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, raw_ctrl_cmd.acc); + raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter( + raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc); + } + + // store acceleration without slope compensation + m_prev_raw_ctrl_cmd = raw_ctrl_cmd; - RCLCPP_DEBUG(logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); - } else if (m_control_state == ControlState::EMERGENCY) { - raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); + ctrl_cmd_as_pedal_pos.acc = + applySlopeCompensation(raw_ctrl_cmd.acc, control_data.slope_angle, control_data.shift); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, ctrl_cmd_as_pedal_pos.acc); + ctrl_cmd_as_pedal_pos.vel = raw_ctrl_cmd.vel; } - // store acceleration without slope compensation - m_prev_raw_ctrl_cmd = raw_ctrl_cmd; + storeAccelCmd(m_prev_raw_ctrl_cmd.acc); - // apply slope compensation and filter acceleration and jerk - const double filtered_acc_cmd = calcFilteredAcc(raw_ctrl_cmd.acc, control_data); - const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd}; + ctrl_cmd_as_pedal_pos.acc = longitudinal_utils::applyDiffLimitFilter( + ctrl_cmd_as_pedal_pos.acc, m_prev_ctrl_cmd.acc, control_data.dt, m_max_acc_cmd_diff); // update debug visualization updateDebugVelAcc(control_data); - return filtered_ctrl_cmd; + RCLCPP_DEBUG( + logger_, "[final output]: acc: %3.3f, v_curr: %3.3f", ctrl_cmd_as_pedal_pos.acc, + control_data.current_motion.vel); + + return ctrl_cmd_as_pedal_pos; } // Do not use nearest_idx here @@ -912,28 +944,6 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift return m_prev_shift; } -double PidLongitudinalController::calcFilteredAcc( - const double raw_acc, const ControlData & control_data) -{ - const double acc_max_filtered = std::clamp(raw_acc, m_min_acc, m_max_acc); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered); - - // store ctrl cmd without slope filter - storeAccelCmd(acc_max_filtered); - - // apply slope compensation - const double acc_slope_filtered = - applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); - - // This jerk filter must be applied after slope compensation - const double acc_jerk_filtered = longitudinal_utils::applyDiffLimitFilter( - acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); - - return acc_jerk_filtered; -} - void PidLongitudinalController::storeAccelCmd(const double accel) { if (m_control_state == ControlState::DRIVE) { diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index ad6217663fbae..5027c94afe7c1 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -53,12 +53,11 @@ # stopped state stopped_vel: 0.0 - stopped_acc: -3.4 - stopped_jerk: -5.0 + stopped_acc: -3.4 # denotes pedal position # emergency state emergency_vel: 0.0 - emergency_acc: -5.0 + emergency_acc: -5.0 # denotes acceleration emergency_jerk: -3.0 # acceleration limit @@ -68,6 +67,7 @@ # jerk limit max_jerk: 2.0 min_jerk: -5.0 + max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1] # slope compensation lpf_pitch_gain: 0.95