#include <environment_navxythetalat.h>
Public Member Functions | |
void | ConvertStateIDPathintoXYThetaPath (vector< int > *stateIDPath, vector< EnvNAVXYTHETALAT3Dpt_t > *xythetaPath) |
converts a path given by stateIDs into a sequence of coordinates. Note that since motion primitives are short actions represented as a sequence of points, the path returned by this function contains much more points than the number of points in the input path. The returned coordinates are in meters,meters,radians | |
EnvironmentNAVXYTHETALAT () | |
void | GetCoordFromState (int stateID, int &x, int &y, int &theta) const |
returns state coordinates of state with ID=stateID | |
virtual int | GetFromToHeuristic (int FromStateID, int ToStateID) |
see comments on the same function in the parent class | |
virtual int | GetGoalHeuristic (int stateID) |
see comments on the same function in the parent class | |
virtual void | GetPreds (int TargetStateID, vector< int > *PredIDV, vector< int > *CostV) |
returns all predecessors states and corresponding costs of actions | |
void | GetPredsofChangedEdges (vector< nav2dcell_t > const *changedcellsV, vector< int > *preds_of_changededgesIDV) |
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) |
see comments on the same function in the parent class | |
int | GetStateFromCoord (int x, int y, int theta) |
returns stateID for a state with coords x,y,theta | |
virtual void | GetSuccs (int SourceStateID, vector< int > *SuccIDV, vector< int > *CostV, vector< EnvNAVXYTHETALATAction_t * > *actionindV=NULL) |
returns all successors states, costs of corresponding actions and pointers to corresponding actions, each of which is a motion primitive if actionindV is NULL, then pointers to actions are not returned | |
void | GetSuccsofChangedEdges (vector< nav2dcell_t > const *changedcellsV, vector< int > *succs_of_changededgesIDV) |
same as GetPredsofChangedEdges, but returns successor states. Both functions need to be present for incremental search | |
void | PrintState (int stateID, bool bVerbose, FILE *fOut=NULL) |
prints state info (coordinates) into file | |
virtual void | PrintVars () |
see comments on the same function in the parent class | |
virtual void | SetAllActionsandAllOutcomes (CMDPSTATE *state) |
see comments on the same function in the parent class | |
int | SetGoal (double x, double y, double theta) |
sets goal in meters/radians | |
void | SetGoalTolerance (double tol_x, double tol_y, double tol_theta) |
sets goal tolerance. (Note goal tolerance is ignored currently) | |
int | SetStart (double x, double y, double theta) |
sets start in meters/radians | |
virtual int | SizeofCreatedEnv () |
see comments on the same function in the parent class | |
~EnvironmentNAVXYTHETALAT () | |
Protected Member Functions | |
EnvNAVXYTHETALATHashEntry_t * | CreateNewHashEntry_hash (int X, int Y, int Theta) |
EnvNAVXYTHETALATHashEntry_t * | CreateNewHashEntry_lookup (int X, int Y, int Theta) |
unsigned int | GETHASHBIN (unsigned int X, unsigned int Y, unsigned int Theta) |
EnvNAVXYTHETALATHashEntry_t * | GetHashEntry_hash (int X, int Y, int Theta) |
EnvNAVXYTHETALATHashEntry_t * | GetHashEntry_lookup (int X, int Y, int Theta) |
virtual void | InitializeEnvironment () |
void | PrintHashTableHist (FILE *fOut) |
Protected Attributes | |
vector < EnvNAVXYTHETALATHashEntry_t * > * | Coord2StateIDHashTable |
EnvNAVXYTHETALATHashEntry_t ** | Coord2StateIDHashTable_lookup |
EnvNAVXYTHETALATHashEntry_t *(EnvironmentNAVXYTHETALAT::* | CreateNewHashEntry )(int X, int Y, int Theta) |
EnvNAVXYTHETALATHashEntry_t *(EnvironmentNAVXYTHETALAT::* | GetHashEntry )(int X, int Y, int Theta) |
int | HashTableSize |
vector < EnvNAVXYTHETALATHashEntry_t * > | StateID2CoordTable |
Definition at line 429 of file environment_navxythetalat.h.
EnvironmentNAVXYTHETALAT::EnvironmentNAVXYTHETALAT | ( | ) | [inline] |
Definition at line 433 of file environment_navxythetalat.h.
Definition at line 2012 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::ConvertStateIDPathintoXYThetaPath | ( | vector< int > * | stateIDPath, |
vector< EnvNAVXYTHETALAT3Dpt_t > * | xythetaPath | ||
) |
converts a path given by stateIDs into a sequence of coordinates. Note that since motion primitives are short actions represented as a sequence of points, the path returned by this function contains much more points than the number of points in the input path. The returned coordinates are in meters,meters,radians
Definition at line 2056 of file environment_navxythetalat.cpp.
EnvNAVXYTHETALATHashEntry_t * EnvironmentNAVXYTHETALAT::CreateNewHashEntry_hash | ( | int | X, |
int | Y, | ||
int | Theta | ||
) | [protected] |
Definition at line 2380 of file environment_navxythetalat.cpp.
EnvNAVXYTHETALATHashEntry_t * EnvironmentNAVXYTHETALAT::CreateNewHashEntry_lookup | ( | int | X, |
int | Y, | ||
int | Theta | ||
) | [protected] |
Definition at line 2324 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::GetCoordFromState | ( | int | stateID, |
int & | x, | ||
int & | y, | ||
int & | theta | ||
) | const |
returns state coordinates of state with ID=stateID
Definition at line 2039 of file environment_navxythetalat.cpp.
int EnvironmentNAVXYTHETALAT::GetFromToHeuristic | ( | int | FromStateID, |
int | ToStateID | ||
) | [virtual] |
see comments on the same function in the parent class
Implements EnvironmentNAVXYTHETALATTICE.
Definition at line 2781 of file environment_navxythetalat.cpp.
int EnvironmentNAVXYTHETALAT::GetGoalHeuristic | ( | int | stateID | ) | [virtual] |
see comments on the same function in the parent class
Implements EnvironmentNAVXYTHETALATTICE.
Definition at line 2810 of file environment_navxythetalat.cpp.
unsigned int EnvironmentNAVXYTHETALAT::GETHASHBIN | ( | unsigned int | X, |
unsigned int | Y, | ||
unsigned int | Theta | ||
) | [protected] |
Definition at line 2750 of file environment_navxythetalat.cpp.
EnvNAVXYTHETALATHashEntry_t * EnvironmentNAVXYTHETALAT::GetHashEntry_hash | ( | int | X, |
int | Y, | ||
int | Theta | ||
) | [protected] |
Definition at line 2284 of file environment_navxythetalat.cpp.
EnvNAVXYTHETALATHashEntry_t * EnvironmentNAVXYTHETALAT::GetHashEntry_lookup | ( | int | X, |
int | Y, | ||
int | Theta | ||
) | [protected] |
Definition at line 2275 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::GetPreds | ( | int | TargetStateID, |
vector< int > * | PredIDV, | ||
vector< int > * | CostV | ||
) | [virtual] |
returns all predecessors states and corresponding costs of actions
Implements EnvironmentNAVXYTHETALATTICE.
Definition at line 2492 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::GetPredsofChangedEdges | ( | vector< nav2dcell_t > const * | changedcellsV, |
vector< int > * | preds_of_changededgesIDV | ||
) | [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
Implements EnvironmentNAVXYTHETALATTICE.
Reimplemented in EnvironmentNAVXYTHETAMLEVLAT.
Definition at line 2619 of file environment_navxythetalat.cpp.
int EnvironmentNAVXYTHETALAT::GetStartHeuristic | ( | int | stateID | ) | [virtual] |
see comments on the same function in the parent class
Implements EnvironmentNAVXYTHETALATTICE.
Definition at line 2834 of file environment_navxythetalat.cpp.
int EnvironmentNAVXYTHETALAT::GetStateFromCoord | ( | int | x, |
int | y, | ||
int | theta | ||
) |
returns stateID for a state with coords x,y,theta
Definition at line 2046 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::GetSuccs | ( | int | SourceStateID, |
vector< int > * | SuccIDV, | ||
vector< int > * | CostV, | ||
vector< EnvNAVXYTHETALATAction_t * > * | actionindV = NULL |
||
) | [virtual] |
returns all successors states, costs of corresponding actions and pointers to corresponding actions, each of which is a motion primitive if actionindV is NULL, then pointers to actions are not returned
Implements EnvironmentNAVXYTHETALATTICE.
Definition at line 2430 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::GetSuccsofChangedEdges | ( | vector< nav2dcell_t > const * | changedcellsV, |
vector< int > * | succs_of_changededgesIDV | ||
) | [virtual] |
same as GetPredsofChangedEdges, but returns successor states. Both functions need to be present for incremental search
Implements EnvironmentNAVXYTHETALATTICE.
Reimplemented in EnvironmentNAVXYTHETAMLEVLAT.
Definition at line 2652 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::InitializeEnvironment | ( | ) | [protected, virtual] |
Implements EnvironmentNAVXYTHETALATTICE.
Definition at line 2689 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::PrintHashTableHist | ( | FILE * | fOut | ) | [protected] |
Definition at line 2756 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::PrintState | ( | int | stateID, |
bool | bVerbose, | ||
FILE * | fOut = NULL |
||
) | [virtual] |
prints state info (coordinates) into file
Implements DiscreteSpaceInformation.
Definition at line 2246 of file environment_navxythetalat.cpp.
virtual void EnvironmentNAVXYTHETALAT::PrintVars | ( | ) | [inline, virtual] |
see comments on the same function in the parent class
Reimplemented from EnvironmentNAVXYTHETALATTICE.
Definition at line 501 of file environment_navxythetalat.h.
void EnvironmentNAVXYTHETALAT::SetAllActionsandAllOutcomes | ( | CMDPSTATE * | state | ) | [virtual] |
see comments on the same function in the parent class
Implements EnvironmentNAVXYTHETALATTICE.
Definition at line 2552 of file environment_navxythetalat.cpp.
int EnvironmentNAVXYTHETALAT::SetGoal | ( | double | x, |
double | y, | ||
double | theta | ||
) |
sets goal in meters/radians
Definition at line 2157 of file environment_navxythetalat.cpp.
void EnvironmentNAVXYTHETALAT::SetGoalTolerance | ( | double | tol_x, |
double | tol_y, | ||
double | tol_theta | ||
) | [inline] |
sets goal tolerance. (Note goal tolerance is ignored currently)
< not used yet
Definition at line 450 of file environment_navxythetalat.h.
int EnvironmentNAVXYTHETALAT::SetStart | ( | double | x, |
double | y, | ||
double | theta | ||
) |
sets start in meters/radians
Definition at line 2204 of file environment_navxythetalat.cpp.
int EnvironmentNAVXYTHETALAT::SizeofCreatedEnv | ( | ) | [virtual] |
see comments on the same function in the parent class
Implements DiscreteSpaceInformation.
Definition at line 2860 of file environment_navxythetalat.cpp.
Definition at line 507 of file environment_navxythetalat.h.
Definition at line 511 of file environment_navxythetalat.h.
EnvNAVXYTHETALATHashEntry_t*(EnvironmentNAVXYTHETALAT::* EnvironmentNAVXYTHETALAT::CreateNewHashEntry)(int X, int Y, int Theta) [protected] |
Definition at line 522 of file environment_navxythetalat.h.
EnvNAVXYTHETALATHashEntry_t*(EnvironmentNAVXYTHETALAT::* EnvironmentNAVXYTHETALAT::GetHashEntry)(int X, int Y, int Theta) [protected] |
Definition at line 521 of file environment_navxythetalat.h.
int EnvironmentNAVXYTHETALAT::HashTableSize [protected] |
Definition at line 501 of file environment_navxythetalat.h.
Definition at line 509 of file environment_navxythetalat.h.