Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes
EnvironmentNAVXYTHETACARTLATTICE Class Reference

3D (x,y,theta) planning using lattice-based graph problem. For general structure see comments on parent class DiscreteSpaceInformation For info on lattice-based planning used here, you can check out the paper: Maxim Likhachev and Dave Ferguson, " Planning Long Dynamically-Feasible Maneuvers for Autonomous Vehicles", IJRR'09 More...

#include <environment_cart_planner.h>

Inheritance diagram for EnvironmentNAVXYTHETACARTLATTICE:
Inheritance graph
[legend]

List of all members.

Public Member Functions

virtual void EnsureHeuristicsUpdated (bool bGoalHeuristics)
 see comments on the same function in the parent class
 EnvironmentNAVXYTHETACARTLATTICE ()
const
EnvNAVXYTHETACARTLATConfig_t
GetEnvNavConfig ()
 get internal configuration data structure
virtual int GetEnvParameter (const char *parameter)
 returns the value of specific parameter - see function body for the list of parameters
void GetEnvParms (int *size_x, int *size_y, double *startx, double *starty, double *starttheta, double *startcartangle, double *goalx, double *goaly, double *goaltheta, double *goalcartangle, double *cellsize_m, double *nominalvel_mpersecs, double *timetoturn45degsinplace_secs, unsigned char *obsthresh, vector< SBPL_xythetacart_mprimitive > *mprimitiveV)
 returns environment parameters. Useful for creating a copy environment
virtual int GetFromToHeuristic (int FromStateID, int ToStateID)=0
 see comments on the same function in the parent class
virtual int GetGoalHeuristic (int stateID)=0
 see comments on the same function in the parent class
unsigned char GetMapCost (int x, int y)
 returns the cost corresponding to the cell <x,y>
virtual void GetPreds (int TargetStateID, vector< int > *PredIDV, vector< int > *CostV)=0
 see comments on the same function in the parent class
virtual void GetPredsofChangedEdges (vector< nav2dcell_t > const *changedcellsV, vector< int > *preds_of_changededgesIDV)=0
 this function fill in Predecessor/Successor states of edges whose costs changed It takes in an array of cells whose traversability changed, and returns (in vector preds_of_changededgesIDV) the IDs of all states that have outgoing edges that go through the changed cells
virtual int GetStartHeuristic (int stateID)=0
 see comments on the same function in the parent class
virtual void GetSuccs (int SourceStateID, vector< int > *SuccIDV, vector< int > *CostV)
 see comments on the same function in the parent class
virtual void GetSuccsofChangedEdges (vector< nav2dcell_t > const *changedcellsV, vector< int > *succs_of_changededgesIDV)=0
 same as GetPredsofChangedEdges, but returns successor states. Both functions need to be present for incremental search
bool InitializeEnv (const char *sEnvFile, const vector< sbpl_2Dpt_t > &perimeterptsV, const vector< sbpl_2Dpt_t > &cartperimeterptsV, sbpl_2Dpt_t &cart_offset, const char *sMotPrimFile)
 initialization of environment from file. See .cfg files for examples it also takes the perimeter of the robot with respect to some reference point centered at x=0,y=0 and orientation = 0 (along x axis). The perimeter is defined in meters as a sequence of vertices of a polygon defining the perimeter. If vector is of zero size, then robot is assumed to be point robot (you may want to inflate all obstacles by its actual radius) Motion primitives file defines the motion primitives available to the robot
bool InitializeEnv (const char *sEnvFile)
 see comments on the same function in the parent class
