envInterval.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Maxim Likhachev
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the University of Pennsylvania 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 OWNER 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 __ENVINTERVAL_H_
#define __ENVINTERVAL_H_

#include <sbpl_dynamic_planner/DiscreteSpaceTimeIntervalInformation.h>
#include <sbpl_dynamic_planner/sbpl_dynamicObstacles.h>

//eight-connected grid
#define ENVINTERVALLAT_DXYWIDTH 8

#define ENVINTERVALLAT_DEFAULTOBSTHRESH 254     //see explanation of the value below
#define ENVINTERVALLAT_DEFAULTDYNOBSTHRESH 254  //see explanation of the value below


//definition of theta orientations
//0 - is aligned with X-axis in the positive direction (1,0 in polar coordinates)
//theta increases as we go counterclockwise
//number of theta values - should be power of 2
#define ENVINTERVALLAT_THETADIRS 16

//number of actions per x,y,theta state
#define ENVINTERVALLAT_DEFAULT_ACTIONWIDTH 5 //decrease, increase, same angle while moving plus decrease, increase angle while standing.

#define ENVINTERVALLAT_COSTMULT_MTOMM 1000

#define CONTTIME2DISC(X, CELLSIZE) (((X)>=0)?((int)((X)/(CELLSIZE)+0.5)):((int)((X)/(CELLSIZE))-1))
#define DISCTIME2CONT(X, CELLSIZE) ((X)*(CELLSIZE))

#define FAIL 0
#define SUCCESS_WITH_TIME 1
#define SUCCESS_NO_TIME 2

typedef struct
{
        char starttheta;
        char dX;
        char dY;
        int dT;
  char endtheta;
        unsigned int cost;
  
  //all cells within footprint of polygon for all pose on trajectory
        vector<SBPL_4Dcell_t> intersectingcellsV;
        
  //start at 0,0,starttheta and end at endcell in continuous domain with half-bin less to account for 0,0 start
  //points along centerpoint trajectory (continuous)
        vector<SBPL_4Dpt_t> intermptV;
        
  //start at 0,0,starttheta and end at endcell in discrete domain
  //centerpoint cells along all poses in trajectory
        vector<SBPL_4Dcell_t> interm4DcellsV;
} envIntervalLatAction_t;


typedef struct 
{
        int stateID;
        int X;
        int Y;
        char Theta;
  int Interval;
  int T;
  int tempT;
  bool Optimal;
  bool closed;
  bool init;
        int iteration;
} envIntervalLatHashEntry_t;


typedef struct
{
        int motprimID;
        unsigned char starttheta_c;
        int additionalactioncostmult;
        SBPL_4Dcell_t endcell;
        //intermptV start at 0,0,starttheta and end at endcell in continuous domain with half-bin less to account for 0,0 start
        vector<SBPL_4Dpt_t> intermptV; 
}SBPL_xythetainterval_mprimitive;


//variables that dynamically change (e.g., array of states, ...)
typedef struct
{

        int startstateid;
        int goalstateid;

        bool bInitialized;

        //any additional variables


}EnvIntervalLat_t;

