EnvSIPPLattice Class Reference

#include <sipp.h>

Inheritance diagram for EnvSIPPLattice:

[legend]

List of all members.

Public Member Functions
	EnvSIPPLattice ()
const envSIPPLatConfig_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_xythetasipp_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)
	~EnvSIPPLattice ()
Protected Member Functions
void	CalculateFootprintForPose (SBPL_4Dpt_t pose, vector< SBPL_4Dcell_t > *footprint)
bool	CheckQuant (FILE *fOut)
void	ComputeHeuristicValues ()
void	ComputeReplanningData ()
void	ComputeReplanningDataforAction (envSIPPLatAction_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, envSIPPLatAction_t *action)
int	getArrivalTimeToInterval (envSIPPLatHashEntry_t *state, envSIPPLatAction_t *action, int start_t, int end_t)
virtual envSIPPLatHashEntry_t *	getEntryFromID (int id)=0
int	getInterval (int x, int y, int t)
void	getIntervals (vector< int > *intervals, vector< int > *times, envSIPPLatHashEntry_t *state, envSIPPLatAction_t *action)
virtual void	GetSuccs (int SourceStateID, vector< int > *SuccIDV, vector< int > *CostV, vector< envSIPPLatAction_t * > *actionV=NULL)=0
bool	InitGeneral (vector< SBPL_xythetasipp_mprimitive > *motionprimitiveV)
void	InitializeEnvConfig (vector< SBPL_xythetasipp_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_xythetasipp_mprimitive > *motionprimitiveV)
void	PrecomputeActionswithCompleteMotionPrimitive (vector< SBPL_xythetasipp_mprimitive > *motionprimitiveV)
void	PrintHeuristicValues ()
virtual void	ReadConfiguration (FILE *fCfg)
void	ReadDynamicObstacles (FILE *fDynObs)
int	ReadinCell (SBPL_4Dcell_t *cell, char *fIn)
bool	ReadinMotionPrimitive (SBPL_xythetasipp_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)
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
EnvSIPPLat_t	envSIPPLat
envSIPPLatConfig_t	envSIPPLatCfg
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 176 of file sipp.h.

---

Constructor & Destructor Documentation

EnvSIPPLattice::EnvSIPPLattice

(

)

Definition at line 53 of file sipp.cpp.

EnvSIPPLattice::~EnvSIPPLattice

(

)

Definition at line 72 of file sipp.cpp.

---

Member Function Documentation

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

Definition at line 2094 of file sipp.cpp.

bool EnvSIPPLattice::CheckQuant

(

FILE *

fOut

)

[protected]

Definition at line 2263 of file sipp.cpp.

void EnvSIPPLattice::ComputeHeuristicValues

(

)

[protected]

Definition at line 2249 of file sipp.cpp.

void EnvSIPPLattice::ComputeReplanningData

(

)

[protected]

Definition at line 1423 of file sipp.cpp.

void EnvSIPPLattice::ComputeReplanningDataforAction

(

envSIPPLatAction_t *

action

)

[protected]

Definition at line 1314 of file sipp.cpp.

void EnvSIPPLattice::CreateStartandGoalStates

(

)

[protected]

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

Definition at line 2085 of file sipp.cpp.

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

Definition at line 446 of file sipp.cpp.

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

Definition at line 505 of file sipp.cpp.

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

Definition at line 469 of file sipp.cpp.

int EnvSIPPLattice::GetActionCost	(	int	SourceX,
		int	SourceY,
		int	SourceTheta,
		envSIPPLatAction_t *	action
	)			[protected, virtual]

Definition at line 2001 of file sipp.cpp.

int EnvSIPPLattice::getArrivalTimeToInterval	(	envSIPPLatHashEntry_t *	state,
		envSIPPLatAction_t *	action,
		int	start_t,
		int	end_t
	)			[protected]

Definition at line 593 of file sipp.cpp.

virtual envSIPPLatHashEntry_t* EnvSIPPLattice::getEntryFromID

(

int

id

)

[protected, pure virtual]

Implemented in EnvSIPPLat.

const envSIPPLatConfig_t * EnvSIPPLattice::GetEnvNavConfig

(

)

Definition at line 2608 of file sipp.cpp.

int EnvSIPPLattice::GetEnvParameter

