#include <EnvironmentXYZTheta.hpp>

Inheritance diagram for ugv_nav4d::EnvironmentXYZTheta:

Collaboration diagram for ugv_nav4d::EnvironmentXYZTheta:

Classes
struct	Distance

struct	EnvironmentXYZThetaException

struct	Hash

struct	PlannerData

struct	ThetaNode

Public Types
typedef traversability_generator3d::TraversabilityGenerator3d::MLGrid	MLGrid

Public Member Functions
bool	obstacleCheck (const maps::grid::Vector3d &pos, double theta, const ObstacleMapGenerator3D &obsGen, const traversability_generator3d::TraversabilityConfig &travConf, const sbpl_spline_primitives::SplinePrimitivesConfig &splineConf, const std::string &nodeName="node")

	EnvironmentXYZTheta (std::shared_ptr< MLGrid > mlsGrid, const traversability_generator3d::TraversabilityConfig &travConf, const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig, const Mobility &mobilityConfig)

virtual	~EnvironmentXYZTheta ()

void	updateMap (std::shared_ptr< MLGrid > mlsGrid)

void	setInitialPatch (const Eigen::Affine3d &ground2Mls, double patchRadius)

virtual bool	InitializeEnv (const char *sEnvFile)

virtual bool	InitializeMDPCfg (MDPConfig *MDPCfg)

void	expandMap (const std::vector< Eigen::Vector3d > &positions)

std::shared_ptr< trajectory_follower::SubTrajectory >	findTrajectoryOutOfObstacle (const Eigen::Vector3d &start, double theta, const Eigen::Affine3d &ground2Body)

virtual int	GetFromToHeuristic (int FromStateID, int ToStateID)
	heuristic estimate from state FromStateID to state ToStateID More...

virtual int	GetStartHeuristic (int stateID)
	heuristic estimate from start state to state with stateID More...

virtual int	GetGoalHeuristic (int stateID)
	heuristic estimate from state with stateID to goal state More...

virtual void	GetPreds (int TargetStateID, std::vector< int > PredIDV, std::vector< int > CostV)

virtual void	GetSuccs (int SourceStateID, std::vector< int > SuccIDV, std::vector< int > CostV)

virtual void	GetSuccs (int SourceStateID, std::vector< int > SuccIDV, std::vector< int > CostV, std::vector< size_t > &motionIdV)

virtual void	PrintEnv_Config (FILE *fOut)

virtual void	PrintState (int stateID, bool bVerbose, FILE *fOut=0)

virtual void	SetAllActionsandAllOutcomes (CMDPSTATE *state)

virtual void	SetAllPreds (CMDPSTATE *state)

virtual int	SizeofCreatedEnv ()

void	setStart (const Eigen::Vector3d &startPos, double theta)

void	setGoal (const Eigen::Vector3d &goalPos, double theta)

maps::grid::Vector3d	getStatePosition (const int stateID) const

const Motion &	getMotion (const int fromStateID, const int toStateID)

const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > &	getTraversabilityMap () const

const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > &	getObstacleMap () const

traversability_generator3d::TraversabilityGenerator3d &	getTravGen ()

traversability_generator3d::TraversabilityGenerator3d &	getObstacleGen ()

const MLGrid &	getMlsMap () const

std::vector< Motion >	getMotions (const std::vector< int > &stateIDPath)

void	getTrajectory (const std::vector< int > &stateIDPath, std::vector< trajectory_follower::SubTrajectory > &result, bool setZToZero, const Eigen::Vector3d &startPos, const Eigen::Vector3d &goalPos, const double &goalHeading, const Eigen::Affine3d &plan2Body=Eigen::Affine3d::Identity())

const PreComputedMotions &	getAvailableMotions () const

void	clear ()

void	setTravConfig (const traversability_generator3d::TraversabilityConfig &cfg)

void	dijkstraComputeCost (const traversability_generator3d::TravGenNode *source, std::vector< double > &outDistances, const double maxDist) const

void	enablePathStatistics (bool enable)

