sim_model_delay_steer_vel.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_VEL_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_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 SimModelDelaySteerVel : public SimModelInterface
{
public:
  SimModelDelaySteerVel(
    double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
    double dt, double vx_delay, double vx_time_constant, double steer_delay,
    double steer_time_constant, double steer_dead_band);
 
  ~SimModelDelaySteerVel() = default;
 
private:
  const double MIN_TIME_CONSTANT;  
 
  enum IDX {
    X = 0,
    Y,
    YAW,
    VX,
    STEER,
  };
  enum IDX_U {
    VX_DES = 0,
    STEER_DES,
  };
 
  const double vx_lim_;          
  const double vx_rate_lim_;     
  const double steer_lim_;       
  const double steer_rate_lim_;  
  const double wheelbase_;       
  double prev_vx_ = 0.0;
  double current_ax_ = 0.0;
 
  std::deque<double> vx_input_queue_;     
  std::deque<double> steer_input_queue_;  
  const double vx_delay_;                 
  const double vx_time_constant_;
  const double steer_delay_;  
  const double
    steer_time_constant_;  
  const double steer_dead_band_;  
 
  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;
};
 
#endif  // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_