(

const char *

parameter

)

[virtual]

Definition at line 2785 of file sipp.cpp.

void EnvSIPPLattice::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_xythetasipp_mprimitive > *	motionprimitiveV
	)

Definition at line 2672 of file sipp.cpp.

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

Implemented in EnvSIPPLat.

virtual int EnvSIPPLattice::GetGoalHeuristic

(

int

stateID

)

[pure virtual]

Implemented in EnvSIPPLat.

int EnvSIPPLattice::getGoalID

(

)

Definition at line 2668 of file sipp.cpp.

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

Definition at line 527 of file sipp.cpp.

void EnvSIPPLattice::getIntervals	(	vector< int > *	intervals,
		vector< int > *	times,
		envSIPPLatHashEntry_t *	state,
		envSIPPLatAction_t *	action
	)			[protected]

Definition at line 534 of file sipp.cpp.

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

Implements DiscreteSpaceTimeInformation.

Definition at line 2722 of file sipp.cpp.

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

Implemented in EnvSIPPLat.

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

Implemented in EnvSIPPLat.

virtual int EnvSIPPLattice::GetStartHeuristic

(

int

stateID

)

[pure virtual]

Implemented in EnvSIPPLat.

int EnvSIPPLattice::getStartID

(

)

Definition at line 2664 of file sipp.cpp.

virtual void EnvSIPPLattice::GetSuccs	(	int	SourceStateID,
		vector< int > *	SuccIDV,
		vector< int > *	CostV,
		vector< envSIPPLatAction_t * > *	actionV = NULL
	)			[protected, pure virtual]

Implemented in EnvSIPPLat.

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

Implements DiscreteSpaceTimeInformation.

Definition at line 2601 of file sipp.cpp.

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

Implemented in EnvSIPPLat.

bool EnvSIPPLattice::InitGeneral

(

vector< SBPL_xythetasipp_mprimitive > *

motionprimitiveV

)

[protected]

Definition at line 2531 of file sipp.cpp.

bool EnvSIPPLattice::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 2470 of file sipp.cpp.

bool EnvSIPPLattice::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 2374 of file sipp.cpp.

bool EnvSIPPLattice::InitializeEnv

(

const char *

sEnvFile

)

Definition at line 2356 of file sipp.cpp.

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

Definition at line 2304 of file sipp.cpp.

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

Definition at line 2289 of file sipp.cpp.

void EnvSIPPLattice::InitializeEnvConfig

(

vector< SBPL_xythetasipp_mprimitive > *

motionprimitiveV

)

[protected]

Definition at line 1904 of file sipp.cpp.

virtual void EnvSIPPLattice::InitializeEnvironment

(

)

[protected, pure virtual]

Implemented in EnvSIPPLat.

bool EnvSIPPLattice::InitializeMDPCfg

(

MDPConfig *

MDPCfg

)

Definition at line 2546 of file sipp.cpp.

void EnvSIPPLattice::InitializeTimelineMap

(

)

[protected]

Definition at line 362 of file sipp.cpp.

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

Definition at line 511 of file sipp.cpp.

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

Definition at line 2652 of file sipp.cpp.

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

Definition at line 1957 of file sipp.cpp.

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

Definition at line 1970 of file sipp.cpp.

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

Definition at line 1964 of file sipp.cpp.

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

Definition at line 296 of file sipp.h.

bool EnvSIPPLattice::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 2698 of file sipp.cpp.

bool EnvSIPPLattice::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 2710 of file sipp.cpp.

void EnvSIPPLattice::PrecomputeActions

(

)

[protected]

Definition at line 1729 of file sipp.cpp.

void EnvSIPPLattice::PrecomputeActionswithBaseMotionPrimitive

(

vector< SBPL_xythetasipp_mprimitive > *

motionprimitiveV

)

[protected]

Definition at line 1440 of file sipp.cpp.

void EnvSIPPLattice::PrecomputeActionswithCompleteMotionPrimitive

(

vector< SBPL_xythetasipp_mprimitive > *

motionprimitiveV

)

[protected]

Definition at line 1576 of file sipp.cpp.

void EnvSIPPLattice::PrintEnv_Config

(

FILE *

fOut

)

Definition at line 2630 of file sipp.cpp.

