From 319d3a4ae9e237acb6bd28a139297785aca8e039 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 20 Sep 2024 21:56:10 +0900 Subject: [PATCH] enable articulate model VehicleModelType for psim Signed-off-by: Autumn60 --- .../simple_planning_simulator_core.hpp | 3 ++- .../sim_model_delay_articulate_acc_geared.hpp | 19 +++++++++---------- .../vehicle_model/sim_model.hpp | 1 + .../simple_planning_simulator_core.cpp | 17 +++++++++++++++-- .../sim_model_delay_articulate_acc_geared.cpp | 15 ++++++++------- 5 files changed, 35 insertions(+), 20 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 86e4346770f0c..886603a826606 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 @@ -206,7 +206,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node DELAY_STEER_VEL = 5, DELAY_STEER_MAP_ACC_GEARED = 6, LEARNED_STEER_VEL = 7, - DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8 + DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8, + DELAY_ARTICULATE_ACC_GEARED = 9 } 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/articulate/sim_model_delay_articulate_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp index 02a9cf02d16ef..c601eaf07b865 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__ARTICULATE__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__ARTICULATE__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_ +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" @@ -33,8 +33,7 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface * @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] front_wheelbase vehicle front wheelbase length [m] - * @param [in] rear_wheelbase vehicle rear wheelbase length [m] + * @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 @@ -78,8 +77,8 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface 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 front_wheelbase_; //!< @brief vehicle front wheelbase length [m] - const double rear_wheelbase_; //!< @brief vehicle rear wheelbase length [m] + const double front_wheelbase_; //!< @brief front wheelbase length [m] + const double rear_wheelbase_; //!< @brief rear wheelbase length [m] std::deque acc_input_queue_; //!< @brief buffer for accel command std::deque steer_input_queue_; //!< @brief buffer for steering command @@ -92,7 +91,7 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command - double state_steer_rate_; + double state_steer_rate_; //!< @brief steering angular velocity [rad/s] /** * @brief set queue buffer for input command @@ -136,7 +135,7 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface double getWz() override; /** - * @brief get vehicle steering(=articulation) angle + * @brief get vehicle steering angle */ double getSteer() override; @@ -147,7 +146,7 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface void update(const double & dt) override; /** - * @brief calculate derivative of states with time delay steering(=articulation) model + * @brief calculate derivative of states with time delay steering model * @param [in] state current model state * @param [in] input input vector to model */ @@ -165,4 +164,4 @@ class SimModelDelayArticulateAccGeared : public SimModelInterface const double dt); }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__ARTICULATE__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_ +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_ 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 2d38ef31498b6..865db705d2da3 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 @@ -15,6 +15,7 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ +#include "simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp" #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" 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 7d09aabdc9abd..79c68a9d68008 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 @@ -234,6 +234,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0); const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; + // const double rear_wheelbase_ratio = declare_parameter("rear_wheelbase_ratio", 1.0); std::vector model_module_paths = declare_parameter>( "model_module_paths", std::vector({""})); @@ -297,6 +298,16 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_ptr_ = std::make_shared( timer_sampling_time_ms_ / 1000.0, model_module_paths, model_param_paths, model_class_names); + } else if (vehicle_model_type_str == "DELAY_ARTICULATE_ACC_GEARED") { + // const double front_wheelbase = wheelbase * (1.0 - rear_wheelbase_ratio); + // const double rear_wheelbase = wheelbase * rear_wheelbase_ratio; + + vehicle_model_type_ = VehicleModelType::DELAY_ARTICULATE_ACC_GEARED; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase * 0.5, wheelbase * 0.5, + 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 { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } @@ -507,7 +518,8 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by } 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) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_ARTICULATE_ACC_GEARED) { input << combined_acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD) { @@ -612,7 +624,8 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & 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) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_ARTICULATE_ACC_GEARED) { state << x, y, yaw, vx, steer, accx; } vehicle_model_ptr_->setState(state); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp index 1993795b7b760..7e46e6c82bdd1 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp @@ -70,11 +70,11 @@ double SimModelDelayArticulateAccGeared::getAx() double SimModelDelayArticulateAccGeared::getWz() { const double steer = state_(IDX::STEER); - const double c_steer = std::cos(steer); - const double pseudo_wheelbase = front_wheelbase_ * c_steer + rear_wheelbase_; + const double front_wheelbase_lon = front_wheelbase_ * std::cos(steer); + const double pseudo_wheelbase = front_wheelbase_lon + rear_wheelbase_; return state_(IDX::VX) * std::sin(steer) / pseudo_wheelbase - - state_steer_rate_ * front_wheelbase_ * c_steer / pseudo_wheelbase; + state_steer_rate_ * front_wheelbase_lon / pseudo_wheelbase; } double SimModelDelayArticulateAccGeared::getSteer() { @@ -140,17 +140,18 @@ Eigen::VectorXd SimModelDelayArticulateAccGeared::calcModel( return 0.0; } }); - state_steer_rate_ = + double state_steer_rate_ = sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); - const double c_steer = std::cos(steer); - const double pseudo_wheelbase = front_wheelbase_ * c_steer + rear_wheelbase_; + const double front_wheelbase_lon = front_wheelbase_ * std::cos(steer); + const double pseudo_wheelbase = front_wheelbase_lon + rear_wheelbase_; 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::sin(steer) / pseudo_wheelbase - - state_steer_rate_ * front_wheelbase_ * c_steer / pseudo_wheelbase; + state_steer_rate_ * front_wheelbase_lon / pseudo_wheelbase; + d_state(IDX::VX) = acc; d_state(IDX::STEER) = state_steer_rate_; d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_;