Public Attributes
Eigen::Vector3d	robotHalfSize

Protected Types
typedef maps::grid::TraversabilityNode< PlannerData >	XYZNode

Protected Member Functions
ThetaNode *	createNewState (const DiscreteTheta &curTheta, EnvironmentXYZTheta::XYZNode *curNode)

XYZNode *	createNewXYZState (traversability_generator3d::TravGenNode *travNode)

ThetaNode *	createNewStateFromPose (const std::string &name, const Eigen::Vector3d &pos, double theta, ugv_nav4d::EnvironmentXYZTheta::XYZNode **xyzBackNode)

bool	checkStartGoalNode (const std::string &name, traversability_generator3d::TravGenNode *node, double theta)

traversability_generator3d::TravGenNode *	findObstacleNode (const traversability_generator3d::TravGenNode *travNode) const

Protected Attributes
traversability_generator3d::TraversabilityGenerator3d	travGen

ObstacleMapGenerator3D	obsGen

std::shared_ptr< MLGrid >	mlsGrid

maps::grid::TraversabilityMap3d< XYZNode * >	searchGrid

std::vector< Hash >	idToHash

std::vector< Distance >	travNodeIdToDistance

PreComputedMotions	availableMotions

ThetaNode *	startThetaNode

XYZNode *	startXYZNode

ThetaNode *	goalThetaNode

XYZNode *	goalXYZNode

traversability_generator3d::TravGenNode *	obstacleStartNode

Member Typedef Documentation

◆ MLGrid

typedef traversability_generator3d::TraversabilityGenerator3d::MLGrid ugv_nav4d::EnvironmentXYZTheta::MLGrid

◆ XYZNode

typedef maps::grid::TraversabilityNode<PlannerData> ugv_nav4d::EnvironmentXYZTheta::XYZNode

protected

A position on the traversability map

Constructor & Destructor Documentation

◆ EnvironmentXYZTheta()

ugv_nav4d::EnvironmentXYZTheta::EnvironmentXYZTheta	(	std::shared_ptr< MLGrid >	mlsGrid,
		const traversability_generator3d::TraversabilityConfig &	travConf,
		const sbpl_spline_primitives::SplinePrimitivesConfig &	primitiveConfig,
		const Mobility &	mobilityConfig
	)

Parameters

generateDebugData If true, lots of debug information will becollected and stored in members starting with debug

Here is the call graph for this function:

◆ ~EnvironmentXYZTheta()

ugv_nav4d::EnvironmentXYZTheta::~EnvironmentXYZTheta ( )

virtual

Here is the call graph for this function:

Member Function Documentation

◆ checkStartGoalNode()

bool ugv_nav4d::EnvironmentXYZTheta::checkStartGoalNode	(	const std::string &	name,
		traversability_generator3d::TravGenNode *	node,
		double	theta
	)

protected

Here is the call graph for this function:

Here is the caller graph for this function:

◆ clear()

void ugv_nav4d::EnvironmentXYZTheta::clear ( )

Clears the state of the environment. Clears everything except the mls map.

Here is the caller graph for this function:

◆ createNewState()

EnvironmentXYZTheta::ThetaNode * ugv_nav4d::EnvironmentXYZTheta::createNewState	(	const DiscreteTheta &	curTheta,
		EnvironmentXYZTheta::XYZNode *	curNode
	)

protected

Here is the caller graph for this function:

◆ createNewStateFromPose()

EnvironmentXYZTheta::ThetaNode * ugv_nav4d::EnvironmentXYZTheta::createNewStateFromPose	(	const std::string &	name,
		const Eigen::Vector3d &	pos,
		double	theta,
		ugv_nav4d::EnvironmentXYZTheta::XYZNode **	xyzBackNode
	)

protected

Here is the call graph for this function:

Here is the caller graph for this function:

◆ createNewXYZState()

EnvironmentXYZTheta::XYZNode * ugv_nav4d::EnvironmentXYZTheta::createNewXYZState ( traversability_generator3d::TravGenNode * travNode )

protected

Here is the caller graph for this function:

