#include <envInterval.h>
Public Member Functions | |
EnvIntervalLattice () | |
The constructor. One of the 3 initialize functions should be called after this. | |
const envIntervalLatConfig_t * | GetEnvNavConfig () |
virtual int | GetEnvParameter (const char *parameter) |
Get the value of parameters such as cost_inscribed_thresh and cost_possibly_circumscribed_thresh. | |
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) |
Get the value of parameters. | |
virtual int | GetFromToHeuristic (int FromStateID, int ToStateID)=0 |
virtual int | GetGoalHeuristic (int stateID)=0 |
int | getGoalID () |
Gets the state ID of the goal state. This can be given to the planner when setting the planner goal state. | |
unsigned char | GetMapCost (int x, int y) |
Returns the value of the cell. | |
virtual int | GetStartHeuristic (int stateID)=0 |
int | getStartID () |
Gets the state ID of the start state. This can be given to the planner when setting the planner start state. | |
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) |
An initialize function if you only have the motion primitive file. This is the initialize function generally used with a real simulator or robot. | |
bool | InitializeEnv (const char *sEnvFile, const vector< sbpl_2Dpt_t > &perimeterptsV, const char *sMotPrimFile) |
An initialize function if you have the costmap and motion primitive file, but would rather provide dynamic obstacle information using the setDynamicObstacles function. | |
bool | InitializeEnv (const char *sEnvFile, const vector< sbpl_2Dpt_t > &perimeterptsV, const char *sMotPrimFile, const char *sDynObsFile) |
An initialize function if you have the costmap, motion primitive, and dynamic obstacles all in the proper text file formats. | |
bool | IsObstacle (int x, int y) |
Returns if the cell is an obstacle. | |
bool | IsValidConfiguration (int X, int Y, int Theta) |
Returns if the configuration is collision free on the static costmap. | |
bool | IsWithinMapCell (int X, int Y) |
Returns if the coordinate is on the map. | |
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 |
void | PrintEnv_Config (FILE *fOut) |
void | PrintTimeStat (FILE *fOut) |
virtual void | PrintVars () |
virtual bool | SetEnvParameter (const char *parameter, int value) |
Allows the setting of a few other parameters such as cost_inscribed_thresh and cost_possibly_circumscribed_thresh. | |
bool | UpdateCost (int x, int y, unsigned char newcost) |
Changes the cost of a cell in the map. This can be used to update the costmap from sensor updates between replans. | |
~EnvIntervalLattice () | |
Destructor. | |
Protected Member Functions | |
void | CalculateFootprintForPose (SBPL_4Dpt_t pose, vector< SBPL_4Dcell_t > *footprint) |
bool | CheckQuant (FILE *fOut) |
void | ComputeHeuristicValues () |
void | ComputeReplanningData () |
void | ComputeReplanningDataforAction (envIntervalLatAction_t *action) |
void | CreateStartandGoalStates () |
double | EuclideanDistance_m (int X1, int Y1, int X2, int Y2) |
void | FillInBubble (int CenterX, int CenterY, int T, int rad) |
void | FillInBubbleCell (int x, int y, int T) |
void | FillInBubbleColumn (int x, int topY, int botY, int T) |
virtual int | GetActionCost (int SourceX, int SourceY, int SourceTheta, envIntervalLatAction_t *action) |
int | getArrivalTimeToInterval (envIntervalLatHashEntry_t *state, envIntervalLatAction_t *action, int start_t, int end_t) |
virtual envIntervalLatHashEntry_t * | getEntryFromID (int id)=0 |
int | getInterval (int x, int y, int t) |
void | getIntervals (vector< int > *intervals, vector< int > *times, envIntervalLatHashEntry_t *state, envIntervalLatAction_t *action) |
virtual void | GetPreds (int TargetStateID, vector< int > *PredIDV, vector< int > *CostV)=0 |
virtual void | GetPredsofChangedEdges (vector< nav2dcell_t > const *changedcellsV, vector< int > *preds_of_changededgesIDV)=0 |
virtual void | GetSuccs (int SourceStateID, vector< int > *SuccIDV, vector< int > *CostV, vector< bool > *OptV, bool optSearch, vector< envIntervalLatAction_t * > *actionV=NULL)=0 |
virtual void | GetSuccs (int SourceStateID, vector< int > *SuccIDV, vector< int > *CostV, vector< bool > *OptV, bool optSearch) |
virtual void | GetSuccsofChangedEdges (vector< nav2dcell_t > const *changedcellsV, vector< int > *succs_of_changededgesIDV)=0 |
bool | InitGeneral (vector< SBPL_xythetainterval_mprimitive > *motionprimitiveV) |
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) |
bool | InitializeEnv (const char *sEnvFile) |
void | InitializeEnvConfig (vector< SBPL_xythetainterval_mprimitive > *motionprimitiveV) |
virtual void | InitializeEnvironment ()=0 |
bool | InitializeMDPCfg (MDPConfig *MDPCfg) |
void | InitializeTimelineMap () |
void | intervals2Timeline (int x, int y) |
bool | IsValidCell (int X, int Y) |
void | PrecomputeActions () |
void | PrecomputeActionswithBaseMotionPrimitive (vector< SBPL_xythetainterval_mprimitive > *motionprimitiveV) |
void | PrecomputeActionswithCompleteMotionPrimitive (vector< SBPL_xythetainterval_mprimitive > *motionprimitiveV) |
void | PrintHeuristicValues () |
virtual void | ReadConfiguration (FILE *fCfg) |
void | ReadDynamicObstacles (FILE *fDynObs) |
int | ReadinCell (SBPL_4Dcell_t *cell, char *fIn) |
bool | ReadinMotionPrimitive (SBPL_xythetainterval_mprimitive *pMotPrim, FILE *fIn, bool &isWaitMotion) |
int | ReadinPose (SBPL_4Dpt_t *pose, char *fIn) |
bool | ReadMotionPrimitives (FILE *fMotPrims) |
void | RemoveSourceFootprint (SBPL_4Dpt_t sourcepose, vector< SBPL_4Dcell_t > *footprint) |
virtual void | SetAllActionsandAllOutcomes (CMDPSTATE *state)=0 |
virtual void | SetAllPreds (CMDPSTATE *state) |
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 | UpdateTimelineMap () |
Static Protected Member Functions | |
static bool | pairCompare (pair< int, int > i, pair< int, int > j) |
Protected Attributes | |
vector< SBPL_4Dcell_t > | affectedpredstatesV |
vector< SBPL_4Dcell_t > | affectedsuccstatesV |
bool | bNeedtoRecomputeGoalHeuristics |
bool | bNeedtoRecomputeStartHeuristics |
vector< SBPL_DynamicObstacle_t > | dynamicObstacles |
EnvIntervalLat_t | envIntervalLat |
envIntervalLatConfig_t | envIntervalLatCfg |
SBPL2DGridSearch * | grid2Dsearchfromgoal |
SBPL2DGridSearch * | grid2Dsearchfromstart |
vector< vector< vector< pair < int, int > > > > | intervalMap |
int | iteration |
int | temporal_padding |
vector< vector< vector< int > > > | timelineMap |
Definition at line 179 of file envInterval.h.
EnvIntervalLattice::EnvIntervalLattice | ( | ) |
The constructor. One of the 3 initialize functions should be called after this.
Definition at line 49 of file envInterval.cpp.
EnvIntervalLattice::~EnvIntervalLattice | ( | ) |
Destructor.
Definition at line 68 of file envInterval.cpp.
void EnvIntervalLattice::CalculateFootprintForPose | ( | SBPL_4Dpt_t | pose, | |
vector< SBPL_4Dcell_t > * | footprint | |||
) | [protected] |
Definition at line 2092 of file envInterval.cpp.
bool EnvIntervalLattice::CheckQuant | ( | FILE * | fOut | ) | [protected] |
Definition at line 2261 of file envInterval.cpp.
void EnvIntervalLattice::ComputeHeuristicValues | ( | ) | [protected] |
Definition at line 2247 of file envInterval.cpp.
void EnvIntervalLattice::ComputeReplanningData | ( | ) | [protected] |
Definition at line 1421 of file envInterval.cpp.
void EnvIntervalLattice::ComputeReplanningDataforAction | ( | envIntervalLatAction_t * | action | ) | [protected] |
Definition at line 1312 of file envInterval.cpp.
void EnvIntervalLattice::CreateStartandGoalStates | ( | ) | [protected] |
double EnvIntervalLattice::EuclideanDistance_m | ( | int | X1, | |
int | Y1, | |||
int | X2, | |||
int | Y2 | |||
) | [protected] |
Definition at line 2083 of file envInterval.cpp.
void EnvIntervalLattice::FillInBubble | ( | int | CenterX, | |
int | CenterY, | |||
int | T, | |||
int | rad | |||
) | [protected] |
Definition at line 442 of file envInterval.cpp.
void EnvIntervalLattice::FillInBubbleCell | ( | int | x, | |
int | y, | |||
int | T | |||
) | [protected] |
Definition at line 501 of file envInterval.cpp.
void EnvIntervalLattice::FillInBubbleColumn | ( | int | x, | |
int | topY, | |||
int | botY, | |||
int | T | |||
) | [protected] |
Definition at line 465 of file envInterval.cpp.
int EnvIntervalLattice::GetActionCost | ( | int | SourceX, | |
int | SourceY, | |||
int | SourceTheta, | |||
envIntervalLatAction_t * | action | |||
) | [protected, virtual] |
Definition at line 1999 of file envInterval.cpp.
int EnvIntervalLattice::getArrivalTimeToInterval | ( | envIntervalLatHashEntry_t * | state, | |
envIntervalLatAction_t * | action, | |||
int | start_t, | |||
int | end_t | |||
) | [protected] |
Definition at line 589 of file envInterval.cpp.
virtual envIntervalLatHashEntry_t* EnvIntervalLattice::getEntryFromID | ( | int | id | ) | [protected, pure virtual] |
Implemented in EnvIntervalLat.
const envIntervalLatConfig_t * EnvIntervalLattice::GetEnvNavConfig | ( | ) |
Definition at line 2610 of file envInterval.cpp.
int EnvIntervalLattice::GetEnvParameter | ( | const char * | parameter | ) | [virtual] |
Get the value of parameters such as cost_inscribed_thresh and cost_possibly_circumscribed_thresh.
parameter | The name of the parameter |
Definition at line 2788 of file envInterval.cpp.
void EnvIntervalLattice::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 | |||
) |
Get the value of parameters.
Definition at line 2674 of file envInterval.cpp.
virtual int EnvIntervalLattice::GetFromToHeuristic | ( | int | FromStateID, | |
int | ToStateID | |||
) | [pure virtual] |
Implemented in EnvIntervalLat.
virtual int EnvIntervalLattice::GetGoalHeuristic | ( | int | stateID | ) | [pure virtual] |
Implemented in EnvIntervalLat.
int EnvIntervalLattice::getGoalID | ( | ) |
Gets the state ID of the goal state. This can be given to the planner when setting the planner goal state.
Definition at line 2670 of file envInterval.cpp.
int EnvIntervalLattice::getInterval | ( | int | x, | |
int | y, | |||
int | t | |||
) | [protected] |
Definition at line 523 of file envInterval.cpp.
void EnvIntervalLattice::getIntervals | ( | vector< int > * | intervals, | |
vector< int > * | times, | |||
envIntervalLatHashEntry_t * | state, | |||
envIntervalLatAction_t * | action | |||
) | [protected] |
Definition at line 530 of file envInterval.cpp.
unsigned char EnvIntervalLattice::GetMapCost | ( | int | x, | |
int | y | |||
) | [virtual] |
Returns the value of the cell.
x | The x coordinate of the cell | |
y | The y coordinate of the cell |
Implements DiscreteSpaceTimeIntervalInformation.
Definition at line 2725 of file envInterval.cpp.
virtual void EnvIntervalLattice::GetPreds | ( | int | TargetStateID, | |
vector< int > * | PredIDV, | |||
vector< int > * | CostV | |||
) | [protected, pure virtual] |
Implemented in EnvIntervalLat.
virtual void EnvIntervalLattice::GetPredsofChangedEdges | ( | vector< nav2dcell_t > const * | changedcellsV, | |
vector< int > * | preds_of_changededgesIDV | |||
) | [protected, pure virtual] |
Implemented in EnvIntervalLat.
virtual int EnvIntervalLattice::GetStartHeuristic | ( | int | stateID | ) | [pure virtual] |
Implemented in EnvIntervalLat.
int EnvIntervalLattice::getStartID | ( | ) |
Gets the state ID of the start state. This can be given to the planner when setting the planner start state.
Definition at line 2666 of file envInterval.cpp.
virtual void EnvIntervalLattice::GetSuccs | ( | int | SourceStateID, | |
vector< int > * | SuccIDV, | |||
vector< int > * | CostV, | |||
vector< bool > * | OptV, | |||
bool | optSearch, | |||
vector< envIntervalLatAction_t * > * | actionV = NULL | |||
) | [protected, pure virtual] |
Implemented in EnvIntervalLat.
void EnvIntervalLattice::GetSuccs | ( | int | SourceStateID, | |
vector< int > * | SuccIDV, | |||
vector< int > * | CostV, | |||
vector< bool > * | OptV, | |||
bool | optSearch | |||
) | [protected, virtual] |
Implements DiscreteSpaceTimeIntervalInformation.
Definition at line 2603 of file envInterval.cpp.
virtual void EnvIntervalLattice::GetSuccsofChangedEdges | ( | vector< nav2dcell_t > const * | changedcellsV, | |
vector< int > * | succs_of_changededgesIDV | |||
) | [protected, pure virtual] |
Implemented in EnvIntervalLat.
bool EnvIntervalLattice::InitGeneral | ( | vector< SBPL_xythetainterval_mprimitive > * | motionprimitiveV | ) | [protected] |
Definition at line 2533 of file envInterval.cpp.
bool EnvIntervalLattice::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 | |||
) | [protected] |
Definition at line 2469 of file envInterval.cpp.
bool EnvIntervalLattice::InitializeEnv | ( | const char * | sEnvFile | ) | [protected] |
Definition at line 2353 of file envInterval.cpp.
bool EnvIntervalLattice::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 | |||
) | [virtual] |
An initialize function if you only have the motion primitive file. This is the initialize function generally used with a real simulator or robot.
width | Number of map cells along the x-axis | |
height | Number of map cells along the y-axis | |
mapdata | The initial costmap. If this is null, it is initialized to all freespace. | |
startx | The initial start x in meters (this can be updated after init) | |
starty | The initial start y in meters (this can be updated after init) | |
starttheta | The initial start theta in radians (this can be updated after init) | |
starttime | The initial start time in seconds (this can be updated after init) | |
goalx | The initial goal x in meters (this can be updated after init) | |
goaly | The initial goal y in meters (this can be updated after init) | |
goaltheta | The initial goal theta in radians (this can be updated after init) | |
goaltol_x | Goal x tolerance (not currently used) | |
goaltol_y | Goal y tolerance (not currently used) | |
goaltol_theta | Goal theta tolerance (not currently used) | |
perimeterptsV | A vector containing the (x,y) points in meters of the polygonal footprint assuming the robot was centered at (0,0). Leave empty to assume a circular robot (where the costmap is assumed to be inflated by the robot radius). | |
cellsize_m | The cell size in meters | |
timeResolution | The time resolution in seconds (used when doing collision checking against dynamic obstacles) | |
temporal_padding_c | The minimum amount of time between when an obstacle occupies a cell and when the robot can occupy that same cell (in seconds). This should be at least one timestep. | |
nominalvel_mpersecs | The linear velocity of the robot (meters per second) | |
timetoturn45degsinplace_secs | The amount of time it takes the robot to turn 45 degrees (seconds) | |
obsthresh | The threshold value for obstacles. Costmap values greater or equal to this will be considered obstacles. | |
dynobsthresh | A dynamic obstacle collision threshold (not used right now) | |
sMotPrimFile | The path to the motion primitive file | |
dynObs | A vector of the dynamic obstacles and their trajectories. (this can be updated after init) |
mapdata | if mapdata is NULL the grid is initialized to all freespace |
Implements DiscreteSpaceTimeIntervalInformation.
Definition at line 2371 of file envInterval.cpp.
bool EnvIntervalLattice::InitializeEnv | ( | const char * | sEnvFile, | |
const vector< sbpl_2Dpt_t > & | perimeterptsV, | |||
const char * | sMotPrimFile | |||
) |
An initialize function if you have the costmap and motion primitive file, but would rather provide dynamic obstacle information using the setDynamicObstacles function.
sEnvFile | The path to the costmap file | |
perimeterptsV | A vector containing the (x,y) points of the polygonal footprint assuming the robot was centered at (0,0). Leave empty to assume a circular robot (where the costmap is assumed to be inflated by the robot radius). | |
sMotPrimFile | The path to the motion primitive file |
Definition at line 2302 of file envInterval.cpp.
bool EnvIntervalLattice::InitializeEnv | ( | const char * | sEnvFile, | |
const vector< sbpl_2Dpt_t > & | perimeterptsV, | |||
const char * | sMotPrimFile, | |||
const char * | sDynObsFile | |||
) |
An initialize function if you have the costmap, motion primitive, and dynamic obstacles all in the proper text file formats.
sEnvFile | The path to the costmap file | |
perimeterptsV | A vector containing the (x,y) points of the polygonal footprint assuming the robot was centered at (0,0). Leave empty to assume a circular robot (where the costmap is assumed to be inflated by the robot radius). | |
sMotPrimFile | The path to the motion primitive file | |
sDynObsFile | The path to the dynamic obstacle file |
Definition at line 2287 of file envInterval.cpp.
void EnvIntervalLattice::InitializeEnvConfig | ( | vector< SBPL_xythetainterval_mprimitive > * | motionprimitiveV | ) | [protected] |
Definition at line 1902 of file envInterval.cpp.
virtual void EnvIntervalLattice::InitializeEnvironment | ( | ) | [protected, pure virtual] |
Implemented in EnvIntervalLat.
bool EnvIntervalLattice::InitializeMDPCfg | ( | MDPConfig * | MDPCfg | ) | [protected] |
Definition at line 2548 of file envInterval.cpp.
void EnvIntervalLattice::InitializeTimelineMap | ( | ) | [protected] |
Definition at line 358 of file envInterval.cpp.
void EnvIntervalLattice::intervals2Timeline | ( | int | x, | |
int | y | |||
) | [protected] |
Definition at line 507 of file envInterval.cpp.
bool EnvIntervalLattice::IsObstacle | ( | int | x, | |
int | y | |||
) |
Returns if the cell is an obstacle.
x | The x coordinate of the cell | |
y | The y coordinate of the cell |
Definition at line 2654 of file envInterval.cpp.
bool EnvIntervalLattice::IsValidCell | ( | int | X, | |
int | Y | |||
) | [protected] |
Definition at line 1955 of file envInterval.cpp.
bool EnvIntervalLattice::IsValidConfiguration | ( | int | X, | |
int | Y, | |||
int | Theta | |||
) |
Returns if the configuration is collision free on the static costmap.
X | The x coordinate of the center of the robot | |
Y | The y coordinate of the center of the robot | |
Theta | The orientation of the robot |
Definition at line 1968 of file envInterval.cpp.
bool EnvIntervalLattice::IsWithinMapCell | ( | int | X, | |
int | Y | |||
) |
Returns if the coordinate is on the map.
X | The x coordinate | |
Y | The y coordinate |
Definition at line 1962 of file envInterval.cpp.
static bool EnvIntervalLattice::pairCompare | ( | pair< int, int > | i, | |
pair< int, int > | j | |||
) | [inline, static, protected] |
Definition at line 358 of file envInterval.h.
bool EnvIntervalLattice::PoseContToDisc | ( | double | px, | |
double | py, | |||
double | pth, | |||
double | pt, | |||
int & | ix, | |||
int & | iy, | |||
int & | ith, | |||
int & | it | |||
) | const |
Definition at line 2701 of file envInterval.cpp.
bool EnvIntervalLattice::PoseDiscToCont | ( | int | ix, | |
int | iy, | |||
int | ith, | |||
int | it, | |||
double & | px, | |||
double & | py, | |||
double & | pth, | |||
double & | pt | |||
) | const |
Definition at line 2713 of file envInterval.cpp.
void EnvIntervalLattice::PrecomputeActions | ( | ) | [protected] |
Definition at line 1727 of file envInterval.cpp.
void EnvIntervalLattice::PrecomputeActionswithBaseMotionPrimitive | ( | vector< SBPL_xythetainterval_mprimitive > * | motionprimitiveV | ) | [protected] |
Definition at line 1438 of file envInterval.cpp.
void EnvIntervalLattice::PrecomputeActionswithCompleteMotionPrimitive | ( | vector< SBPL_xythetainterval_mprimitive > * | motionprimitiveV | ) | [protected] |
Definition at line 1574 of file envInterval.cpp.
void EnvIntervalLattice::PrintEnv_Config | ( | FILE * | fOut | ) |
Definition at line 2632 of file envInterval.cpp.
void EnvIntervalLattice::PrintHeuristicValues | ( | ) | [protected] |
Definition at line 2558 of file envInterval.cpp.
void EnvIntervalLattice::PrintTimeStat | ( | FILE * | fOut | ) |
Definition at line 2642 of file envInterval.cpp.
virtual void EnvIntervalLattice::PrintVars | ( | ) | [inline, virtual] |
Reimplemented in EnvIntervalLat.
Definition at line 341 of file envInterval.h.
void EnvIntervalLattice::ReadConfiguration | ( | FILE * | fCfg | ) | [protected, virtual] |
Definition at line 688 of file envInterval.cpp.
void EnvIntervalLattice::ReadDynamicObstacles | ( | FILE * | fDynObs | ) | [protected] |
Definition at line 117 of file envInterval.cpp.
int EnvIntervalLattice::ReadinCell | ( | SBPL_4Dcell_t * | cell, | |
char * | fIn | |||
) | [protected] |
Definition at line 1010 of file envInterval.cpp.
bool EnvIntervalLattice::ReadinMotionPrimitive | ( | SBPL_xythetainterval_mprimitive * | pMotPrim, | |
FILE * | fIn, | |||
bool & | isWaitMotion | |||
) | [protected] |
Definition at line 1065 of file envInterval.cpp.
int EnvIntervalLattice::ReadinPose | ( | SBPL_4Dpt_t * | pose, | |
char * | fIn | |||
) | [protected] |
Definition at line 1038 of file envInterval.cpp.
bool EnvIntervalLattice::ReadMotionPrimitives | ( | FILE * | fMotPrims | ) | [protected] |
Definition at line 1216 of file envInterval.cpp.
void EnvIntervalLattice::RemoveSourceFootprint | ( | SBPL_4Dpt_t | sourcepose, | |
vector< SBPL_4Dcell_t > * | footprint | |||
) | [protected] |
Definition at line 2218 of file envInterval.cpp.
virtual void EnvIntervalLattice::SetAllActionsandAllOutcomes | ( | CMDPSTATE * | state | ) | [protected, pure virtual] |
Implemented in EnvIntervalLat.
void EnvIntervalLattice::SetAllPreds | ( | CMDPSTATE * | state | ) | [protected, virtual] |
Definition at line 2594 of file envInterval.cpp.
void EnvIntervalLattice::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 | |||
) | [protected] |
mapdata | if mapdata is NULL the grid is initialized to all freespace |
Definition at line 614 of file envInterval.cpp.
bool EnvIntervalLattice::SetEnvParameter | ( | const char * | parameter, | |
int | value | |||
) | [virtual] |
Allows the setting of a few other parameters such as cost_inscribed_thresh and cost_possibly_circumscribed_thresh.
parameter | The name of the parameter | |
value | The value to set it to |
Definition at line 2732 of file envInterval.cpp.
bool EnvIntervalLattice::UpdateCost | ( | int | x, | |
int | y, | |||
unsigned char | newcost | |||
) | [virtual] |
Changes the cost of a cell in the map. This can be used to update the costmap from sensor updates between replans.
x | The x coordinate of the cell to update | |
y | The y coordinate of the cell to update | |
newcost | The new value for the cell |
Implements DiscreteSpaceTimeIntervalInformation.
Definition at line 2616 of file envInterval.cpp.
bool EnvIntervalLattice::UpdateTimelineMap | ( | ) | [protected] |
Definition at line 371 of file envInterval.cpp.
vector<SBPL_4Dcell_t> EnvIntervalLattice::affectedpredstatesV [protected] |
Definition at line 364 of file envInterval.h.
vector<SBPL_4Dcell_t> EnvIntervalLattice::affectedsuccstatesV [protected] |
Definition at line 363 of file envInterval.h.
bool EnvIntervalLattice::bNeedtoRecomputeGoalHeuristics [protected] |
Definition at line 373 of file envInterval.h.
bool EnvIntervalLattice::bNeedtoRecomputeStartHeuristics [protected] |
Definition at line 372 of file envInterval.h.
vector<SBPL_DynamicObstacle_t> EnvIntervalLattice::dynamicObstacles [protected] |
Definition at line 366 of file envInterval.h.
EnvIntervalLat_t EnvIntervalLattice::envIntervalLat [protected] |
Definition at line 362 of file envInterval.h.
Definition at line 358 of file envInterval.h.
SBPL2DGridSearch* EnvIntervalLattice::grid2Dsearchfromgoal [protected] |
Definition at line 375 of file envInterval.h.
SBPL2DGridSearch* EnvIntervalLattice::grid2Dsearchfromstart [protected] |
Definition at line 374 of file envInterval.h.
vector< vector< vector< pair<int,int> > > > EnvIntervalLattice::intervalMap [protected] |
Definition at line 368 of file envInterval.h.
int EnvIntervalLattice::iteration [protected] |
Definition at line 365 of file envInterval.h.
int EnvIntervalLattice::temporal_padding [protected] |
Definition at line 369 of file envInterval.h.
vector< vector< vector<int> > > EnvIntervalLattice::timelineMap [protected] |
Definition at line 367 of file envInterval.h.