//configuration parameters
typedef struct envIntervalLat_config
{
        int EnvWidth_c;
        int EnvHeight_c;
        int StartX_c;
        int StartY_c;
        int StartTheta;
        int StartTime;
        int EndX_c;
        int EndY_c;
        int EndTheta;
        unsigned char** Grid2D;

        //the value at which and above which cells are obstacles in the maps sent from outside
        //the default is defined above
        unsigned char obsthresh; 

        //the value at which and above which until obsthresh (not including it) cells have the nearest obstacle at distance smaller than or equal to 
        //the inner circle of the robot. In other words, the robot is definitely colliding with the obstacle, independently of its orientation
        //if no such cost is known, then it should be set to obsthresh (if center of the robot collides with obstacle, then the whole robot collides with it
        //independently of its rotation)
        unsigned char cost_inscribed_thresh; 

        //the value at which and above which until cost_inscribed_thresh (not including it) cells 
        //**may** have a nearest osbtacle within the distance that is in between the robot inner circle and the robot outer circle
        //any cost below this value means that the robot will NOT collide with any obstacle, independently of its orientation
        //if no such cost is known, then it should be set to 0 or -1 (then no cell cost will lower than it, and therefore the robot's footprint will always be checked)
        int cost_possibly_circumscribed_thresh; //it has to be integer, because -1 means that it is not provided.

  //theshold for how close we get to a dynamic obstacle before it is called a collision
        unsigned char dynamic_obstacle_collision_cost_thresh;

        double nominalvel_mpersecs;
        double timetoturn45degsinplace_secs;
        double cellsize_m;
        double timeResolution;

        int dXY[ENVINTERVALLAT_DXYWIDTH][2];

        envIntervalLatAction_t** ActionsV; //array of actions, ActionsV[i][j] - jth action for sourcetheta = i
        vector<envIntervalLatAction_t*>* PredActionsV; //PredActionsV[i] - vector of pointers to the actions that result in a state with theta = i

        int actionwidth; //number of motion primitives
        vector<SBPL_xythetainterval_mprimitive> mprimV;

        vector<sbpl_2Dpt_t> FootprintPolygon;
  double robotRadius;
  double maxMovement;
} envIntervalLatConfig_t;


//-------------------------------------------------------------------------//

class SBPL2DGridSearch;

class EnvIntervalLattice : public DiscreteSpaceTimeIntervalInformation
{

public:

        EnvIntervalLattice();

        bool InitializeEnv(const char* sEnvFile, const vector<sbpl_2Dpt_t>& perimeterptsV, const char* sMotPrimFile, const char* sDynObsFile);  

        bool InitializeEnv(const char* sEnvFile, const vector<sbpl_2Dpt_t>& perimeterptsV, const char* sMotPrimFile);   

  bool InitializeEnv(int width, int height,
           const unsigned char* mapdata,
           double startx, double starty, double starttheta, double startTime,
           double goalx, double goaly, double goaltheta,
           double goaltol_x, double goaltol_y, double goaltol_theta,
           const vector<sbpl_2Dpt_t> & perimeterptsV,
           double cellsize_m, double timeResolution, double temporal_padding_c,
           double nominalvel_mpersecs, double timetoturn45degsinplace_secs, 
           unsigned char obsthresh, unsigned char dynobsthresh, const char* sMotPrimFile,
           vector<SBPL_DynamicObstacle_t> & dynObs);

  int getStartID();

  int getGoalID();

  bool UpdateCost(int x, int y, unsigned char newcost);

        bool IsObstacle(int x, int y);

        bool IsValidConfiguration(int X, int Y, int Theta);

        unsigned char GetMapCost(int x, int y);

  bool IsWithinMapCell(int X, int Y);


        virtual bool SetEnvParameter(const char* parameter, int value);

        virtual int GetEnvParameter(const char* parameter);

        void GetEnvParms(int *size_x, int *size_y, double* startx, double* starty, double* starttheta, double* startTime, double* goalx, double* goaly, double* goaltheta,
                        double* cellsize_m, double* timeResolution, double* temporal_padding_, double* nominalvel_mpersecs, double* timetoturn45degsinplace_secs, unsigned char* obsthresh, unsigned char* dyn_obs_thresh, vector<SBPL_xythetainterval_mprimitive>* motionprimitiveV);

  ~EnvIntervalLattice();

  
        virtual int  GetFromToHeuristic(int FromStateID, int ToStateID) = 0;
        virtual int  GetGoalHeuristic(int stateID) = 0;
        virtual int  GetStartHeuristic(int stateID) = 0;

        const envIntervalLatConfig_t* GetEnvNavConfig();
  void PrintTimeStat(FILE* fOut);
        void PrintEnv_Config(FILE* fOut);
  
  bool PoseContToDisc(double px, double py, double pth, double pt,
                      int &ix, int &iy, int &ith, int &it) const;
  bool PoseDiscToCont(int ix, int iy, int ith, int it,
                      double &px, double &py, double &pth, double &pt) const;
  virtual void PrintVars(){};

 protected:

  virtual int GetActionCost(int SourceX, int SourceY, int SourceTheta, envIntervalLatAction_t* action);

