LocalGuidance
A library that generates velocity references to follow a path.
navigation_common::PoseGraph Class Reference

#include <PoseGraph.hpp>

Public Types

typedef std::shared_ptr< PoseGraphPoseGraphPtr
 
typedef std_msgs::Header Header
 

Public Member Functions

 PoseGraph ()=default
 
 PoseGraph (const navigation_msgs::PoseGraph &poseGraphMsg)
 
 ~PoseGraph ()=default
 
void fromRos (const navigation_msgs::PoseGraph &poseGraphMsg)
 
navigation_msgs::PoseGraph toRos () const
 
bool fromFile (const std::string &absolutePath)
 
bool toFile (const std::string &absolutePath) const
 
void clear ()
 
unsigned int getNumberOfNodes () const
 
unsigned int getNumberOfEdges () const
 
const std::vector< NodePtr > & getNodes () const
 
const std::vector< EdgePtr > & getEdges () const
 
template<enum DimensionType Dim>
NodePtr findNearestNode (const PoseStamped &poseStamped, double maxDistance=std::numeric_limits< double >::infinity()) const
 
template<enum DimensionType Dim>
NodePtr findNode (const PoseStamped &poseStamped) const
 
NodePtr getNode (const PoseStampedPtr &poseStamped) const
 
void addNode (const NodePtr &node)
 
void addEdge (const EdgePtr &edge)
 
void removeNode (const NodePtr &node)
 
void removeEdge (const EdgePtr edge)
 
void removeUntraversableEdges ()
 
void removeUnconnectedNodes ()
 
void removeNodesWithinArea (const double lowX, const double highX, const double lowY, const double highY, const double lowZ=std::numeric_limits< double >::min(), const double highZ=std::numeric_limits< double >::max())
 
void fillDefaultController (const LocomotionController &defaultLocomotionController)
 
void splitLongEdges (double maxEdgeLength)
 
void discretizeYaw (unsigned int steps, const LocomotionController &yawingLocomotionController)
 
void addYawToZ (double zScale, double yawScale)
 
PoseGraphPtr getDeepCopy () const
 

Public Attributes

Header header_
 Header. More...
 
std::vector< NodePtrnodes_
 Nodes. More...
 
std::vector< EdgePtredges_
 Edges. More...
 

Member Typedef Documentation

typedef std_msgs::Header navigation_common::PoseGraph::Header

Constructor & Destructor Documentation

navigation_common::PoseGraph::PoseGraph ( )
default

Constructor.

navigation_common::PoseGraph::PoseGraph ( const navigation_msgs::PoseGraph poseGraphMsg)
explicit

Constructor.

Parameters
poseGraphMsgros pose graph message.
navigation_common::PoseGraph::~PoseGraph ( )
default

Destructor.

Member Function Documentation

void navigation_common::PoseGraph::addEdge ( const EdgePtr edge)

Adds an edge to the pose graph.

Parameters
edgeedge to add to pose graph.
void navigation_common::PoseGraph::addNode ( const NodePtr node)

Adds a node to the pose graph.

Parameters
nodenode to add to pose graph.
void navigation_common::PoseGraph::addYawToZ ( double  zScale,
double  yawScale 
)

Add the yaw angle to z coordinate for visual debugging purposes. z := zScale*z + yawScale*yaw

Parameters
zScalescaling of z.
yawScalescaling of yaw.
void navigation_common::PoseGraph::clear ( )

Clears the pose graph.

void navigation_common::PoseGraph::discretizeYaw ( unsigned int  steps,
const LocomotionController yawingLocomotionController 
)

Discretize the yaw angle of the pose graph:

  • Where the orientation is important, it is projected to the xy-plane.
  • Where the orientation is ignored, new nodes with different orientations in the xy-plane are introduced.
    Parameters
    steps(preferably even) number of discretization steps.
    turningLocomotionControllerlocomotion controller which is used for yawing.
void navigation_common::PoseGraph::fillDefaultController ( const LocomotionController defaultLocomotionController)

Fill default controller.