bool InitializeEnv (int width, int height, const unsigned char *mapdata, double startx, double starty, double starttheta, double startcartangle, double goalx, double goaly, double goaltheta, double goalcartangle, double goaltol_x, double goaltol_y, double goaltol_theta, double goaltol_cartangle, const vector< sbpl_2Dpt_t > &perimeterptsV, const sbpl_2Dpt_t &cart_offset, const vector< sbpl_2Dpt_t > &cartptsV, double cellsize_m, double nominalvel_mpersecs, double timetoturn45degsinplace_secs, unsigned char obsthresh, const char *sMotPrimFile)
 initialize environment. Gridworld is defined as matrix A of size width by height. So, internally, it is accessed as A[x][y] with x ranging from 0 to width-1 and and y from 0 to height-1 Each element in A[x][y] is unsigned char. A[x][y] = 0 corresponds to fully traversable and cost is just Euclidean distance The cost of transition between two neighboring cells is EuclideanDistance*(max(A[sourcex][sourcey],A[targetx][targety])+1) f A[x][y] >= obsthresh, then in the above equation it is assumed to be infinite. The cost also incorporates the length of a motion primitive and its cost_multiplier (see getcost function) mapdata is a pointer to the values of A. If it is null, then A is initialized to all zeros. Mapping is: A[x][y] = mapdata[x+y*width] start/goal are given by startx, starty, starttheta, goalx,goaly, goaltheta in meters/radians. If they are not known yet, just set them to 0. Later setgoal/setstart can be executed finally obsthresh defined obstacle threshold, as mentioned above goaltolerances are currently ignored for explanation of perimeter, see comments for InitializeEnv function that reads all from file cellsize is discretization in meters nominalvel_mpersecs is assumed velocity of vehicle while moving forward in m/sec timetoturn45degsinplace_secs is rotational velocity in secs/45 degrees turn
bool InitializeMDPCfg (MDPConfig *MDPCfg)
 see comments on the same function in the parent class
bool IsObstacle (int x, int y)
bool IsValidConfiguration (int X, int Y, int Theta, int CartAngle)
 returns false if robot intersects obstacles or lies outside of the map. Note this is pretty expensive operation since it computes the footprint of the robot based on its x,y,theta
bool IsWithinMapCell (int X, int Y)
 returns true if cell is within map
void PrintEnv_Config (FILE *fOut)
 see comments on the same function in the parent class
void PrintTimeStat (FILE *fOut)
 prints time statistics
virtual void PrintVars ()
 prints environment variables for debugging
virtual void SetAllActionsandAllOutcomes (CMDPSTATE *state)=0
 see comments on the same function in the parent class
virtual void SetAllPreds (CMDPSTATE *state)
 see comments on the same function in the parent class
virtual bool SetEnvParameter (const char *parameter, int value)
 way to set up various parameters. For a list of parameters, see the body of the function - it is pretty straightforward
bool UpdateCost (int x, int y, unsigned char newcost)
 update the traversability of a cell<x,y>
virtual ~EnvironmentNAVXYTHETACARTLATTICE ()

Public Attributes

bool initialized_

Protected Member Functions