  void InitializeTimelineMap();
  bool UpdateTimelineMap();
  void FillInBubble(int CenterX, int CenterY, int T, int rad);
  void FillInBubbleColumn(int x, int topY, int botY, int T);
  void FillInBubbleCell(int x, int y, int T);

  void intervals2Timeline(int x, int y);
  int getInterval(int x, int y, int t);
  void getIntervals(vector<int>* intervals, vector<int>* times, envIntervalLatHashEntry_t* state, envIntervalLatAction_t* action);
  int getArrivalTimeToInterval(envIntervalLatHashEntry_t* state, envIntervalLatAction_t* action, int start_t, int end_t);
  virtual envIntervalLatHashEntry_t* getEntryFromID(int id) = 0;
  static bool pairCompare (pair<int,int> i, pair<int,int> j) { return (i.first<j.first); };

        //member data
        envIntervalLatConfig_t envIntervalLatCfg;
        EnvIntervalLat_t envIntervalLat;
        vector<SBPL_4Dcell_t> affectedsuccstatesV; //arrays of states whose outgoing actions cross cell 0,0
        vector<SBPL_4Dcell_t> affectedpredstatesV; //arrays of states whose incoming actions cross cell 0,0
        int iteration;
  vector<SBPL_DynamicObstacle_t> dynamicObstacles;
  vector< vector< vector<int> > > timelineMap;
  vector< vector< vector< pair<int,int> > > > intervalMap;
  int temporal_padding;

        //2D search for heuristic computations
        bool bNeedtoRecomputeStartHeuristics; //set whenever grid2Dsearchfromstart needs to be re-executed
        bool bNeedtoRecomputeGoalHeuristics; //set whenever grid2Dsearchfromgoal needs to be re-executed
        SBPL2DGridSearch* grid2Dsearchfromstart; //computes h-values that estimate distances from start x,y to all cells
        SBPL2DGridSearch* grid2Dsearchfromgoal;  //computes h-values that estimate distances to goal x,y from all cells

  //load dynamic obstacles
  void ReadDynamicObstacles(FILE* fDynObs);

        virtual void ReadConfiguration(FILE* fCfg);

        void InitializeEnvConfig(vector<SBPL_xythetainterval_mprimitive>* motionprimitiveV);


        bool CheckQuant(FILE* fOut);

        void SetConfiguration(int width, int height,
                              const unsigned char* mapdata,
                              int startx, int starty, int starttheta, int startTime,
                              int goalx, int goaly, int goaltheta,
                                  double cellsize_m, double timeResolution,
          double nominalvel_mpersecs, double timetoturn45degsinplace_secs, const vector<sbpl_2Dpt_t> & robot_perimeterV);
        
        bool InitGeneral( vector<SBPL_xythetainterval_mprimitive>* motionprimitiveV);
        void PrecomputeActionswithBaseMotionPrimitive(vector<SBPL_xythetainterval_mprimitive>* motionprimitiveV);
        void PrecomputeActionswithCompleteMotionPrimitive(vector<SBPL_xythetainterval_mprimitive>* motionprimitiveV);
        void PrecomputeActions();

        void CreateStartandGoalStates();

        virtual void InitializeEnvironment() = 0;

        void ComputeHeuristicValues();

        bool IsValidCell(int X, int Y);

        void CalculateFootprintForPose(SBPL_4Dpt_t pose, vector<SBPL_4Dcell_t>* footprint);
        void RemoveSourceFootprint(SBPL_4Dpt_t sourcepose, vector<SBPL_4Dcell_t>* footprint);