Parameters
defaultLocomotionControllerDefault locomotion controller
template<enum DimensionType Dim>
NodePtr navigation_common::PoseGraph::findNearestNode ( const PoseStamped poseStamped,
double  maxDistance = std::numeric_limits<double>::infinity() 
) const
inline

Finds the nearest node in the pose graph in the given dimension.

Parameters
[in]poseStampedstamped pose to search for.
[in]maxDistancemax distance for the nearest node.
Returns
node on the pose graph. empty pointer if not within max distance.
template<enum DimensionType Dim>
NodePtr navigation_common::PoseGraph::findNode ( const PoseStamped poseStamped) const
inline

Finds a node in the pose graph.

Parameters
[in]poseStampedstamped pose of the node.
Returns
node on the pose graph. empty pointer if not found.
bool navigation_common::PoseGraph::fromFile ( const std::string &  absolutePath)

Loads the pose graph from a file.

Parameters
absolutePathabsolute path to pose graph file.
Returns
true if successful.
void navigation_common::PoseGraph::fromRos ( const navigation_msgs::PoseGraph poseGraphMsg)

Imports a ros pose graph message.

Parameters
poseGraphMsgros pose graph message.
PoseGraphPtr navigation_common::PoseGraph::getDeepCopy ( ) const

Create a deep copy of this pose graph.

Returns
new pose graph.
const std::vector<EdgePtr>& navigation_common::PoseGraph::getEdges ( ) const

Gets all edges of the pose graph.

Returns
all edges.
NodePtr navigation_common::PoseGraph::getNode ( const PoseStampedPtr poseStamped) const

Gets a node in the pose graph.

Parameters
[in]poseStampedpointer to the stamped pose of the node.
Returns
node on the pose graph. empty pointer if not found.
const std::vector<NodePtr>& navigation_common::PoseGraph::getNodes ( ) const

Gets all nodes of the pose graph.

Returns
all nodes.
unsigned int navigation_common::PoseGraph::getNumberOfEdges ( ) const

Gets the number of edges in the pose graph.

Returns
number of edges in the pose graph.
unsigned int navigation_common::PoseGraph::getNumberOfNodes ( ) const

Gets the number of nodes in the pose graph.

Returns
number of nodes in the pose graph.
void navigation_common::PoseGraph::removeEdge ( const EdgePtr  edge)

Removes an edge in the pose graph.

Parameters
edgepointer to the edge.
void navigation_common::PoseGraph::removeNode ( const NodePtr node)

Removes a node in the pose graph.

Parameters
nodepointer to the node.
void navigation_common::PoseGraph::removeNodesWithinArea ( const double  lowX,
const double  highX,
const double  lowY,
const double  highY,
const double  lowZ = std::numeric_limits< double >::min(),
const double  highZ = std::numeric_limits< double >::max() 
)

Removes all nodes that are within the specified rectangular area.

Parameters
[in]lowXlower bound of rectangle in x-direction.
[in]highXupper bound of rectangle in x-direction.
[in]lowYlower bound of rectangle in y-direction.
[in]highYupper bound of rectangle in y-direction.
void navigation_common::PoseGraph::removeUnconnectedNodes ( )

Removes all nodes that are not connected to another node.

void navigation_common::PoseGraph::removeUntraversableEdges ( )

Removes all edges that are not traversable (zero traversability).

void navigation_common::PoseGraph::splitLongEdges ( double  maxEdgeLength)

Splits long edges by inserting new nodes.

Parameters
maxEdgeLengthmaximal length of the edges.
bool navigation_common::PoseGraph::toFile ( const std::string &  absolutePath) const

Saves the pose graph to a file.

Parameters
absolutePathabsolute path to pose graph file.
Returns
true if successful.
navigation_msgs::PoseGraph navigation_common::PoseGraph::toRos ( ) const

Exports a ros pose graph message.

Returns
ros pose graph message.

Member Data Documentation

std::vector<EdgePtr> navigation_common::PoseGraph::edges_

Edges.

Header navigation_common::PoseGraph::header_

Header.

std::vector<NodePtr> navigation_common::PoseGraph::nodes_

Nodes.


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