EnvDBubbleLattice Class Reference

#include <envDBubble.h>

Inheritance diagram for EnvDBubbleLattice:

Public Member Functions
	EnvDBubbleLattice ()
const envDBubbleLatConfig_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_xythetatimebubble_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, vector< int > stateBubbles, vector< int > bubbleCollisions)
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)
	~EnvDBubbleLattice ()
Protected Member Functions
void	CalculateFootprintForPose (SBPL_4Dpt_t pose, vector< SBPL_4Dcell_t > *footprint)
bool	CheckQuant (FILE *fOut)
void	ComputeHeuristicValues ()
void	ComputeReplanningData ()
void	ComputeReplanningDataforAction (envDBubbleLatAction_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, SBPL_DynamicObstacle_t *obs, int rad, int ID, bool isInnerBubble)
void	FillInBubbleCell (int x, int y, int T, SBPL_DynamicObstacle_t *obs, int ID, bool isInnerBubble)
void	FillInBubbleColumn (int x, int topY, int botY, int T, SBPL_DynamicObstacle_t *obs, int ID, bool isInnerBubble)
virtual int	GetActionCost (int SourceX, int SourceY, int SourceTheta, int SourceTime, envDBubbleLatAction_t action, vector< int > bubbleCollisions=NULL)
unsigned char	getDynamicObstacleCost (SBPL_4Dcell_t cell, vector< int > *bubbleCollisions)
int	getNumBubbles ()
virtual void	GetSuccs (int SourceStateID, vector< int > SuccIDV, vector< int > CostV, vector< int > stateBubbles, vector< int > bubbleCollisions, vector< envDBubbleLatAction_t * > *actionV=NULL)=0
bool	InitGeneral (vector< SBPL_xythetatimebubble_mprimitive > *motionprimitiveV)
void	InitializeBubbleMap ()
void	InitializeEnvConfig (vector< SBPL_xythetatimebubble_mprimitive > *motionprimitiveV)
virtual void	InitializeEnvironment ()=0
bool	isInBubble (int x, int y, int t)
bool	IsValidCell (int X, int Y)
void	PrecomputeActions ()
void	PrecomputeActionswithBaseMotionPrimitive (vector< SBPL_xythetatimebubble_mprimitive > *motionprimitiveV)
void	PrecomputeActionswithCompleteMotionPrimitive (vector< SBPL_xythetatimebubble_mprimitive > *motionprimitiveV)
void	PrintHeuristicValues ()
virtual void	ReadConfiguration (FILE *fCfg)
void	ReadDynamicObstacles (FILE *fDynObs)
int	ReadinCell (SBPL_4Dcell_t cell, char fIn)
bool	ReadinMotionPrimitive (SBPL_xythetatimebubble_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)
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)
void	UpdateBubbleMap ()
Protected Attributes
vector< SBPL_4Dcell_t >	affectedpredstatesV
vector< SBPL_4Dcell_t >	affectedsuccstatesV
bool	bNeedtoRecomputeGoalHeuristics
bool	bNeedtoRecomputeStartHeuristics
vector< bool >	bubble4Dactive
vector< vector < envDBubbleLat_BubbleCell_t > >	bubblemap
vector< SBPL_DynamicObstacle_t >	dynamicObstacles
EnvDBubbleLat_t	envDBubbleLat
envDBubbleLatConfig_t	envDBubbleLatCfg
SBPL2DGridSearch *	grid2Dsearchfromgoal
SBPL2DGridSearch *	grid2Dsearchfromstart
int	iteration
int	temporal_padding

Detailed Description

Definition at line 192 of file envDBubble.h.

Constructor & Destructor Documentation

EnvDBubbleLattice::EnvDBubbleLattice ( )

Definition at line 49 of file envDBubble.cpp.

EnvDBubbleLattice::~EnvDBubbleLattice ( )

Definition at line 68 of file envDBubble.cpp.

