Simox  2.3.50
Saba::CSpaceTree Class Reference

Contains CSpaceTree which uses a CSpace for managing configurations. More...

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW CSpaceTree (CSpacePtr cspace)
 constructor More...
 
virtual ~CSpaceTree ()
 destructor More...
 
CSpaceNodePtr getNode (unsigned int id)
 
CSpaceNodePtr getLastAddedNode ()
 
CSpacePtr getCSpace ()
 
virtual unsigned int getNrOfNodes () const
 get number of nodes More...
 
unsigned int getDimension () const
 
virtual bool createPath (CSpaceNodePtr startNode, CSpaceNodePtr goalNode, CSpacePathPtr fillPath)
 
virtual void reset ()
 set the number of nodes to zero More...
 
virtual bool saveAllNodes (char const *filename)
 saves all nodes with ID, configuration and parentID to a file More...
 
void setUpdateChildren (bool enable)
 
bool getUpdateChildren ()
 
virtual CSpaceNodePtr appendNode (const Eigen::VectorXf &config, int parentID, bool calcDistance=false)
 creates new CSpaceNode with configuration config and parentID More...
 
virtual void removeNode (CSpaceNodePtr n)
 remove tree node More...
 
virtual unsigned int getNearestNeighborID (const Eigen::VectorXf &config, float *storeDist=NULL)
 
virtual CSpaceNodePtr getNearestNeighbor (const Eigen::VectorXf &config, float *storeDist=NULL)
 
virtual bool appendPath (CSpaceNodePtr startNode, const Eigen::VectorXf &config, int *storeLastAddedID=NULL)
 
virtual bool appendPath (CSpaceNodePtr startNode, CSpacePathPtr path, int *storeLastAddedID=NULL)
 
virtual bool appendPathUntilCollision (CSpaceNodePtr startNode, const Eigen::VectorXf &config, int *storeLastAddedID)
 
virtual std::vector< CSpaceNodePtrgetNodes ()
 
void lock ()
 
void unlock ()
 
float getTreeLength (bool useMetricWeights=true)
 
bool hasNode (CSpaceNodePtr n)
 

Protected Member Functions

virtual float getPathDist (unsigned int idStart, unsigned int idEnd, bool useMetricWeights)
 get distance of path start to end More...
 

Protected Attributes

unsigned int dimension
 shutdown tree, reset all data More...
 
CSpacePtr cspace
 
Eigen::VectorXf tmpConfig
 
std::vector< CSpaceNodePtrnodes
 
bool updateChildren
 vector with pointers to all used nodes More...
 
float randMult
 
std::map< unsigned int, CSpaceNodePtridNodeMapping
 
boost::mutex mutex
 

Detailed Description

Contains CSpaceTree which uses a CSpace for managing configurations.

A connected tree in c-space which represents a RRT.

Constructor & Destructor Documentation

Saba::CSpaceTree::CSpaceTree ( CSpacePtr  cspace)

constructor

Saba::CSpaceTree::~CSpaceTree ( )
virtual

destructor

Member Function Documentation

CSpaceNodePtr Saba::CSpaceTree::appendNode ( const Eigen::VectorXf &  config,
int  parentID,
bool  calcDistance = false 
)
virtual

creates new CSpaceNode with configuration config and parentID

Parameters
configthe configuration
parentIDID of the parent
calcDistanceif set, the distance to obstacles is calculated and stored in the node (be careful: expensive), otherwise the dist is set to -1.0
Returns
pointer to the new node on success, otherwise NULL
bool Saba::CSpaceTree::appendPath ( CSpaceNodePtr  startNode,
const Eigen::VectorXf &  config,
int *  storeLastAddedID = NULL 
)
virtual

append a path to the tree (without any checks) Intermediate configs are generated according to the current cspace

Parameters
startNodestart from this node
config
storeLastAddedIDif given, the id of the last added node is stored here
bool Saba::CSpaceTree::appendPath ( CSpaceNodePtr  startNode,
CSpacePathPtr  path,
int *  storeLastAddedID = NULL 
)
virtual