◆ dijkstraComputeCost()

void ugv_nav4d::EnvironmentXYZTheta::dijkstraComputeCost	(	const traversability_generator3d::TravGenNode *	source,
		std::vector< double > &	outDistances,
		const double	maxDist
	)		const

Parameters

maxDist The value that should be used as maximum distance. This value is used for non-traversable nodes and for initialization.

◆ enablePathStatistics()

void ugv_nav4d::EnvironmentXYZTheta::enablePathStatistics ( bool enable )

Should a computationally expensive obstacle check be done to check whether the robot bounding box is in collision with obstacles. This mode is useful for highly cluttered and tight spaced environments

◆ expandMap()

void ugv_nav4d::EnvironmentXYZTheta::expandMap ( const std::vector< Eigen::Vector3d > & positions )

Expand the underlying travmap and obstacle map starting from all given positions.

◆ findObstacleNode()

traversability_generator3d::TravGenNode * ugv_nav4d::EnvironmentXYZTheta::findObstacleNode ( const traversability_generator3d::TravGenNode * travNode ) const

protected

Find the obstacle node corresponding to travNode

Here is the caller graph for this function:

◆ findTrajectoryOutOfObstacle()

std::shared_ptr< SubTrajectory > ugv_nav4d::EnvironmentXYZTheta::findTrajectoryOutOfObstacle	(	const Eigen::Vector3d &	start,
		double	theta,
		const Eigen::Affine3d &	ground2Body
	)

Returns the trajectory of least resistance to leave the obstacle.

Parameters

	start	start position that is inside an obstacle
	theta	robot orientation
[out]	outNewStart	The new start position of the robot after it has moved out of the obstacle in the map frame

Returns: the best trajectory that gets the robot out of the obstacle. Or an empty trajectory if no way out can be found

Here is the call graph for this function:

◆ getAvailableMotions()

const PreComputedMotions & ugv_nav4d::EnvironmentXYZTheta::getAvailableMotions ( ) const

◆ GetFromToHeuristic()

int ugv_nav4d::EnvironmentXYZTheta::GetFromToHeuristic	(	int	FromStateID,
		int	ToStateID
	)

virtual

heuristic estimate from state FromStateID to state ToStateID

◆ GetGoalHeuristic()

int ugv_nav4d::EnvironmentXYZTheta::GetGoalHeuristic ( int stateID )

virtual

heuristic estimate from state with stateID to goal state

Here is the call graph for this function:

◆ getMlsMap()

const EnvironmentXYZTheta::MLGrid & ugv_nav4d::EnvironmentXYZTheta::getMlsMap ( ) const

◆ getMotion()

const Motion & ugv_nav4d::EnvironmentXYZTheta::getMotion	(	const int	fromStateID,
		const int	toStateID
	)

returns the motion connection fromStateID and toStateID.

Exceptions

std::runtime_error if no matching motion exists

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getMotions()

vector< Motion > ugv_nav4d::EnvironmentXYZTheta::getMotions ( const std::vector< int > & stateIDPath )

Here is the call graph for this function:

◆ getObstacleGen()

traversability_generator3d::TraversabilityGenerator3d & ugv_nav4d::EnvironmentXYZTheta::getObstacleGen ( )

◆ getObstacleMap()

const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > & ugv_nav4d::EnvironmentXYZTheta::getObstacleMap ( ) const

◆ GetPreds()

void ugv_nav4d::EnvironmentXYZTheta::GetPreds	(	int	TargetStateID,
		std::vector< int > *	PredIDV,
		std::vector< int > *	CostV
	)

virtual

◆ GetStartHeuristic()

int ugv_nav4d::EnvironmentXYZTheta::GetStartHeuristic ( int stateID )

virtual

heuristic estimate from start state to state with stateID

Here is the call graph for this function:

◆ getStatePosition()

maps::grid::Vector3d ugv_nav4d::EnvironmentXYZTheta::getStatePosition ( const int stateID ) const

Here is the caller graph for this function:

◆ GetSuccs() [1/2]

