#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.