add new vehicle model

Signed-off-by: Yuki Takagi <[email protected]>

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
 )

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

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"

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 <Eigen/Core>
+#include <Eigen/LU>
+
+#include <deque>
+#include <iostream>
+#include <queue>
+
+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<double> acc_input_queue_;       //!< @brief buffer for accel command
+  std::deque<double> 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

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<SimModelDelaySteerAccGearedWoFallGuard>(
+      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,20 +477,23 @@ 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());
   const auto gear = vehicle_model_ptr_->getGear();
 
   // 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;
   }