diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 376553aa62917..af3983c657bb9 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -21,6 +21,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED 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/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp src/simple_planning_simulator/utils/csv_loader.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learning_based_vehicle_model_INCLUDE_DIRS}) 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 new file mode 100644 index 0000000000000..cefcdb5e1b71f --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.hpp @@ -0,0 +1,167 @@ +// 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__ARTICULATE__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__ARTICULATE__SIM_MODEL_DELAY_ARTICULATE_ACC_GEARED_HPP_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include + +#include +#include +#include + +class SimModelDelayArticulateAccGeared : public SimModelInterface +{ +public: + /** + * @brief constructor + * @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 + */ + SimModelDelayArticulateAccGeared( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, + double front_wheelbase, double rear_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 + */ + ~SimModelDelayArticulateAccGeared() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U { + ACCX_DES = 0, + STEER_DES, + DRIVE_SHIFT, + }; + + 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 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 + 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 + + double state_steer_rate_; //!< @brief steering angular velocity [rad/s] + + /** + * @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; + + /** + * @brief update state considering current gear + * @param [in] state current state + * @param [in] prev_state previous state + * @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand) + * @param [in] dt delta time to update state + */ + void updateStateWithGear( + Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, + const double dt); +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__ARTICULATE__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..bc74942fbc291 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, front_wheelbase, rear_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 { 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 new file mode 100644 index 0000000000000..7e46e6c82bdd1 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/articulate/sim_model_delay_articulate_acc_geared.cpp @@ -0,0 +1,194 @@ +// 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/articulate/sim_model_delay_articulate_acc_geared.hpp" + +#include "autoware_vehicle_msgs/msg/gear_command.hpp" + +#include + +SimModelDelayArticulateAccGeared::SimModelDelayArticulateAccGeared( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, + double front_wheelbase, double rear_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 */, 2 /* 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), + front_wheelbase_(front_wheelbase), + rear_wheelbase_(rear_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 SimModelDelayArticulateAccGeared::getX() +{ + return state_(IDX::X); +} +double SimModelDelayArticulateAccGeared::getY() +{ + return state_(IDX::Y); +} +double SimModelDelayArticulateAccGeared::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelDelayArticulateAccGeared::getVx() +{ + return state_(IDX::VX); +} +double SimModelDelayArticulateAccGeared::getVy() +{ + return 0.0; +} +double SimModelDelayArticulateAccGeared::getAx() +{ + return state_(IDX::ACCX); +} +double SimModelDelayArticulateAccGeared::getWz() +{ + const double steer = state_(IDX::STEER); + 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_lon / pseudo_wheelbase; +} +double SimModelDelayArticulateAccGeared::getSteer() +{ + // return measured values with bias added to actual values + return state_(IDX::STEER) + steer_bias_; +} +void SimModelDelayArticulateAccGeared::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); + delayed_input(IDX_U::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(); + + const auto prev_state = state_; + updateRungeKutta(dt, delayed_input); + + // take velocity limit explicitly + state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); + + // consider gear + // update position and velocity first, and then acceleration is calculated naturally + updateStateWithGear(state_, prev_state, gear_, dt); +} + +void SimModelDelayArticulateAccGeared::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 SimModelDelayArticulateAccGeared::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 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_) * 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; + } + }); + double state_steer_rate_ = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); + + 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_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_; + + return d_state; +} + +void SimModelDelayArticulateAccGeared::updateStateWithGear( + Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) +{ + const auto setStopState = [&]() { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + }; + + using autoware_vehicle_msgs::msg::GearCommand; + if ( + gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || + gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || + gear == GearCommand::DRIVE_7 || gear == GearCommand::DRIVE_8 || gear == GearCommand::DRIVE_9 || + gear == GearCommand::DRIVE_10 || gear == GearCommand::DRIVE_11 || + gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || + gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || + gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { + if (state(IDX::VX) < 0.0) { + setStopState(); + } + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { + if (state(IDX::VX) > 0.0) { + setStopState(); + } + } else if (gear == GearCommand::PARK) { + setStopState(); + } else { + setStopState(); + } +}