void CalculateFootprintForPose (EnvNAVXYTHETACARTLAT3Dpt_t pose, vector< sbpl_2Dcell_t > *footprint)
bool CheckQuant (FILE *fOut)
void ComputeHeuristicValues ()
void ComputeReplanningData ()
void ComputeReplanningDataforAction (EnvNAVXYTHETACARTLATAction_t *action)
void CreateStartandGoalStates ()
double EuclideanDistance_m (int X1, int Y1, int X2, int Y2)
virtual int GetActionCost (int SourceX, int SourceY, int SourceTheta, int SourceCartAngle, EnvNAVXYTHETACARTLATAction_t *action)
EnvNAVXYTHETACARTLAT3Dpt_t getCartCenter (EnvNAVXYTHETACARTLAT3Dpt_t pose, sbpl_2Dpt_t cart_center_offset)
virtual void GetSuccs (int SourceStateID, vector< int > *SuccIDV, vector< int > *CostV, vector< EnvNAVXYTHETACARTLATAction_t * > *actionindV=NULL)=0
bool InitGeneral (vector< SBPL_xythetacart_mprimitive > *motionprimitiveV)
void InitializeEnvConfig (vector< SBPL_xythetacart_mprimitive > *motionprimitiveV)
virtual void InitializeEnvironment ()=0
bool IsValidCell (int X, int Y)
void PrecomputeActions ()
void PrecomputeActionswithBaseMotionPrimitive (vector< SBPL_xythetacart_mprimitive > *motionprimitiveV)
void PrecomputeActionswithCompleteMotionPrimitive (vector< SBPL_xythetacart_mprimitive > *motionprimitiveV)
void PrintFootprint ()
void PrintHeuristicValues ()
virtual void ReadConfiguration (FILE *fCfg)
bool ReadinCell (EnvNAVXYTHETACARTLAT3Dcell_t *cell, FILE *fIn)
bool ReadinMotionPrimitive (SBPL_xythetacart_mprimitive *pMotPrim, FILE *fIn)
bool ReadinPose (EnvNAVXYTHETACARTLAT3Dpt_t *pose, FILE *fIn)
bool ReadMotionPrimitives (FILE *fMotPrims)
void RemoveSourceFootprint (EnvNAVXYTHETACARTLAT3Dpt_t sourcepose, vector< sbpl_2Dcell_t > *footprint)
void SetConfiguration (int width, int height, const unsigned char *mapdata, int startx, int starty, int starttheta, int startcartangle, int goalx, int goaly, int goaltheta, int goalcartangle, double cellsize_m, double nominalvel_mpersecs, double timetoturn45degsinplace_secs, const vector< sbpl_2Dpt_t > &robot_perimeterV, const vector< sbpl_2Dpt_t > &cart_perimeterV, const sbpl_2Dpt_t &cart_offset)

Protected Attributes

vector
< EnvNAVXYTHETACARTLAT3Dcell_t
affectedpredstatesV
vector
< EnvNAVXYTHETACARTLAT3Dcell_t
affectedsuccstatesV
bool bNeedtoRecomputeGoalHeuristics
bool bNeedtoRecomputeStartHeuristics
EnvironmentNAVXYTHETACARTLAT_t EnvNAVXYTHETACARTLAT
EnvNAVXYTHETACARTLATConfig_t EnvNAVXYTHETACARTLATCfg
SBPL2DGridSearchgrid2Dsearchfromgoal
SBPL2DGridSearchgrid2Dsearchfromstart
int iteration

Detailed Description

3D (x,y,theta) planning using lattice-based graph problem. For general structure see comments on parent class DiscreteSpaceInformation For info on lattice-based planning used here, you can check out the paper: Maxim Likhachev and Dave Ferguson, " Planning Long Dynamically-Feasible Maneuvers for Autonomous Vehicles", IJRR'09

Definition at line 189 of file environment_cart_planner.h.


Constructor & Destructor Documentation

Definition at line 71 of file environment_cart_planner.cpp.

Definition at line 93 of file environment_cart_planner.cpp.


Member Function Documentation

Definition at line 1627 of file environment_cart_planner.cpp.

bool EnvironmentNAVXYTHETACARTLATTICE::CheckQuant ( FILE *  fOut) [protected]

Definition at line 1952 of file environment_cart_planner.cpp.

Definition at line 1934 of file environment_cart_planner.cpp.

Definition at line 955 of file environment_cart_planner.cpp.

Definition at line 847 of file environment_cart_planner.cpp.

see comments on the same function in the parent class

Reimplemented from DiscreteSpaceInformation.

Definition at line 1903 of file environment_cart_planner.cpp.

double EnvironmentNAVXYTHETACARTLATTICE::EuclideanDistance_m ( int  X1,
int  Y1,
int  X2,
int  Y2 
) [protected]

Definition at line 1618 of file environment_cart_planner.cpp.

int EnvironmentNAVXYTHETACARTLATTICE::GetActionCost ( int  SourceX,
int  SourceY,
int  SourceTheta,
int  SourceCartAngle,
EnvNAVXYTHETACARTLATAction_t action 
) [protected, virtual]

Definition at line 1539 of file environment_cart_planner.cpp.

Definition at line 1861 of file environment_cart_planner.cpp.

get internal configuration data structure

Definition at line 2253 of file environment_cart_planner.cpp.

