enable articulate model VehicleModelType for psim

Signed-off-by: Autumn60 <[email protected]>

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<SimModelInterface> vehicle_model_ptr_;  //!< @brief vehicle model pointer
 

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<double> acc_input_queue_;       //!< @brief buffer for accel command
   std::deque<double> 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_

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"

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<std::string> model_module_paths = declare_parameter<std::vector<std::string>>(
     "model_module_paths", std::vector<std::string>({""}));
@@ -297,6 +298,16 @@ void SimplePlanningSimulator::initialize_vehicle_model()
     vehicle_model_ptr_ = std::make_shared<SimModelLearnedSteerVel>(
       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<SimModelDelayArticulateAccGeared>(
+      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);

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_;