Member Function Documentation

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

Definition at line 2214 of file envDBubble.cpp.

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

Definition at line 2383 of file envDBubble.cpp.

void EnvDBubbleLattice::ComputeHeuristicValues ( ) [protected]

Definition at line 2369 of file envDBubble.cpp.

void EnvDBubbleLattice::ComputeReplanningData ( ) [protected]

Definition at line 1394 of file envDBubble.cpp.

void EnvDBubbleLattice::ComputeReplanningDataforAction ( envDBubbleLatAction_t * action ) [protected]

Definition at line 1285 of file envDBubble.cpp.

void EnvDBubbleLattice::CreateStartandGoalStates ( ) [protected]

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

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

Definition at line 2205 of file envDBubble.cpp.

void EnvDBubbleLattice::FillInBubble	(	int	CenterX,
		int	CenterY,
		int	T,
		SBPL_DynamicObstacle_t *	obs,
		int	rad,
		int	ID,
		bool	isInnerBubble
	)			`[protected]`

Definition at line 439 of file envDBubble.cpp.

void EnvDBubbleLattice::FillInBubbleCell	(	int	x,
		int	y,
		int	T,
		SBPL_DynamicObstacle_t *	obs,
		int	ID,
		bool	isInnerBubble
	)			`[protected]`

Definition at line 506 of file envDBubble.cpp.

void EnvDBubbleLattice::FillInBubbleColumn	(	int	x,
		int	topY,
		int	botY,
		int	T,
		SBPL_DynamicObstacle_t *	obs,
		int	ID,
		bool	isInnerBubble
	)			`[protected]`

Definition at line 462 of file envDBubble.cpp.

int EnvDBubbleLattice::GetActionCost	(	int	SourceX,
		int	SourceY,
		int	SourceTheta,
		int	SourceTime,
		envDBubbleLatAction_t *	action,
		vector< int > *	bubbleCollisions = `NULL`
	)			`[protected, virtual]`

Definition at line 1970 of file envDBubble.cpp.

unsigned char EnvDBubbleLattice::getDynamicObstacleCost	(	SBPL_4Dcell_t	cell,
		vector< int > *	bubbleCollisions
	)			`[protected]`

Definition at line 2058 of file envDBubble.cpp.

const envDBubbleLatConfig_t * EnvDBubbleLattice::GetEnvNavConfig ( )

Definition at line 2732 of file envDBubble.cpp.

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

Definition at line 2909 of file envDBubble.cpp.

void EnvDBubbleLattice::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_xythetatimebubble_mprimitive > *	motionprimitiveV
	)

Definition at line 2796 of file envDBubble.cpp.

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

Implemented in EnvDBubbleLat.

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

Implemented in EnvDBubbleLat.

int EnvDBubbleLattice::getGoalID ( )

Definition at line 2792 of file envDBubble.cpp.

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

Implements DiscreteSpaceTimeInformation.

Definition at line 2846 of file envDBubble.cpp.

int EnvDBubbleLattice::getNumBubbles ( ) [protected, virtual]

Reimplemented from DiscreteSpaceTimeInformation.

Definition at line 545 of file envDBubble.cpp.

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

Implemented in EnvDBubbleLat.

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

Implemented in EnvDBubbleLat.

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

Implemented in EnvDBubbleLat.

int EnvDBubbleLattice::getStartID ( )

Definition at line 2788 of file envDBubble.cpp.

virtual void EnvDBubbleLattice::GetSuccs	(	int	SourceStateID,
		vector< int > *	SuccIDV,
		vector< int > *	CostV,
		vector< int > *	stateBubbles,
		vector< int > *	bubbleCollisions,
		vector< envDBubbleLatAction_t * > *	actionV = `NULL`
	)			`[protected, pure virtual]`

Implemented in EnvDBubbleLat.

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

Reimplemented from DiscreteSpaceTimeInformation.