Append the given path to this tree. No checks are performed.

bool Saba::CSpaceTree::appendPathUntilCollision ( CSpaceNodePtr  startNode,
const Eigen::VectorXf &  config,
int *  storeLastAddedID 
)
virtual

Append a path to the tree until a collision is detected Intermediate configs are generated according to the current cspace

Parameters
startNodestart from this node
config
storeLastAddedIDif given, the id of the last added node is stored here
bool Saba::CSpaceTree::createPath ( CSpaceNodePtr  startNode,
CSpaceNodePtr  goalNode,
CSpacePathPtr  fillPath 
)
virtual

creates path from startNode to goalNode by traversing the parents of goalNode

Parameters
startNodeThe start.
goalNodeThe goal.
fillPathStores all configs.
Returns
true on success.
Saba::CSpacePtr Saba::CSpaceTree::getCSpace ( )
unsigned int Saba::CSpaceTree::getDimension ( ) const
CSpaceNodePtr Saba::CSpaceTree::getLastAddedNode ( )
CSpaceNodePtr Saba::CSpaceTree::getNearestNeighbor ( const Eigen::VectorXf &  config,
float *  storeDist = NULL 
)
virtual
unsigned int Saba::CSpaceTree::getNearestNeighborID ( const Eigen::VectorXf &  config,
float *  storeDist = NULL 
)
virtual

get ID of nearest neighbor

Parameters
configconfiguration
storeDistpointer to float for storing the distance (if not NULL)
Returns
ID of nearest neighbor
CSpaceNodePtr Saba::CSpaceTree::getNode ( unsigned int  id)

Return a cspace node.

Parameters
idthe id of the node
Returns
pointer to the cspace node
std::vector< CSpaceNodePtr > Saba::CSpaceTree::getNodes ( )
virtual
unsigned int Saba::CSpaceTree::getNrOfNodes ( ) const
virtual

get number of nodes

float Saba::CSpaceTree::getPathDist ( unsigned int  idStart,
unsigned int  idEnd,
bool  useMetricWeights 
)
protectedvirtual

get distance of path start to end

float Saba::CSpaceTree::getTreeLength ( bool  useMetricWeights = true)

Computes the complete length of all edges.

Parameters
useMetricWeightsWhen set, the metric weights of the corresponding cspace are used for length calculation. When the cspace is not initialized with any metric weights, this option has no effect.
bool Saba::CSpaceTree::getUpdateChildren ( )
inline
bool Saba::CSpaceTree::hasNode ( CSpaceNodePtr  n)
void Saba::CSpaceTree::lock ( )

Create a new cspace node (does !not! set the config values to zero).

Returns
pointer to new created node in cspace
void Saba::CSpaceTree::removeNode ( CSpaceNodePtr  n)
virtual

remove tree node

void Saba::CSpaceTree::reset ( )
virtual

set the number of nodes to zero

bool Saba::CSpaceTree::saveAllNodes ( char const *  filename)
virtual

saves all nodes with ID, configuration and parentID to a file

void Saba::CSpaceTree::setUpdateChildren ( bool  enable)
inline

consider child vector list when adding nodes to the tree (standard: false)

See also
CSpaceNode
void Saba::CSpaceTree::unlock ( )

Field Documentation

CSpacePtr Saba::CSpaceTree::cspace
protected
unsigned int Saba::CSpaceTree::dimension
protected

shutdown tree, reset all data

dimension of the cspace

std::map<unsigned int, CSpaceNodePtr > Saba::CSpaceTree::idNodeMapping
protected
boost::mutex Saba::CSpaceTree::mutex
protected
std::vector< CSpaceNodePtr > Saba::CSpaceTree::nodes
protected
float Saba::CSpaceTree::randMult
protected
Eigen::VectorXf Saba::CSpaceTree::tmpConfig
protected
bool Saba::CSpaceTree::updateChildren
protected

vector with pointers to all used nodes