trajectory_simulator.h

Go to the documentation of this file.

/***************************************************************************
 *   Software License Agreement (BSD License)                              *
 *   Copyright (C) 2016 by Horatiu George Todoran <todorangrg@gmail.com>   *
 *                                                                         *
 *   Redistribution and use in source and binary forms, with or without    *
 *   modification, are permitted provided that the following conditions    *
 *   are met:                                                              *
 *                                                                         *
 *   1. Redistributions of source code must retain the above copyright     *
 *      notice, this list of conditions and the following disclaimer.      *
 *   2. Redistributions in binary form must reproduce the above copyright  *
 *      notice, this list of conditions and the following disclaimer in    *
 *      the documentation and/or other materials provided with the         *
 *      distribution.                                                      *
 *   3. Neither the name of the copyright holder nor the names of its      *
 *      contributors may be used to endorse or promote products derived    *
 *      from this software without specific prior written permission.      *
 *                                                                         *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS   *
 *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT     *
 *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS     *
 *   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE        *
 *   COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,  *
 *   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,  *
 *   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;      *
 *   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER      *
 *   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT    *
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY *
 *   WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           *
 *   POSSIBILITY OF SUCH DAMAGE.                                           *
 ***************************************************************************/

#ifndef TRAJECTORY_SIMULATOR_H
#define TRAJECTORY_SIMULATOR_H

#include <tuw_control/state/state_sim.h>
#include <tuw_control/param_func/param_func_dist.h>
#include <tuw_control/costs_evaluator/costs_evaluator.hpp>

#include <functional>
#include <memory>

namespace tuw
{
class TrajectorySimulator;
using TrajectorySimulatorSPtr = std::shared_ptr<TrajectorySimulator>;
using TrajectorySimulatorConstSPtr = std::shared_ptr<TrajectorySimulator const>;
using TrajectorySimulatorUPtr = std::unique_ptr<TrajectorySimulator>;
using TrajectorySimulatorConstUPtr = std::unique_ptr<TrajectorySimulator const>;

class TrajectorySimulator
{
  // enums
public:
  enum class SimMode
  {
    ONLINE,  
    PRECALC  
  };
public:
  enum class BaseSimLatticeType : signed int
  {
    ARC_BG_BK = -3,          
    LATTICE_ARC_EQ_DT = -2,  
    LATTICE_ARC_EQ_DS = -1,  
    LATTICE_ENUM_SIZE = 3    
  };
  static constexpr const std::size_t extArcLatIdxBegin =
      asInt(BaseSimLatticeType::LATTICE_ENUM_SIZE) + asInt(BaseSimLatticeType::LATTICE_ARC_EQ_DS);

public:
  struct LatticePoint
  {
    LatticePoint() : arc(-1), statePtr(nullptr)
    {
    }
    LatticePoint(double _arc) : arc(_arc), statePtr(nullptr)
    {
    }
    LatticePoint(double _arc, StateSPtr& _statePtr) : arc(_arc), statePtr(_statePtr)
    {
    }
    virtual ~LatticePoint()
    {
    }
    double arc;
    StateSPtr statePtr;
  };

  using LatticeVec = std::vector<LatticePoint>;
  using LatticeVecSPtr = std::shared_ptr<std::vector<LatticePoint> >;
  using LatticeVecSPtrVec = std::vector<std::shared_ptr<std::vector<LatticePoint> > >;

public:
  struct LatticePointType : public LatticePoint
  {
    LatticePointType() : LatticePoint(), latticeType(-5)
    {
    }
    LatticePointType(double _arc, int _latticeType) : LatticePoint(_arc), latticeType(_latticeType)
    {
    }
    LatticePointType(double _arc, int _latticeType, StateSPtr& _statePtr)
      : LatticePoint(_arc, _statePtr), latticeType(_latticeType)
    {
    }
    int latticeType;
  };

public:
  using CostsEvaluatorClass = CostsEvaluatorBase<TrajectorySimulator::LatticeVec>;

  // special class member functions
public:
  TrajectorySimulator(StateSimPtr _stateSim);

public:
  TrajectorySimulator(StateSimPtr _stateSim, std::unique_ptr<CostsEvaluatorClass> _costsEvaluator);

public:
  virtual ~TrajectorySimulator() = default;

public:
  TrajectorySimulator(const TrajectorySimulator&) = default;

public:
  TrajectorySimulator& operator=(const TrajectorySimulator&) = default;

public:
  TrajectorySimulator(TrajectorySimulator&&) = default;

public:
  TrajectorySimulator& operator=(TrajectorySimulator&&) = default;

public:
  void setBoolDtScale(const bool& _doScale);

public:
  void setBoolDsScale(const bool& _doScale);

public:
  double& dtBase();
public:
  const double& dt() const;
public:
  double& dsBase();
public:
  const double& ds() const;
public:
  StateSimPtr stateSim();
public:
  const StateSimPtr stateSim() const;
public:
  std::vector<LatticePointType>& simLattice();
public:
  const std::vector<LatticePointType>& simLattice() const;
public:
  void setUserDefLattice(const std::vector<std::vector<double*> >& _userDefLattices);

public:
  void updateUserDefLattice();

  // pure virtual function
public:
  virtual void simulateTrajectory(double _lastValidArc = 0) = 0;

protected:
  virtual void populatePartSimLatticesDtOnly(const size_t& _firstLaticeInvalidIdx, double _arcParamMax);
protected:
  virtual void populatePartSimLatticesGeneral(size_t _firstLaticeInvalidIdx, double _arcParamMax,
                                              double _minArcLatticeVal) = 0;

protected:
  void simAppendToSimPartLat(const double& _arcNow, const int& _latticePtType, const std::size_t& _minArcLatCacheIdx);
protected:
  void appendToPartLat(const double& _arcNow, const int& _latticePtType, const std::size_t& _minArcLatCacheIdx);
protected:
  void setBeginStateToLattices(const double& _arcBegin);
protected:
  void setEndStateToLattices(const double& _arcEnd);
protected:
  void setBeginEndArcsToLattices(const double& _arcBegin, const double& _arcEnd);

protected:
  bool initSimLatticeState0(const double& _lastValidArc, size_t& _firstLaticeInvalidIdx);
protected:
  void initExtLatticeCache(const double& _minArcLatticeVal);
protected:
  void populateTrajSimPartLattice(const size_t& _firstLaticeInvalidIdx);

protected:
  bool isEmptyAllExtLattices() const;
public:
  static size_t lattTypeIdx(int _enumIdx);

protected:
  void computeScaleDtDs();

protected:
  StateSimPtr stateSim_;
public:
  std::vector<LatticePointType> simulationLattice_;
public:
  LatticeVecSPtrVec partLattices_;
protected:
  std::vector<int> partLatIdxCache_;
protected:
  bool canComputeDistLattice_;
private:
  double dt_;
private:
  double ds_;

private:
  double dtBase_;

private:
  double dsBase_;

protected:
  bool scaleDt_;

protected:
  bool scaleDs_;

private:
  std::vector<std::vector<double*> > userDefPartLattices_;

public:
  std::unique_ptr<CostsEvaluatorClass> costsEvaluator_;

  friend class TrajectorySimGrade;
  //     public   : size_t partLatticeActiveSize_;
};
}

#endif  // TRAJECTORY_SIMULATOR_H