EnvironmentNAVXYTHETATIMELATTICE Class Reference

#include <envTime.h>

Inheritance diagram for EnvironmentNAVXYTHETATIMELATTICE:

Public Member Functions
	EnvironmentNAVXYTHETATIMELATTICE ()
const EnvNAVXYTHETATIMELATConfig_t *	GetEnvNavConfig ()
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 nominalvel_mpersecs, double timetoturn45degsinplace_secs, unsigned char obsthresh, unsigned char dyn_obs_thresh, vector< SBPL_xythetatime_mprimitive > motionprimitiveV)
virtual int	GetFromToHeuristic (int FromStateID, int ToStateID)=0
virtual int	GetGoalHeuristic (int stateID)=0
int	getGoalID ()
unsigned char	GetMapCost (int x, int y)
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 int	GetStartHeuristic (int stateID)=0
int	getStartID ()
virtual void	GetSuccs (int SourceStateID, vector< int > SuccIDV, vector< int > CostV)
virtual void	GetSuccsofChangedEdges (vector< nav2dcell_t > const changedcellsV, vector< int > succs_of_changededgesIDV)=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 nominalvel_mpersecs, double timetoturn45degsinplace_secs, unsigned char obsthresh, unsigned char dynobsthresh, 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 nominalvel_mpersecs, double timetoturn45degsinplace_secs, unsigned char obsthresh, unsigned char dynobsthresh, const char sMotPrimFile, vector< SBPL_DynamicObstacle_t > &dynObs)
bool	InitializeEnv (const char *sEnvFile)
bool	InitializeEnv (const char sEnvFile, const vector< sbpl_2Dpt_t > &perimeterptsV, const char sMotPrimFile)
bool	InitializeEnv (const char sEnvFile, const vector< sbpl_2Dpt_t > &perimeterptsV, const char sMotPrimFile, const char *sDynObsFile)
bool	InitializeMDPCfg (MDPConfig *MDPCfg)
bool	IsObstacle (int x, int y)
bool	IsValidConfiguration (int X, int Y, int Theta)
bool	IsWithinMapCell (int X, int Y)
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 void	SetAllActionsandAllOutcomes (CMDPSTATE *state)=0
virtual void	SetAllPreds (CMDPSTATE *state)
virtual bool	SetEnvParameter (const char *parameter, int value)
bool	UpdateCost (int x, int y, unsigned char newcost)
	~EnvironmentNAVXYTHETATIMELATTICE ()
Protected Member Functions
void	CalculateFootprintForPose (SBPL_4Dpt_t pose, vector< SBPL_4Dcell_t > *footprint)
bool	CheckQuant (FILE *fOut)
void	ComputeHeuristicValues ()
void	ComputeReplanningData ()
void	ComputeReplanningDataforAction (EnvNAVXYTHETATIMELATAction_t *action)
void	CreateStartandGoalStates ()
double	dynObsPtSqrDist (SBPL_4Dcell_t cell, SBPL_Traj_Pt_t p)
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, int SourceTime, EnvNAVXYTHETATIMELATAction_t *action)
unsigned char	getDynamicObstacleCost (SBPL_4Dcell_t cell)
virtual EnvNAVXYTHETATIMELATHashEntry_t *	getEntryFromID (int id)=0
int	getInterval (int x, int y, int t)
virtual void	GetSuccs (int SourceStateID, vector< int > SuccIDV, vector< int > CostV, vector< EnvNAVXYTHETATIMELATAction_t * > *actionindV=NULL)=0
bool	InitGeneral (vector< SBPL_xythetatime_mprimitive > *motionprimitiveV)
void	InitializeEnvConfig (vector< SBPL_xythetatime_mprimitive > *motionprimitiveV)
virtual void	InitializeEnvironment ()=0
void	InitializeTimelineMap ()
void	intervals2Timeline (int x, int y)
bool	IsValidCell (int X, int Y)
void	PrecomputeActions ()
void	PrecomputeActionswithBaseMotionPrimitive (vector< SBPL_xythetatime_mprimitive > *motionprimitiveV)
void	PrecomputeActionswithCompleteMotionPrimitive (vector< SBPL_xythetatime_mprimitive > *motionprimitiveV)
void	PrintHeuristicValues ()
virtual void	ReadConfiguration (FILE *fCfg)
void	ReadDynamicObstacles (FILE *fDynObs)
int	ReadinCell (SBPL_4Dcell_t cell, char fIn)
bool	ReadinMotionPrimitive (SBPL_xythetatime_mprimitive pMotPrim, FILE fIn)
int	ReadinPose (SBPL_4Dpt_t pose, char fIn)
bool	ReadMotionPrimitives (FILE *fMotPrims)
void	RemoveSourceFootprint (SBPL_4Dpt_t sourcepose, vector< SBPL_4Dcell_t > *footprint)
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
EnvironmentNAVXYTHETATIMELAT_t	EnvNAVXYTHETATIMELAT
EnvNAVXYTHETATIMELATConfig_t	EnvNAVXYTHETATIMELATCfg
SBPL2DGridSearch *	grid2Dsearchfromgoal
SBPL2DGridSearch *	grid2Dsearchfromstart
vector< vector< vector< pair < int, int > > > >	intervalMap
int	iteration
int	temporal_padding
vector< vector< vector< int > > >	timelineMap