virtual void ugv_nav4d::EnvironmentXYZTheta::GetSuccs	(	int	SourceStateID,
		std::vector< int > *	SuccIDV,
		std::vector< int > *	CostV
	)

virtual

Here is the caller graph for this function:

◆ GetSuccs() [2/2]

virtual void ugv_nav4d::EnvironmentXYZTheta::GetSuccs	(	int	SourceStateID,
		std::vector< int > *	SuccIDV,
		std::vector< int > *	CostV,
		std::vector< size_t > &	motionIdV
	)

virtual

◆ getTrajectory()

void ugv_nav4d::EnvironmentXYZTheta::getTrajectory	(	const std::vector< int > &	stateIDPath,
		std::vector< trajectory_follower::SubTrajectory > &	result,
		bool	setZToZero,
		const Eigen::Vector3d &	startPos,
		const Eigen::Vector3d &	goalPos,
		const double &	goalHeading,
		const Eigen::Affine3d &	plan2Body = `Eigen::Affine3d::Identity()`
	)

Here is the call graph for this function:

◆ getTraversabilityMap()

const maps::grid::TraversabilityMap3d< traversability_generator3d::TravGenNode * > & ugv_nav4d::EnvironmentXYZTheta::getTraversabilityMap ( ) const

◆ getTravGen()

traversability_generator3d::TraversabilityGenerator3d & ugv_nav4d::EnvironmentXYZTheta::getTravGen ( )

◆ InitializeEnv()

bool ugv_nav4d::EnvironmentXYZTheta::InitializeEnv ( const char * sEnvFile )

virtual

◆ InitializeMDPCfg()

bool ugv_nav4d::EnvironmentXYZTheta::InitializeMDPCfg ( MDPConfig * MDPCfg )

virtual

◆ obstacleCheck()

bool ugv_nav4d::EnvironmentXYZTheta::obstacleCheck	(	const maps::grid::Vector3d &	pos,
		double	theta,
		const ObstacleMapGenerator3D &	obsGen,
		const traversability_generator3d::TraversabilityConfig &	travConf,
		const sbpl_spline_primitives::SplinePrimitivesConfig &	splineConf,
		const std::string &	nodeName = `"node"`
	)

Parameters

pos	Position in map frame

Here is the call graph for this function:

Here is the caller graph for this function:

◆ PrintEnv_Config()

void ugv_nav4d::EnvironmentXYZTheta::PrintEnv_Config ( FILE * fOut )

virtual

◆ PrintState()

void ugv_nav4d::EnvironmentXYZTheta::PrintState	(	int	stateID,
		bool	bVerbose,
		FILE *	fOut = `0`
	)

virtual

◆ SetAllActionsandAllOutcomes()

void ugv_nav4d::EnvironmentXYZTheta::SetAllActionsandAllOutcomes ( CMDPSTATE * state )

virtual

◆ SetAllPreds()

void ugv_nav4d::EnvironmentXYZTheta::SetAllPreds ( CMDPSTATE * state )

virtual

◆ setGoal()

void ugv_nav4d::EnvironmentXYZTheta::setGoal	(	const Eigen::Vector3d &	goalPos,
		double	theta
	)

Here is the call graph for this function:

◆ setInitialPatch()

void ugv_nav4d::EnvironmentXYZTheta::setInitialPatch	(	const Eigen::Affine3d &	ground2Mls,
		double	patchRadius
	)

◆ setStart()

void ugv_nav4d::EnvironmentXYZTheta::setStart	(	const Eigen::Vector3d &	startPos,
		double	theta
	)

Here is the call graph for this function:

◆ setTravConfig()

void ugv_nav4d::EnvironmentXYZTheta::setTravConfig ( const traversability_generator3d::TraversabilityConfig & cfg )

◆ SizeofCreatedEnv()

int ugv_nav4d::EnvironmentXYZTheta::SizeofCreatedEnv ( )

virtual

◆ updateMap()

void ugv_nav4d::EnvironmentXYZTheta::updateMap ( std::shared_ptr< MLGrid > mlsGrid )