void EnvSIPPLattice::PrintHeuristicValues

(

)

[protected]

Definition at line 2556 of file sipp.cpp.

void EnvSIPPLattice::PrintTimeStat

(

FILE *

fOut

)

Definition at line 2640 of file sipp.cpp.

virtual void EnvSIPPLattice::PrintVars

(

)

[inline, virtual]

Reimplemented in EnvSIPPLat.

Definition at line 279 of file sipp.h.

void EnvSIPPLattice::ReadConfiguration

(

FILE *

fCfg

)

[protected, virtual]

Definition at line 692 of file sipp.cpp.

void EnvSIPPLattice::ReadDynamicObstacles

(

FILE *

fDynObs

)

[protected]

Definition at line 121 of file sipp.cpp.

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

Definition at line 1012 of file sipp.cpp.

bool EnvSIPPLattice::ReadinMotionPrimitive	(	SBPL_xythetasipp_mprimitive *	pMotPrim,
		FILE *	fIn,
		bool &	isWaitMotion
	)			[protected]

Definition at line 1067 of file sipp.cpp.

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

Definition at line 1040 of file sipp.cpp.

bool EnvSIPPLattice::ReadMotionPrimitives

(

FILE *

fMotPrims

)

[protected]

Definition at line 1218 of file sipp.cpp.

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

Definition at line 2220 of file sipp.cpp.

virtual void EnvSIPPLattice::SetAllActionsandAllOutcomes

(

CMDPSTATE *

state

)

[pure virtual]

Implemented in EnvSIPPLat.

void EnvSIPPLattice::SetAllPreds

(

CMDPSTATE *

state

)

[virtual]

Definition at line 2592 of file sipp.cpp.

void EnvSIPPLattice::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]

Parameters:

mapdata

if mapdata is NULL the grid is initialized to all freespace

Definition at line 618 of file sipp.cpp.

bool EnvSIPPLattice::SetEnvParameter	(	const char *	parameter,
		int	value
	)			[virtual]

Definition at line 2729 of file sipp.cpp.

bool EnvSIPPLattice::UpdateCost	(	int	x,
		int	y,
		unsigned char	newcost
	)			[virtual]

Implements DiscreteSpaceTimeInformation.

Definition at line 2614 of file sipp.cpp.

bool EnvSIPPLattice::UpdateTimelineMap

(

)

[protected]

Definition at line 375 of file sipp.cpp.

---

Member Data Documentation

vector<SBPL_4Dcell_t> EnvSIPPLattice::affectedpredstatesV [protected]

Definition at line 302 of file sipp.h.

vector<SBPL_4Dcell_t> EnvSIPPLattice::affectedsuccstatesV [protected]

Definition at line 301 of file sipp.h.

bool EnvSIPPLattice::bNeedtoRecomputeGoalHeuristics [protected]

Definition at line 311 of file sipp.h.

bool EnvSIPPLattice::bNeedtoRecomputeStartHeuristics [protected]

Definition at line 310 of file sipp.h.

vector<SBPL_DynamicObstacle_t> EnvSIPPLattice::dynamicObstacles [protected]

Definition at line 304 of file sipp.h.

EnvSIPPLat_t EnvSIPPLattice::envSIPPLat [protected]

Definition at line 300 of file sipp.h.

envSIPPLatConfig_t EnvSIPPLattice::envSIPPLatCfg [protected]

Definition at line 296 of file sipp.h.

SBPL2DGridSearch* EnvSIPPLattice::grid2Dsearchfromgoal [protected]

Definition at line 313 of file sipp.h.

SBPL2DGridSearch* EnvSIPPLattice::grid2Dsearchfromstart [protected]

Definition at line 312 of file sipp.h.

vector< vector< vector< pair<int,int> > > > EnvSIPPLattice::intervalMap [protected]

Definition at line 306 of file sipp.h.

int EnvSIPPLattice::iteration [protected]

Definition at line 303 of file sipp.h.

int EnvSIPPLattice::temporal_padding [protected]

Definition at line 307 of file sipp.h.

vector< vector< vector<int> > > EnvSIPPLattice::timelineMap [protected]

Definition at line 305 of file sipp.h.

---

The documentation for this class was generated from the following files:

• sipp.h
• sipp.cpp