Detailed Description

Definition at line 173 of file envTime.h.

Constructor & Destructor Documentation

EnvironmentNAVXYTHETATIMELATTICE::EnvironmentNAVXYTHETATIMELATTICE ( )

Definition at line 53 of file envTime.cpp.

EnvironmentNAVXYTHETATIMELATTICE::~EnvironmentNAVXYTHETATIMELATTICE ( )

Definition at line 72 of file envTime.cpp.

Member Function Documentation

void EnvironmentNAVXYTHETATIMELATTICE::CalculateFootprintForPose	(	SBPL_4Dpt_t	pose,
		vector< SBPL_4Dcell_t > *	footprint
	)			`[protected]`

Definition at line 2037 of file envTime.cpp.

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

Definition at line 2206 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::ComputeHeuristicValues ( ) [protected]

Definition at line 2192 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::ComputeReplanningData ( ) [protected]

Definition at line 1313 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::ComputeReplanningDataforAction ( EnvNAVXYTHETATIMELATAction_t * action ) [protected]

Definition at line 1204 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::CreateStartandGoalStates ( ) [protected]

double EnvironmentNAVXYTHETATIMELATTICE::dynObsPtSqrDist	(	SBPL_4Dcell_t	cell,
		SBPL_Traj_Pt_t	p
	)			`[protected]`

Definition at line 2021 of file envTime.cpp.

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

Definition at line 2028 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::FillInBubble	(	int	CenterX,
		int	CenterY,
		int	T,
		int	rad
	)			`[protected]`

Definition at line 445 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::FillInBubbleCell	(	int	x,
		int	y,
		int	T
	)			`[protected]`

Definition at line 504 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::FillInBubbleColumn	(	int	x,
		int	topY,
		int	botY,
		int	T
	)			`[protected]`

Definition at line 468 of file envTime.cpp.

int EnvironmentNAVXYTHETATIMELATTICE::GetActionCost	(	int	SourceX,
		int	SourceY,
		int	SourceTheta,
		int	SourceTime,
		EnvNAVXYTHETATIMELATAction_t *	action
	)			`[protected, virtual]`

Definition at line 1888 of file envTime.cpp.

unsigned char EnvironmentNAVXYTHETATIMELATTICE::getDynamicObstacleCost ( SBPL_4Dcell_t cell ) [protected]

Definition at line 1962 of file envTime.cpp.

virtual EnvNAVXYTHETATIMELATHashEntry_t* EnvironmentNAVXYTHETATIMELATTICE::getEntryFromID ( int id ) [protected, pure virtual]

Implemented in EnvironmentNAVXYTHETATIMELAT.

const EnvNAVXYTHETATIMELATConfig_t * EnvironmentNAVXYTHETATIMELATTICE::GetEnvNavConfig ( )