Here is the call graph for this function:

Member Data Documentation

◆ availableMotions

PreComputedMotions ugv_nav4d::EnvironmentXYZTheta::availableMotions

protected

◆ goalThetaNode

ThetaNode* ugv_nav4d::EnvironmentXYZTheta::goalThetaNode

protected

◆ goalXYZNode

XYZNode* ugv_nav4d::EnvironmentXYZTheta::goalXYZNode

protected

◆ idToHash

std::vector<Hash> ugv_nav4d::EnvironmentXYZTheta::idToHash

protected

maps sbpl state ids to internal planner state (Hash).

◆ mlsGrid

std::shared_ptr<MLGrid > ugv_nav4d::EnvironmentXYZTheta::mlsGrid

protected

◆ obsGen

ObstacleMapGenerator3D ugv_nav4d::EnvironmentXYZTheta::obsGen

protected

◆ obstacleStartNode

traversability_generator3d::TravGenNode* ugv_nav4d::EnvironmentXYZTheta::obstacleStartNode

protected

Start node in obstacle map

◆ robotHalfSize

Eigen::Vector3d ugv_nav4d::EnvironmentXYZTheta::robotHalfSize

◆ searchGrid

maps::grid::TraversabilityMap3d<XYZNode *> ugv_nav4d::EnvironmentXYZTheta::searchGrid

protected

◆ startThetaNode

ThetaNode* ugv_nav4d::EnvironmentXYZTheta::startThetaNode

protected

◆ startXYZNode

XYZNode* ugv_nav4d::EnvironmentXYZTheta::startXYZNode

protected

◆ travGen

traversability_generator3d::TraversabilityGenerator3d ugv_nav4d::EnvironmentXYZTheta::travGen

protected

◆ travNodeIdToDistance

std::vector<Distance> ugv_nav4d::EnvironmentXYZTheta::travNodeIdToDistance

protected

Contains the distance from each travNode to start-node and goal-node Stored in real-world coordinates (i.e. do NOT scale with gridResolution before use)

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

src/EnvironmentXYZTheta.hpp
src/EnvironmentXYZTheta.cpp

Classes

Public Types

Public Member Functions

Public Attributes

Protected Types

Protected Member Functions

Protected Attributes

Member Typedef Documentation

◆ MLGrid

◆ XYZNode

Constructor & Destructor Documentation

◆ EnvironmentXYZTheta()

◆ ~EnvironmentXYZTheta()

Member Function Documentation

◆ checkStartGoalNode()

◆ clear()

◆ createNewState()

◆ createNewStateFromPose()

◆ createNewXYZState()

◆ dijkstraComputeCost()

◆ enablePathStatistics()

◆ expandMap()

◆ findObstacleNode()

◆ findTrajectoryOutOfObstacle()

◆ getAvailableMotions()

◆ GetFromToHeuristic()

◆ GetGoalHeuristic()

◆ getMlsMap()

◆ getMotion()

◆ getMotions()

◆ getObstacleGen()

◆ getObstacleMap()

◆ GetPreds()

◆ GetStartHeuristic()

◆ getStatePosition()

◆ GetSuccs() [1/2]

◆ GetSuccs() [2/2]

◆ getTrajectory()

◆ getTraversabilityMap()

◆ getTravGen()

◆ InitializeEnv()

◆ InitializeMDPCfg()

◆ obstacleCheck()

◆ PrintEnv_Config()

◆ PrintState()

◆ SetAllActionsandAllOutcomes()

◆ SetAllPreds()

◆ setGoal()

◆ setInitialPatch()

◆ setStart()

◆ setTravConfig()

◆ SizeofCreatedEnv()

◆ updateMap()

Member Data Documentation

◆ availableMotions

◆ goalThetaNode

◆ goalXYZNode

◆ idToHash

◆ mlsGrid

◆ obsGen

◆ obstacleStartNode

◆ robotHalfSize

◆ searchGrid

◆ startThetaNode

◆ startXYZNode

◆ travGen

◆ travNodeIdToDistance