diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index 4870c9e900b9c..a63278c258e76 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -38,8 +38,9 @@ class AccelerationMap { CSVLoader csv(csv_path); std::vector> table; - std::cerr << __LINE__ << std::endl; if (!csv.readCSV(table)) { + std::cerr << "[SimModelDelaySteerMapAccGeared]: failed to read acceleration map from " + << csv_path << std::endl; return false; } @@ -47,9 +48,13 @@ class AccelerationMap vel_index_ = CSVLoader::getRowIndex(table); acc_index_ = CSVLoader::getColumnIndex(table); acceleration_map_ = CSVLoader::getMap(table); + + std::cout << "[SimModelDelaySteerMapAccGeared]: success to read acceleration map from " + << csv_path << std::endl; return true; } - bool getAcceleration(const double acc_des, const double vel, double & acc) const + + double getAcceleration(const double acc_des, const double vel) const { std::vector interpolated_acc_vec; const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "acc: vel"); @@ -62,9 +67,9 @@ class AccelerationMap // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc const double clamped_acc = CSVLoader::clampValue(acc_des, acc_index_, "acceleration: acc"); - acc = interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); + const double acc = interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); - return true; + return acc; } std::vector> acceleration_map_; @@ -74,7 +79,7 @@ class AccelerationMap std::vector acc_index_; }; -class SimModelWithConverter : public SimModelInterface +class SimModelDelaySteerMapAccGeared : public SimModelInterface { public: /** @@ -90,7 +95,7 @@ class SimModelWithConverter : public SimModelInterface * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics */ - SimModelWithConverter( + SimModelDelaySteerMapAccGeared( float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, float64_t steer_delay, float64_t steer_time_constant, std::string path); @@ -98,7 +103,7 @@ class SimModelWithConverter : public SimModelInterface /** * @brief default destructor */ - ~SimModelWithConverter() = default; + ~SimModelDelaySteerMapAccGeared() = default; AccelerationMap acc_map_; diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index bf046451b6c72..0070646a20d49 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -18,6 +18,7 @@ from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +import launch_ros.parameter_descriptions from launch_ros.substitutions import FindPackageShare import yaml @@ -35,10 +36,9 @@ def launch_setup(context, *args, **kwargs): vehicle_characteristics_param = yaml.safe_load(f)["/**"]["ros__parameters"] simulator_model_param_path = LaunchConfiguration("simulator_model_param_file").perform(context) - with open(simulator_model_param_path, "r") as f: - simulator_model_param = yaml.safe_load(f)["/**"]["ros__parameters"] - simulator_acc_param_path = LaunchConfiguration("acceleration_param_file").perform(context) - print("ERROR" + simulator_acc_param_path) + simulator_model_param = launch_ros.parameter_descriptions.ParameterFile( + param_file=simulator_model_param_path, allow_substs=True + ) simple_planning_simulator_node = Node( package="simple_planning_simulator", @@ -50,7 +50,6 @@ def launch_setup(context, *args, **kwargs): vehicle_info_param, vehicle_characteristics_param, simulator_model_param, - simulator_acc_param_path, { "initial_engage_state": LaunchConfiguration("initial_engage_state"), }, 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 d84b181f5271f..9a0ea6a6c3659 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 @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -263,12 +264,17 @@ void SimplePlanningSimulator::initialize_vehicle_model() 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; - // TODO consider way to give acceleration map path better way - const std::string acceleration_map_path = declare_parameter( - "acceleration_map_path", - std::string("$HOME/projects/pilot-auto.xx1/src/autoware/universe/simulator/" - "simple_planning_simulator/param/acceleration_map.csv")); - vehicle_model_ptr_ = std::make_shared( + const std::string acceleration_map_path = + declare_parameter("acceleration_map_path"); + if (!std::filesystem::exists(acceleration_map_path)) { + throw std::runtime_error( + "`acceleration_map_path` parameter is necessary for `DELAY_STEER_MAP_ACC_GEARED` simulator " + "model, but " + + acceleration_map_path + + " does not exist. Please confirm that the parameter is set correctly in " + "{simulator_model.param.yaml}."); + } + 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, acceleration_map_path); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 03d23a35dd398..c15cdb27844bf 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -20,7 +20,7 @@ #include -SimModelWithConverter::SimModelWithConverter( +SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, float64_t steer_delay, float64_t steer_time_constant, std::string path) @@ -41,39 +41,39 @@ SimModelWithConverter::SimModelWithConverter( acc_map_.readAccelerationMapFromCSV(path_); } -float64_t SimModelWithConverter::getX() +float64_t SimModelDelaySteerMapAccGeared::getX() { return state_(IDX::X); } -float64_t SimModelWithConverter::getY() +float64_t SimModelDelaySteerMapAccGeared::getY() { return state_(IDX::Y); } -float64_t SimModelWithConverter::getYaw() +float64_t SimModelDelaySteerMapAccGeared::getYaw() { return state_(IDX::YAW); } -float64_t SimModelWithConverter::getVx() +float64_t SimModelDelaySteerMapAccGeared::getVx() { return state_(IDX::VX); } -float64_t SimModelWithConverter::getVy() +float64_t SimModelDelaySteerMapAccGeared::getVy() { return 0.0; } -float64_t SimModelWithConverter::getAx() +float64_t SimModelDelaySteerMapAccGeared::getAx() { return state_(IDX::ACCX); } -float64_t SimModelWithConverter::getWz() +float64_t SimModelDelaySteerMapAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -float64_t SimModelWithConverter::getSteer() +float64_t SimModelDelaySteerMapAccGeared::getSteer() { return state_(IDX::STEER); } -void SimModelWithConverter::update(const float64_t & dt) +void SimModelDelaySteerMapAccGeared::update(const float64_t & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -95,7 +95,7 @@ void SimModelWithConverter::update(const float64_t & dt) updateStateWithGear(state_, prev_state, gear_, dt); } -void SimModelWithConverter::initializeInputQueue(const float64_t & dt) +void SimModelDelaySteerMapAccGeared::initializeInputQueue(const float64_t & dt) { size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); acc_input_queue_.resize(acc_input_queue_size); @@ -106,19 +106,17 @@ void SimModelWithConverter::initializeInputQueue(const float64_t & dt) std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); } -Eigen::VectorXd SimModelWithConverter::calcModel( +Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { - auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); }; - - const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); - const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); + const float64_t vel = std::clamp(state(IDX::VX), -vx_lim_, vx_lim_); + const float64_t acc = std::clamp(state(IDX::ACCX), -vx_rate_lim_, vx_rate_lim_); const float64_t yaw = state(IDX::YAW); const float64_t steer = state(IDX::STEER); - const float64_t acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); - const float64_t steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + const float64_t acc_des = std::clamp(input(IDX_U::ACCX_DES), -vx_rate_lim_, vx_rate_lim_); + const float64_t steer_des = std::clamp(input(IDX_U::STEER_DES), -steer_lim_, steer_lim_); float64_t steer_rate = -(steer - steer_des) / steer_time_constant_; - steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + steer_rate = std::clamp(steer_rate, -steer_rate_lim_, steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); @@ -126,17 +124,15 @@ Eigen::VectorXd SimModelWithConverter::calcModel( d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; d_state(IDX::VX) = acc; d_state(IDX::STEER) = steer_rate; - double converted_acc = acc_des; - if (!acc_map_.acceleration_map_.empty()) { - acc_map_.getAcceleration(acc_des, vel, converted_acc); - } else { - } + const double converted_acc = + acc_map_.acceleration_map_.empty() ? acc_des : acc_map_.getAcceleration(acc_des, vel); + d_state(IDX::ACCX) = -(acc - converted_acc) / acc_time_constant_; return d_state; } -void SimModelWithConverter::updateStateWithGear( +void SimModelDelaySteerMapAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { using autoware_auto_vehicle_msgs::msg::GearCommand;