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

◆ Header

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

◆ PoseGraphPtr

Constructor & Destructor Documentation

◆ PoseGraph() [1/2]

navigation_common::PoseGraph::PoseGraph ( )
default

Constructor.

◆ PoseGraph() [2/2]

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

Constructor.

Parameters
poseGraphMsgros pose graph message.

◆ ~PoseGraph()

navigation_common::PoseGraph::~PoseGraph ( )
default

Destructor.

Member Function Documentation

◆ addEdge()

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

Adds an edge to the pose graph.

Parameters
edgeedge to add to pose graph.

◆ addNode()

void navigation_common::PoseGraph::addNode ( const NodePtr node)

Adds a node to the pose graph.

Parameters
nodenode to add to pose graph.

◆ addYawToZ()

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.

◆ clear()

void navigation_common::PoseGraph::clear ( )

Clears the pose graph.

◆ discretizeYaw()

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.

◆ fillDefaultController()

void navigation_common::PoseGraph::fillDefaultController ( const LocomotionController defaultLocomotionController)

Fill default controller.

Parameters
defaultLocomotionControllerDefault locomotion controller

◆ findNearestNode()

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.

◆ findNode()

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.

◆ fromFile()

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.

◆ fromRos()

void navigation_common::PoseGraph::fromRos ( const navigation_msgs::PoseGraph poseGraphMsg)

Imports a ros pose graph message.

Parameters
poseGraphMsgros pose graph message.

◆ getDeepCopy()

PoseGraphPtr navigation_common::PoseGraph::getDeepCopy ( ) const

Create a deep copy of this pose graph.

Returns
new pose graph.

◆ getEdges()

const std::vector<EdgePtr>& navigation_common::PoseGraph::getEdges ( ) const

Gets all edges of the pose graph.

Returns
all edges.

◆ getNode()

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.

◆ getNodes()

const std::vector<NodePtr>& navigation_common::PoseGraph::getNodes ( ) const

Gets all nodes of the pose graph.

Returns
all nodes.

◆ getNumberOfEdges()

unsigned int navigation_common::PoseGraph::getNumberOfEdges ( ) const

Gets the number of edges in the pose graph.

Returns
number of edges in the pose graph.

◆ getNumberOfNodes()

unsigned int navigation_common::PoseGraph::getNumberOfNodes ( ) const

Gets the number of nodes in the pose graph.

Returns
number of nodes in the pose graph.

◆ removeEdge()

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

Removes an edge in the pose graph.

Parameters
edgepointer to the edge.

◆ removeNode()

void navigation_common::PoseGraph::removeNode ( const NodePtr node)

Removes a node in the pose graph.

Parameters
nodepointer to the node.

◆ removeNodesWithinArea()

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.

◆ removeUnconnectedNodes()

void navigation_common::PoseGraph::removeUnconnectedNodes ( )

Removes all nodes that are not connected to another node.

◆ removeUntraversableEdges()

void navigation_common::PoseGraph::removeUntraversableEdges ( )

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

◆ splitLongEdges()

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

Splits long edges by inserting new nodes.

Parameters
maxEdgeLengthmaximal length of the edges.

◆ toFile()

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.

◆ toRos()

navigation_msgs::PoseGraph navigation_common::PoseGraph::toRos ( ) const

Exports a ros pose graph message.

Returns
ros pose graph message.

Member Data Documentation

◆ edges_

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

Edges.

◆ header_

Header navigation_common::PoseGraph::header_

Header.

◆ nodes_

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

Nodes.


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