From b5369395a59d121779cc90e3d251095b30d546ec Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 24 Jun 2024 15:33:52 +0900 Subject: [PATCH] add new vehicle model Signed-off-by: Yuki Takagi --- .../simple_planning_simulator/CMakeLists.txt | 1 + .../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 | 2 +- 7 files changed, 359 insertions(+), 11 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/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..7bd1991e79e93 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 } 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..ab18cac5c3fd0 --- /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 2021 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..383e5d1ba2117 --- /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 2021 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 sutisfied, but we can use Runge-Kutta method with code restruction. + + // 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..0661c5a93d5da 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -270,7 +270,7 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) // clang-format off 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_VEL", "DELAY_STEER_ACC", "DELAY_STEER_ACC_GEARED", "DELAY_STEER_ACC_GEARED", }; // clang-format on