read acc map path from config

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp

@@ -38,18 +38,23 @@ class AccelerationMap
   {
     CSVLoader csv(csv_path);
     std::vector<std::vector<std::string>> 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;
     }
 
     vehicle_name_ = table[0][0];
     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<double> 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<std::vector<double>> acceleration_map_;
 
@@ -74,7 +79,7 @@ class AccelerationMap
   std::vector<double> acc_index_;
 };
 
-class SimModelWithConverter : public SimModelInterface
+class SimModelDelaySteerMapAccGeared : public SimModelInterface
 {
 public:
   /**
@@ -90,15 +95,15 @@ 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);
 
   /**
    * @brief default destructor
    */
-  ~SimModelWithConverter() = default;
+  ~SimModelDelaySteerMapAccGeared() = default;
 
   AccelerationMap acc_map_;
 

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"),
             },

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -33,6 +33,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <filesystem>
 #include <iostream>
 #include <memory>
 #include <string>
@@ -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<SimModelWithConverter>(
+    const std::string acceleration_map_path =
+      declare_parameter<std::string>("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<SimModelDelaySteerMapAccGeared>(
       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);

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp

@@ -20,7 +20,7 @@
 
 #include <algorithm>
 
-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<size_t>(round(acc_delay_ / dt));
   acc_input_queue_.resize(acc_input_queue_size);
@@ -106,37 +106,33 @@ 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);
   d_state(IDX::Y) = vel * sin(yaw);
   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;