#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 Member Functions
bool	obstacleCheck (const maps::grid::Vector3d &pos, double theta, const traversability_generator3d::TraversabilityConfig &travConf, const sbpl_spline_primitives::SplinePrimitivesConfig &splineConf, const std::string &nodeName="node")

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

virtual	~EnvironmentXYZTheta ()

void	updateMap (std::shared_ptr< const traversability_generator3d::TravMap3d > travMap)

virtual bool	InitializeEnv (const char *sEnvFile)

virtual bool	InitializeMDPCfg (MDPConfig *MDPCfg)

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

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

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

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

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

traversability_generator3d::TravGenNode *	findMatchingTraversabilityPatchAt (const Eigen::Vector3d &pos)

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

const std::shared_ptr< const traversability_generator3d::TravMap3d >	getTraversabilityMap () 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)

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

std::vector< Hash >	idToHash

std::vector< Distance >	travNodeIdToDistance

std::shared_ptr< const traversability_generator3d::TravMap3d >	travMap

PreComputedMotions	availableMotions

ThetaNode *	startThetaNode

XYZNode *	startXYZNode

ThetaNode *	goalThetaNode

XYZNode *	goalXYZNode

Member Typedef Documentation

◆ 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< const traversability_generator3d::TravMap3d >	travMap,
		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.

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

◆ findMatchingTraversabilityPatchAt()

traversability_generator3d::TravGenNode * ugv_nav4d::EnvironmentXYZTheta::findMatchingTraversabilityPatchAt ( const Eigen::Vector3d & pos )

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,
		bool	setZToZero
	)

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:

◆ 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:

◆ 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 std::shared_ptr< const traversability_generator3d::TravMap3d > ugv_nav4d::EnvironmentXYZTheta::getTraversabilityMap ( ) const

◆ 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 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:

◆ 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< const traversability_generator3d::TravMap3d > travMap )

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).

◆ 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

◆ travMap

std::shared_ptr<const traversability_generator3d::TravMap3d> ugv_nav4d::EnvironmentXYZTheta::travMap

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 Member Functions

Public Attributes

Protected Types

Protected Member Functions

Protected Attributes

Member Typedef Documentation

◆ XYZNode

Constructor & Destructor Documentation

◆ EnvironmentXYZTheta()

◆ ~EnvironmentXYZTheta()

Member Function Documentation

◆ checkStartGoalNode()

◆ clear()

◆ createNewState()

◆ createNewStateFromPose()

◆ createNewXYZState()

◆ dijkstraComputeCost()

◆ enablePathStatistics()

◆ findMatchingTraversabilityPatchAt()

◆ findTrajectoryOutOfObstacle()

◆ getAvailableMotions()

◆ GetFromToHeuristic()

◆ GetGoalHeuristic()

◆ getMotion()

◆ getMotions()

◆ GetPreds()

◆ GetStartHeuristic()

◆ getStatePosition()

◆ GetSuccs() [1/2]

◆ GetSuccs() [2/2]

◆ getTrajectory()

◆ getTraversabilityMap()

◆ InitializeEnv()

◆ InitializeMDPCfg()

◆ obstacleCheck()

◆ PrintEnv_Config()

◆ PrintState()

◆ SetAllActionsandAllOutcomes()

◆ SetAllPreds()

◆ setGoal()

◆ setStart()

◆ setTravConfig()

◆ SizeofCreatedEnv()

◆ updateMap()

Member Data Documentation

◆ availableMotions

◆ goalThetaNode

◆ goalXYZNode

◆ idToHash

◆ robotHalfSize

◆ searchGrid

◆ startThetaNode

◆ startXYZNode

◆ travMap

◆ travNodeIdToDistance