Definition at line 2555 of file envTime.cpp.

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

Definition at line 2758 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::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 *	nominalvel_mpersecs,
		double *	timetoturn45degsinplace_secs,
		unsigned char *	obsthresh,
		unsigned char *	dyn_obs_thresh,
		vector< SBPL_xythetatime_mprimitive > *	motionprimitiveV
	)

Definition at line 2619 of file envTime.cpp.

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

Implemented in EnvironmentNAVXYTHETATIMELAT.

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

Implemented in EnvironmentNAVXYTHETATIMELAT.

int EnvironmentNAVXYTHETATIMELATTICE::getGoalID ( )

Definition at line 2615 of file envTime.cpp.

int EnvironmentNAVXYTHETATIMELATTICE::getInterval	(	int	x,
		int	y,
		int	t
	)			`[protected]`

Definition at line 526 of file envTime.cpp.

unsigned char EnvironmentNAVXYTHETATIMELATTICE::GetMapCost	(	int	x,
		int	y
	)			`[virtual]`

Implements DiscreteSpaceTimeInformation.

Definition at line 2695 of file envTime.cpp.

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

Implemented in EnvironmentNAVXYTHETATIMELAT.

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

Implemented in EnvironmentNAVXYTHETATIMELAT.

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

Implemented in EnvironmentNAVXYTHETATIMELAT.

int EnvironmentNAVXYTHETATIMELATTICE::getStartID ( )

Definition at line 2611 of file envTime.cpp.

virtual void EnvironmentNAVXYTHETATIMELATTICE::GetSuccs	(	int	SourceStateID,
		vector< int > *	SuccIDV,
		vector< int > *	CostV,
		vector< EnvNAVXYTHETATIMELATAction_t * > *	actionindV = `NULL`
	)			`[protected, pure virtual]`

Implemented in EnvironmentNAVXYTHETATIMELAT.

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

Implements DiscreteSpaceTimeInformation.

Definition at line 2546 of file envTime.cpp.

virtual void EnvironmentNAVXYTHETATIMELATTICE::GetSuccsofChangedEdges	(	vector< nav2dcell_t > const *	changedcellsV,
		vector< int > *	succs_of_changededgesIDV
	)			`[pure virtual]`

Implemented in EnvironmentNAVXYTHETATIMELAT.

bool EnvironmentNAVXYTHETATIMELATTICE::InitGeneral ( vector< SBPL_xythetatime_mprimitive > * motionprimitiveV ) [protected]

Definition at line 2476 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::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	nominalvel_mpersecs,
		double	timetoturn45degsinplace_secs,
		unsigned char	obsthresh,
		unsigned char	dynobsthresh,
		const char *	sMotPrimFile
	)

Parameters:

mapdata

if mapdata is NULL the grid is initialized to all freespace

Definition at line 2415 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::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	nominalvel_mpersecs,
		double	timetoturn45degsinplace_secs,
		unsigned char	obsthresh,
		unsigned char	dynobsthresh,
		const char *	sMotPrimFile,
		vector< SBPL_DynamicObstacle_t > &	dynObs
	)			`[virtual]`

Parameters:

mapdata

if mapdata is NULL the grid is initialized to all freespace

Implements DiscreteSpaceTimeInformation.

Definition at line 2319 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::InitializeEnv ( const char * sEnvFile )

Definition at line 2301 of file envTime.cpp.

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

Definition at line 2247 of file envTime.cpp.

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

Definition at line 2232 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::InitializeEnvConfig ( vector< SBPL_xythetatime_mprimitive > * motionprimitiveV ) [protected]

Definition at line 1791 of file envTime.cpp.

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

Implemented in EnvironmentNAVXYTHETATIMELAT.

bool EnvironmentNAVXYTHETATIMELATTICE::InitializeMDPCfg ( MDPConfig * MDPCfg )

Definition at line 2491 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::InitializeTimelineMap ( ) [protected]

Definition at line 362 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::intervals2Timeline	(	int	x,
		int	y
	)			`[protected]`

Definition at line 510 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::IsObstacle	(	int	x,
		int	y
	)

