sim_model_delay_steer_acc_geared.hpp

Go to the documentation of this file.

// 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_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_
 
#include <deque>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>
#include <iostream>
#include <queue>
#include <simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp>
 
class SimModelDelaySteerAccGeared : public SimModelInterface
{
public:
  SimModelDelaySteerAccGeared(
    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 debug_acc_scaling_factor,
    double debug_steer_scaling_factor);
 
  ~SimModelDelaySteerAccGeared() = default;
 
private:
  const double MIN_TIME_CONSTANT;  
 
  enum IDX {
    X = 0,
    Y,
    YAW,
    VX,
    STEER,
    ACCX,
  };
  enum IDX_U {
    ACCX_DES = 0,
    STEER_DES,
    DRIVE_SHIFT,
  };
 
  const double vx_lim_;          
  const double vx_rate_lim_;     
  const double steer_lim_;       
  const double steer_rate_lim_;  
  const double wheelbase_;       
 
  std::deque<double> acc_input_queue_;       
  std::deque<double> steer_input_queue_;     
  const double acc_delay_;                   
  const double acc_time_constant_;           
  const double steer_delay_;                 
  const double steer_time_constant_;         
  const double steer_dead_band_;             
  const double debug_acc_scaling_factor_;    
  const double debug_steer_scaling_factor_;  
 
  void initializeInputQueue(const double & dt);
 
  double getX() override;
 
  double getY() override;
 
  double getYaw() override;
 
  double getVx() override;
 
  double getVy() override;
 
  double getAx() override;
 
  double getWz() override;
 
  double getSteer() override;
 
  void update(const double & dt) override;
 
  Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override;
 
  void updateStateWithGear(
    Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear,
    const double dt);
};
 
#endif  // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_