int EnvironmentNAVXYTHETACARTLATTICE::GetEnvParameter ( const char *  parameter) [virtual]

returns the value of specific parameter - see function body for the list of parameters

Definition at line 2390 of file environment_cart_planner.cpp.

void EnvironmentNAVXYTHETACARTLATTICE::GetEnvParms ( int *  size_x,
int *  size_y,
double *  startx,
double *  starty,
double *  starttheta,
double *  startcartangle,
double *  goalx,
double *  goaly,
double *  goaltheta,
double *  goalcartangle,
double *  cellsize_m,
double *  nominalvel_mpersecs,
double *  timetoturn45degsinplace_secs,
unsigned char *  obsthresh,
vector< SBPL_xythetacart_mprimitive > *  mprimitiveV 
)

returns environment parameters. Useful for creating a copy environment

Definition at line 2309 of file environment_cart_planner.cpp.

virtual int EnvironmentNAVXYTHETACARTLATTICE::GetFromToHeuristic ( int  FromStateID,
int  ToStateID 
) [pure virtual]

see comments on the same function in the parent class

Implements DiscreteSpaceInformation.

Implemented in EnvironmentNAVXYTHETACARTLAT.

virtual int EnvironmentNAVXYTHETACARTLATTICE::GetGoalHeuristic ( int  stateID) [pure virtual]

see comments on the same function in the parent class

Implements DiscreteSpaceInformation.

Implemented in EnvironmentNAVXYTHETACARTLAT.

unsigned char EnvironmentNAVXYTHETACARTLATTICE::GetMapCost ( int  x,
int  y 
)

returns the cost corresponding to the cell <x,y>

Definition at line 2338 of file environment_cart_planner.cpp.

virtual void EnvironmentNAVXYTHETACARTLATTICE::GetPreds ( int  TargetStateID,
vector< int > *  PredIDV,
vector< int > *  CostV 
) [pure virtual]

see comments on the same function in the parent class

Implemented in EnvironmentNAVXYTHETACARTLAT.

virtual void EnvironmentNAVXYTHETACARTLATTICE::GetPredsofChangedEdges ( vector< nav2dcell_t > const *  changedcellsV,
vector< int > *  preds_of_changededgesIDV 
) [pure virtual]

this function fill in Predecessor/Successor states of edges whose costs changed It takes in an array of cells whose traversability changed, and returns (in vector preds_of_changededgesIDV) the IDs of all states that have outgoing edges that go through the changed cells

Implemented in EnvironmentNAVXYTHETACARTLAT.

virtual int EnvironmentNAVXYTHETACARTLATTICE::GetStartHeuristic ( int  stateID) [pure virtual]

see comments on the same function in the parent class

Implements DiscreteSpaceInformation.

Implemented in EnvironmentNAVXYTHETACARTLAT.

void EnvironmentNAVXYTHETACARTLATTICE::GetSuccs ( int  SourceStateID,
vector< int > *  SuccIDV,
vector< int > *  CostV 
) [virtual]

see comments on the same function in the parent class

Definition at line 2244 of file environment_cart_planner.cpp.

virtual void EnvironmentNAVXYTHETACARTLATTICE::GetSuccs ( int  SourceStateID,
vector< int > *  SuccIDV,
vector< int > *  CostV,
vector< EnvNAVXYTHETACARTLATAction_t * > *  actionindV = NULL 
) [protected, pure virtual]
virtual void EnvironmentNAVXYTHETACARTLATTICE::GetSuccsofChangedEdges ( vector< nav2dcell_t > const *  changedcellsV,
vector< int > *  succs_of_changededgesIDV 
) [pure virtual]

same as GetPredsofChangedEdges, but returns successor states. Both functions need to be present for incremental search

Implemented in EnvironmentNAVXYTHETACARTLAT.

Definition at line 2174 of file environment_cart_planner.cpp.