Definition at line 2725 of file envDBubble.cpp.

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

Implements DiscreteSpaceTimeInformation.

Definition at line 2719 of file envDBubble.cpp.

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

Implemented in EnvDBubbleLat.

bool EnvDBubbleLattice::InitGeneral ( vector< SBPL_xythetatimebubble_mprimitive > * motionprimitiveV ) [protected]

Definition at line 2649 of file envDBubble.cpp.

void EnvDBubbleLattice::InitializeBubbleMap ( ) [protected]

Definition at line 358 of file envDBubble.cpp.

bool EnvDBubbleLattice::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 2589 of file envDBubble.cpp.

bool EnvDBubbleLattice::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 2495 of file envDBubble.cpp.

bool EnvDBubbleLattice::InitializeEnv ( const char * sEnvFile )

Definition at line 2478 of file envDBubble.cpp.

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

Definition at line 2426 of file envDBubble.cpp.

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

Definition at line 2409 of file envDBubble.cpp.

void EnvDBubbleLattice::InitializeEnvConfig ( vector< SBPL_xythetatimebubble_mprimitive > * motionprimitiveV ) [protected]

Definition at line 1873 of file envDBubble.cpp.

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

Implemented in EnvDBubbleLat.

bool EnvDBubbleLattice::InitializeMDPCfg ( MDPConfig * MDPCfg )

Definition at line 2664 of file envDBubble.cpp.

bool EnvDBubbleLattice::isInBubble	(	int	x,
		int	y,
		int	t
	)			`[protected]`

Definition at line 3415 of file envDBubble.cpp.

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

Definition at line 2776 of file envDBubble.cpp.

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

Definition at line 1926 of file envDBubble.cpp.

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

Definition at line 1939 of file envDBubble.cpp.

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

Definition at line 1933 of file envDBubble.cpp.

bool EnvDBubbleLattice::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 2822 of file envDBubble.cpp.

bool EnvDBubbleLattice::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 2834 of file envDBubble.cpp.

void EnvDBubbleLattice::PrecomputeActions ( ) [protected]

Definition at line 1700 of file envDBubble.cpp.

void EnvDBubbleLattice::PrecomputeActionswithBaseMotionPrimitive ( vector< SBPL_xythetatimebubble_mprimitive > * motionprimitiveV ) [protected]

Definition at line 1411 of file envDBubble.cpp.

void EnvDBubbleLattice::PrecomputeActionswithCompleteMotionPrimitive ( vector< SBPL_xythetatimebubble_mprimitive > * motionprimitiveV ) [protected]

Definition at line 1547 of file envDBubble.cpp.

void EnvDBubbleLattice::PrintEnv_Config ( FILE * fOut )

Definition at line 2754 of file envDBubble.cpp.

void EnvDBubbleLattice::PrintHeuristicValues ( ) [protected]

Definition at line 2674 of file envDBubble.cpp.

void EnvDBubbleLattice::PrintTimeStat ( FILE * fOut )

Definition at line 2764 of file envDBubble.cpp.

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

Reimplemented in EnvDBubbleLat.

Definition at line 296 of file envDBubble.h.

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

Definition at line 623 of file envDBubble.cpp.

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

Definition at line 117 of file envDBubble.cpp.

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

Definition at line 936 of file envDBubble.cpp.

bool EnvDBubbleLattice::ReadinMotionPrimitive	(	SBPL_xythetatimebubble_mprimitive *	pMotPrim,
		FILE *	fIn,
		bool &	isWaitMotion
	)			`[protected]`

Definition at line 991 of file envDBubble.cpp.

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

Definition at line 964 of file envDBubble.cpp.

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

Definition at line 1141 of file envDBubble.cpp.

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

Definition at line 2340 of file envDBubble.cpp.

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

Implemented in EnvDBubbleLat.

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

Definition at line 2710 of file envDBubble.cpp.

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