Definition at line 2599 of file envTime.cpp.

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

Definition at line 1844 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::IsValidConfiguration	(	int	X,
		int	Y,
		int	Theta
	)

Definition at line 1857 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::IsWithinMapCell	(	int	X,
		int	Y
	)

Definition at line 1851 of file envTime.cpp.

static bool EnvironmentNAVXYTHETATIMELATTICE::pairCompare	(	pair< int, int >	i,
		pair< int, int >	j
	)			`[inline, static, protected]`

Definition at line 290 of file envTime.h.

bool EnvironmentNAVXYTHETATIMELATTICE::PoseContToDisc	(	double	px,
		double	py,
		double	pth,
		double	pt,
		int &	ix,
		int &	iy,
		int &	ith,
		int &	it
	)			const

Transform a pose into discretized form. The angle 'pth' is considered to be valid if it lies between -2pi and 2pi (some people will prefer 0<=pth<2pi, others -pi<pth<=pi, so this compromise should suit everyone).

Note:: Even if this method returns false, you can still use the computed indices, for example to figure out how big your map should have been.

Returns:: true if the resulting indices lie within the grid bounds and the angle was valid.

Definition at line 2658 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::PoseDiscToCont	(	int	ix,
		int	iy,
		int	ith,
		int	it,
		double &	px,
		double &	py,
		double &	pth,
		double &	pt
	)			const

Transform grid indices into a continuous pose. The computed angle lies within 0<=pth<2pi.

Note:: Even if this method returns false, you can still use the computed indices, for example to figure out poses that lie outside of your current map.

Returns:: true if all the indices are within grid bounds.

Definition at line 2683 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::PrecomputeActions ( ) [protected]

Definition at line 1619 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::PrecomputeActionswithBaseMotionPrimitive ( vector< SBPL_xythetatime_mprimitive > * motionprimitiveV ) [protected]

Definition at line 1330 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::PrecomputeActionswithCompleteMotionPrimitive ( vector< SBPL_xythetatime_mprimitive > * motionprimitiveV ) [protected]

Definition at line 1466 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::PrintEnv_Config ( FILE * fOut )

Definition at line 2577 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::PrintHeuristicValues ( ) [protected]

Definition at line 2501 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::PrintTimeStat ( FILE * fOut )

Definition at line 2587 of file envTime.cpp.

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

Reimplemented in EnvironmentNAVXYTHETATIMELAT.

Definition at line 276 of file envTime.h.

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

Definition at line 607 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::ReadDynamicObstacles ( FILE * fDynObs ) [protected]

Definition at line 121 of file envTime.cpp.

int EnvironmentNAVXYTHETATIMELATTICE::ReadinCell	(	SBPL_4Dcell_t *	cell,
		char *	fIn
	)			`[protected]`

Definition at line 916 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::ReadinMotionPrimitive	(	SBPL_xythetatime_mprimitive *	pMotPrim,
		FILE *	fIn
	)			`[protected]`

Definition at line 971 of file envTime.cpp.

int EnvironmentNAVXYTHETATIMELATTICE::ReadinPose	(	SBPL_4Dpt_t *	pose,
		char *	fIn
	)			`[protected]`

Definition at line 944 of file envTime.cpp.

bool EnvironmentNAVXYTHETATIMELATTICE::ReadMotionPrimitives ( FILE * fMotPrims ) [protected]

Definition at line 1119 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::RemoveSourceFootprint	(	SBPL_4Dpt_t	sourcepose,
		vector< SBPL_4Dcell_t > *	footprint
	)			`[protected]`

Definition at line 2163 of file envTime.cpp.

virtual void EnvironmentNAVXYTHETATIMELATTICE::SetAllActionsandAllOutcomes ( CMDPSTATE * state ) [pure virtual]

Implemented in EnvironmentNAVXYTHETATIMELAT.

void EnvironmentNAVXYTHETATIMELATTICE::SetAllPreds ( CMDPSTATE * state ) [virtual]

Definition at line 2537 of file envTime.cpp.

void EnvironmentNAVXYTHETATIMELATTICE::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]`