bool EnvironmentNAVXYTHETACARTLATTICE::InitializeEnv ( const char *  sEnvFile,
const vector< sbpl_2Dpt_t > &  perimeterptsV,
const vector< sbpl_2Dpt_t > &  cartperimeterptsV,
sbpl_2Dpt_t cart_offset,
const char *  sMotPrimFile 
)

initialization of environment from file. See .cfg files for examples it also takes the perimeter of the robot with respect to some reference point centered at x=0,y=0 and orientation = 0 (along x axis). The perimeter is defined in meters as a sequence of vertices of a polygon defining the perimeter. If vector is of zero size, then robot is assumed to be point robot (you may want to inflate all obstacles by its actual radius) Motion primitives file defines the motion primitives available to the robot

Definition at line 1978 of file environment_cart_planner.cpp.

bool EnvironmentNAVXYTHETACARTLATTICE::InitializeEnv ( const char *  sEnvFile) [virtual]

see comments on the same function in the parent class

Implements DiscreteSpaceInformation.

Definition at line 2019 of file environment_cart_planner.cpp.

bool EnvironmentNAVXYTHETACARTLATTICE::InitializeEnv ( int  width,
int  height,
const unsigned char *  mapdata,
double  startx,
double  starty,
double  starttheta,
double  startcartangle,
double  goalx,
double  goaly,
double  goaltheta,
double  goalcartangle,
double  goaltol_x,
double  goaltol_y,
double  goaltol_theta,
double  goaltol_cartangle,
const vector< sbpl_2Dpt_t > &  perimeterptsV,
const sbpl_2Dpt_t cart_offset,
const vector< sbpl_2Dpt_t > &  cartptsV,
double  cellsize_m,
double  nominalvel_mpersecs,
double  timetoturn45degsinplace_secs,
unsigned char  obsthresh,
const char *  sMotPrimFile 
)

initialize environment. Gridworld is defined as matrix A of size width by height. So, internally, it is accessed as A[x][y] with x ranging from 0 to width-1 and and y from 0 to height-1 Each element in A[x][y] is unsigned char. A[x][y] = 0 corresponds to fully traversable and cost is just Euclidean distance The cost of transition between two neighboring cells is EuclideanDistance*(max(A[sourcex][sourcey],A[targetx][targety])+1) f A[x][y] >= obsthresh, then in the above equation it is assumed to be infinite. The cost also incorporates the length of a motion primitive and its cost_multiplier (see getcost function) mapdata is a pointer to the values of A. If it is null, then A is initialized to all zeros. Mapping is: A[x][y] = mapdata[x+y*width] start/goal are given by startx, starty, starttheta, goalx,goaly, goaltheta in meters/radians. If they are not known yet, just set them to 0. Later setgoal/setstart can be executed finally obsthresh defined obstacle threshold, as mentioned above goaltolerances are currently ignored for explanation of perimeter, see comments for InitializeEnv function that reads all from file cellsize is discretization in meters nominalvel_mpersecs is assumed velocity of vehicle while moving forward in m/sec timetoturn45degsinplace_secs is rotational velocity in secs/45 degrees turn

Parameters:
mapdataif mapdata is NULL the grid is initialized to all freespace

Definition at line 2039 of file environment_cart_planner.cpp.

Definition at line 1442 of file environment_cart_planner.cpp.

virtual void EnvironmentNAVXYTHETACARTLATTICE::InitializeEnvironment ( ) [protected, pure virtual]

see comments on the same function in the parent class

Implements DiscreteSpaceInformation.

Definition at line 2189 of file environment_cart_planner.cpp.

returns true if cell is untraversable

Definition at line 2296 of file environment_cart_planner.cpp.

bool EnvironmentNAVXYTHETACARTLATTICE::IsValidCell ( int  X,
int  Y 
) [protected]

Definition at line 1494 of file environment_cart_planner.cpp.

bool EnvironmentNAVXYTHETACARTLATTICE::IsValidConfiguration ( int  X,
int  Y,
int  Theta,
int  CartAngle 
)

returns false if robot intersects obstacles or lies outside of the map. Note this is pretty expensive operation since it computes the footprint of the robot based on its x,y,theta