  virtual void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV, vector<bool>* OptV, bool optSearch);
  virtual void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV, vector<bool>* OptV, bool optSearch, vector<envIntervalLatAction_t*>* actionV=NULL) = 0;

        double EuclideanDistance_m(int X1, int Y1, int X2, int Y2);

        void ComputeReplanningData();
        void ComputeReplanningDataforAction(envIntervalLatAction_t* action);

        bool ReadMotionPrimitives(FILE* fMotPrims);
  bool ReadinMotionPrimitive(SBPL_xythetainterval_mprimitive* pMotPrim, FILE* fIn, bool &isWaitMotion);
        int ReadinCell(SBPL_4Dcell_t* cell, char* fIn);
        int ReadinPose(SBPL_4Dpt_t* pose, char* fIn);

        void PrintHeuristicValues();

  //Deprecated
        bool InitializeEnv(const char* sEnvFile);
        bool InitializeMDPCfg(MDPConfig *MDPCfg);
        virtual void SetAllActionsandAllOutcomes(CMDPSTATE* state) = 0;
        virtual void SetAllPreds(CMDPSTATE* state);
        virtual void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV) = 0;
  bool InitializeEnv(int width, int height,
             const unsigned char* mapdata,
             double startx, double starty, double starttheta, double startTime,
             double goalx, double goaly, double goaltheta,
                                           double goaltol_x, double goaltol_y, double goaltol_theta,
                                           const vector<sbpl_2Dpt_t> & perimeterptsV,
                                           double cellsize_m, double timeResolution, double temporal_padding_c,
             double nominalvel_mpersecs, double timetoturn45degsinplace_secs, 
                                           unsigned char obsthresh, unsigned char dynobsthresh, const char* sMotPrimFile);
        virtual void GetPredsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *preds_of_changededgesIDV) = 0;
        virtual void GetSuccsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *succs_of_changededgesIDV) = 0;

};


class EnvIntervalLat : public EnvIntervalLattice
{

 public:
  ~EnvIntervalLat();

  int SetStart(double x, double y, double theta, double startTime);

  int SetGoal(double x, double y, double theta);

  bool setDynamicObstacles(vector<SBPL_DynamicObstacle_t> dynObs, bool reset_states = true);
  

  void ConvertStateIDPathintoXYThetaPath(vector<int>* stateIDPath, vector<SBPL_4Dpt_t>* xythetaPath); 

  virtual void GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV, vector<bool>* OptV, bool optSearch, vector<envIntervalLatAction_t*>* actionV=NULL);

  void Relax(int stateID);

  void GetCoordFromState(int stateID, int& x, int& y, int& theta, int& t) const;
  
  void GetCoordFromState(int stateID, int& x, int& y, int& theta, int& t, bool& opt) const;

  int GetStateFromCoord(int x, int y, int theta, int t);

  void PrintState(int stateID, bool bVerbose, FILE* fOut=NULL);

  virtual int  GetFromToHeuristic(int FromStateID, int ToStateID);
  virtual int  GetGoalHeuristic(int stateID);
  virtual int  GetStartHeuristic(int stateID);

  virtual int    SizeofCreatedEnv();

  virtual void PrintVars(){};

  void getExpansions(vector<SBPL_4Dpt_t>* p);
  void dumpStatesToFile();
  void dumpEnvironmentToFile();
  void dumpDynamicObstaclesToFile();
 protected:
  void ClearStates();
  envIntervalLatHashEntry_t* getEntryFromID(int id);


  //hash table of size x_size*y_size. Maps from coords to stateId       
  int HashTableSize;
  vector<envIntervalLatHashEntry_t*>* Coord2StateIDHashTable;
  //vector that maps from stateID to coords     
  vector<envIntervalLatHashEntry_t*> StateID2CoordTable;

  unsigned int GETHASHBIN(unsigned int X, unsigned int Y, unsigned int Theta, unsigned int interval, bool Opt);

  envIntervalLatHashEntry_t* GetHashEntry(int X, int Y, int Theta, int interval, int T, bool Opt);
  envIntervalLatHashEntry_t* CreateNewHashEntry(int X, int Y, int Theta, int interval, int T, bool Opt);

  virtual void InitializeEnvironment();

  void PrintHashTableHist();

  //Deprecated
  virtual void GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV);
  void GetPredsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *preds_of_changededgesIDV);
  void GetSuccsofChangedEdges(vector<nav2dcell_t> const * changedcellsV, vector<int> *succs_of_changededgesIDV);
  virtual void SetAllActionsandAllOutcomes(CMDPSTATE* state);
};


#endif

---

sbpl_dynamic_planner
Author(s): Michael Phillips, Maxim Likhachev
autogenerated on Fri Jan 11 09:41:03 2013