Definition at line 1507 of file environment_cart_planner.cpp.

returns true if cell is within map

Definition at line 1501 of file environment_cart_planner.cpp.

Definition at line 1312 of file environment_cart_planner.cpp.

Definition at line 973 of file environment_cart_planner.cpp.

Definition at line 1101 of file environment_cart_planner.cpp.

void EnvironmentNAVXYTHETACARTLATTICE::PrintEnv_Config ( FILE *  fOut) [virtual]

see comments on the same function in the parent class

Implements DiscreteSpaceInformation.

Definition at line 2275 of file environment_cart_planner.cpp.

Definition at line 2112 of file environment_cart_planner.cpp.

Definition at line 2199 of file environment_cart_planner.cpp.

prints time statistics

Definition at line 2284 of file environment_cart_planner.cpp.

virtual void EnvironmentNAVXYTHETACARTLATTICE::PrintVars ( ) [inline, virtual]

prints environment variables for debugging

Reimplemented in EnvironmentNAVXYTHETACARTLAT.

Definition at line 348 of file environment_cart_planner.h.

void EnvironmentNAVXYTHETACARTLATTICE::ReadConfiguration ( FILE *  fCfg) [protected, virtual]

Definition at line 265 of file environment_cart_planner.cpp.

Definition at line 620 of file environment_cart_planner.cpp.

Definition at line 668 of file environment_cart_planner.cpp.

Definition at line 645 of file environment_cart_planner.cpp.

Definition at line 778 of file environment_cart_planner.cpp.

Definition at line 1876 of file environment_cart_planner.cpp.

see comments on the same function in the parent class

Implements DiscreteSpaceInformation.

Implemented in EnvironmentNAVXYTHETACARTLAT.

see comments on the same function in the parent class

Implements DiscreteSpaceInformation.

Definition at line 2235 of file environment_cart_planner.cpp.

void EnvironmentNAVXYTHETACARTLATTICE::SetConfiguration ( int  width,
int  height,
const unsigned char *  mapdata,
int  startx,
int  starty,
int  starttheta,
int  startcartangle,
int  goalx,
int  goaly,
int  goaltheta,
int  goalcartangle,
double  cellsize_m,
double  nominalvel_mpersecs,
double  timetoturn45degsinplace_secs,
const vector< sbpl_2Dpt_t > &  robot_perimeterV,
const vector< sbpl_2Dpt_t > &  cart_perimeterV,
const sbpl_2Dpt_t cart_offset 
) [protected]
Parameters:
mapdataif mapdata is NULL the grid is initialized to all freespace

Definition at line 148 of file environment_cart_planner.cpp.

bool EnvironmentNAVXYTHETACARTLATTICE::SetEnvParameter ( const char *  parameter,
int  value 
) [virtual]

way to set up various parameters. For a list of parameters, see the body of the function - it is pretty straightforward

Reimplemented from DiscreteSpaceInformation.

Definition at line 2343 of file environment_cart_planner.cpp.

bool EnvironmentNAVXYTHETACARTLATTICE::UpdateCost ( int  x,
int  y,
unsigned char  newcost 
)

update the traversability of a cell<x,y>

Definition at line 2259 of file environment_cart_planner.cpp.


Member Data Documentation

Definition at line 361 of file environment_cart_planner.h.

Definition at line 360 of file environment_cart_planner.h.

Definition at line 366 of file environment_cart_planner.h.

Definition at line 365 of file environment_cart_planner.h.

Definition at line 359 of file environment_cart_planner.h.

Definition at line 358 of file environment_cart_planner.h.

Definition at line 368 of file environment_cart_planner.h.

Definition at line 367 of file environment_cart_planner.h.

Definition at line 348 of file environment_cart_planner.h.

Definition at line 362 of file environment_cart_planner.h.


The documentation for this class was generated from the following files:


sbpl_cart_planner
Author(s): Sachin Chitta
autogenerated on Tue Jan 7 2014 11:12:32