LocalGuidance
A library that generates velocity references to follow a path.
navigation_common::PathBase< PathSegmentType > Class Template Reference

#include <PathBase.hpp>

Public Types

using PathSegmentTypePtr = std::shared_ptr< PathSegmentType >
 
using PathBasePtr = std::shared_ptr< PathBase >
 
using Header = std_msgs::Header
 

Public Member Functions

 PathBase ()=default
 
 PathBase (const PathBasePtr &pathBase)
 
virtual ~PathBase ()=default
 
void clear ()
 
unsigned int getNumberOfSegments () const
 
PathSegmentTypePtr getSegment (unsigned int id) const
 
PathSegmentTypePtr getSegmentStartingFromEnd (unsigned int id) const
 
const PathSegmentTypePtrgetFirstSegment () const
 
const PathSegmentTypePtrgetLastSegment () const
 
void addSegment (unsigned int id, const PathSegmentTypePtr &segment)
 
void addSegmentStartingFromEnd (unsigned int id, const PathSegmentTypePtr &segment)
 
void addSegmentToStart (const PathSegmentTypePtr &segment)
 
void addSegmentToEnd (const PathSegmentTypePtr &segment)
 
void removeSegment (unsigned int id)
 
void removeSegmentStartingFromEnd (unsigned int id)
 
void removeSegmentAtStart ()
 
void removeSegmentAtEnd ()
 
template<enum navigation_common::DimensionType Dim>
PathSegmentTypePtr getNearestSegment (const PoseStamped &pose) const
 
template<enum navigation_common::DimensionType Dim>
bool getClosestPoseOnPath (const PoseStamped &poseIn, PoseStamped *closestPoseOnPath, double *distance, unsigned int *idSegmentClosestPoseOnPath) const
 
template<enum navigation_common::DimensionType Dim>
void getClosestPoseOnLine (const PoseStamped &poseIn, const PoseStamped &pose0, const PoseStamped &pose1, PoseStamped *closestPoseOnLine, double *distance, bool *poseOutCloserToPose0) const
 
bool moveForwardAlongPath (const PoseStamped &poseIn, const double requestedDistance, PoseStamped *closestPoseOnPath, unsigned int *idSegmentClosestPoseOnPath, PoseStamped *poseOut, double *travelledDistanceOnPath, unsigned int *idSegmentPoseOut) const
 
template<enum navigation_common::DimensionType Dim>
bool removeSegmentsBehindPose (const PoseStamped &pose)
 
bool getIdOfSegment (const PathSegmentTypePtr &segment, unsigned int &id) const
 
unsigned int getIdFirstSegment () const
 
unsigned int getIdLastSegment () const
 
template<enum navigation_common::DimensionType Dim>
double getLengthOfSegment (unsigned int id) const
 
template<enum navigation_common::DimensionType Dim>
double getLengthOfFragment (unsigned int startId, unsigned int endId) const
 
template<enum navigation_common::DimensionType Dim>
double getLength () const
 
void interpolateOrientations ()
 
void addYawToZ (double zScale, double yawScale)
 
PathBasePtr getDeepCopyBase () const
 
template<class T >
std::enable_if<!std::numeric_limits< T >::is_integer, bool >::type almostEqual (T x, T y, int ulp=2) const
 

Public Attributes

Header header_
 Header. More...
 
std::deque< PathSegmentTypePtrpathSegments_
 Array of segments. More...
 

Member Typedef Documentation

template<typename PathSegmentType>
using navigation_common::PathBase< PathSegmentType >::Header = std_msgs::Header
template<typename PathSegmentType>
using navigation_common::PathBase< PathSegmentType >::PathBasePtr = std::shared_ptr<PathBase>
template<typename PathSegmentType>
using navigation_common::PathBase< PathSegmentType >::PathSegmentTypePtr = std::shared_ptr<PathSegmentType>

Constructor & Destructor Documentation

template<typename PathSegmentType>
navigation_common::PathBase< PathSegmentType >::PathBase ( )
default

Constructor.

template<typename PathSegmentType>
navigation_common::PathBase< PathSegmentType >::PathBase ( const PathBasePtr pathBase)

Copy Constructor.

template<typename PathSegmentType>
virtual navigation_common::PathBase< PathSegmentType >::~PathBase ( )
virtualdefault

Destructor.

Member Function Documentation

template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::addSegment ( unsigned int  id,
const PathSegmentTypePtr segment 
)

Add a segment to the path.

Parameters
idid of the new segment, following segments are pushed back.
segmentsegment to add to path.
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::addSegmentStartingFromEnd ( unsigned int  id,
const PathSegmentTypePtr segment 
)

Add a segment to the path.

Parameters
idid of the new segment, counted from the last to the first segment, following segments are pushed back.
segmentsegment to add to path.
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::addSegmentToEnd ( const PathSegmentTypePtr segment)

Add a segment to the end of the path.

Parameters
segmentsegment to add to path.
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::addSegmentToStart ( const PathSegmentTypePtr segment)

Add a segment to the start of the path.

Parameters
segmentsegment to add to path.
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::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.
template<typename PathSegmentType>
template<class T >
std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type navigation_common::PathBase< PathSegmentType >::almostEqual ( x,
y,
int  ulp = 2 
) const
inline
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::clear ( )

Clear the path.

template<typename PathSegmentType>
template<enum navigation_common::DimensionType Dim>
void navigation_common::PathBase< PathSegmentType >::getClosestPoseOnLine ( const PoseStamped poseIn,
const PoseStamped pose0,
const PoseStamped pose1,
PoseStamped closestPoseOnLine,
double *  distance,
bool *  poseOutCloserToPose0 
) const

Compute closest pose on a line segment with respect to a given pose, in the given dimension. Orientation is set according to direction between the two poses forming the segment. Adapted from Matlab function 'closestPointOnLine'.

Parameters
poseInreference pose.
pose0pose0, where the line segment starts.
pose1pose1, where the line segment ends.
closestPointOnLinepose on the segment which is closest to reference pose.
distanceminimum distance from reference point to closest point on the line.
poseOutCloserToPose0tru if computed pose is closer to pose0, false if closer to pose1.
template<typename PathSegmentType>
template<enum navigation_common::DimensionType Dim>
bool navigation_common::PathBase< PathSegmentType >::getClosestPoseOnPath ( const PoseStamped poseIn,
PoseStamped closestPoseOnPath,
double *  distance,
unsigned int *  idSegmentClosestPoseOnPath 
) const

Get the closest pose on the path the given dimension.

Parameters
poseIninput pose.
closestPoseOnPathpose belonging to the path that is closest to poseIn (in the given dimension).
distanceminimum distance from reference point to closest point on the path.
idSegmentClosestPoseOnPathsegment id where closestPoseOnPath belongs. WARNING: if the new pose is very close to segment0.pose, then the new pose belongs segment0!
Returns
true if it was possible to find closest pose, false in case of error.
template<typename PathSegmentType>
PathBasePtr navigation_common::PathBase< PathSegmentType >::getDeepCopyBase ( ) const

Create a deep copy of this base path.

Returns
new path.
template<typename PathSegmentType>
const PathSegmentTypePtr& navigation_common::PathBase< PathSegmentType >::getFirstSegment ( ) const

Get the first path segment.

Returns
first segment.
template<typename PathSegmentType>
unsigned int navigation_common::PathBase< PathSegmentType >::getIdFirstSegment ( ) const

Get id of first segment.

Returns
id of first segment.
template<typename PathSegmentType>
unsigned int navigation_common::PathBase< PathSegmentType >::getIdLastSegment ( ) const

Get id of last segment.

Returns
id of last segment.
template<typename PathSegmentType>
bool navigation_common::PathBase< PathSegmentType >::getIdOfSegment ( const PathSegmentTypePtr segment,
unsigned int &  id 
) const

Get the id of path segment

Parameters
[in]segmentthe desired path segment.
[out]idof the path segment.
Returns
true if segment has been found.
template<typename PathSegmentType>
const PathSegmentTypePtr& navigation_common::PathBase< PathSegmentType >::getLastSegment ( ) const

Get the last path segment.

Returns
last segment.
template<typename PathSegmentType>
template<enum navigation_common::DimensionType Dim>
double navigation_common::PathBase< PathSegmentType >::getLength ( ) const

Get the length of the path in the given dimension.

Returns
length of the path.
template<typename PathSegmentType>
template<enum navigation_common::DimensionType Dim>
double navigation_common::PathBase< PathSegmentType >::getLengthOfFragment ( unsigned int  startId,
unsigned int  endId 
) const

Get the length of the path fragment between two given segments in the given dimension.

Parameters
startIdstart id of the fragment.
endIdend id of the fragment.
Returns
length of the path fragment.
template<typename PathSegmentType>
template<enum navigation_common::DimensionType Dim>
double navigation_common::PathBase< PathSegmentType >::getLengthOfSegment ( unsigned int  id) const

Get the length of a segment. The first segment length is defined to be 0.

Parameters
idid of the segment.
Returns
length of the segment.
template<typename PathSegmentType>
template<enum navigation_common::DimensionType Dim>
PathSegmentTypePtr navigation_common::PathBase< PathSegmentType >::getNearestSegment ( const PoseStamped pose) const

Get the nearest segment on the path in the given dimension.

Parameters
poseinput pose.
Returns
nearest segment.
template<typename PathSegmentType>
unsigned int navigation_common::PathBase< PathSegmentType >::getNumberOfSegments ( ) const

Get the number of path segments.

Returns
number of path segments.
template<typename PathSegmentType>
PathSegmentTypePtr navigation_common::PathBase< PathSegmentType >::getSegment ( unsigned int  id) const

Get a path segment.

Parameters
idid of the segment to get.
Returns
segment.
template<typename PathSegmentType>
PathSegmentTypePtr navigation_common::PathBase< PathSegmentType >::getSegmentStartingFromEnd ( unsigned int  id) const

Get a path segment.

Parameters
idid of the segment to get, counted from the end to the start segment.
Returns
segment.
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::interpolateOrientations ( )

Overwrites the orientations with tangents to the path. The orientation is set to vector which points from the last position to the next one. Only overwrites orientations where ignoreOrientation is true.

template<typename PathSegmentType>
bool navigation_common::PathBase< PathSegmentType >::moveForwardAlongPath ( const PoseStamped poseIn,
const double  requestedDistance,
PoseStamped closestPoseOnPath,
unsigned int *  idSegmentClosestPoseOnPath,
PoseStamped poseOut,
double *  travelledDistanceOnPath,
unsigned int *  idSegmentPoseOut 
) const

Compute a pose laying on the path (by projecting the specified input pose) and then move forward on the path based on the distance specified. The new pose is not added to the path. Input pose is projected on the path to have a starting pose, so the requested distance is travelled starting from the projected pose on the path, in a forward direction.

Parameters
poseInpose where to start moving to create the new pose. PoseIn is projected on the path to get starting pose.
requestedDistancedistance to be travelled along the path, in a forward direction.
closestPoseOnPathpose belonging to the path that is closest to poseIn (in xyz dimension, no rotation taken into account).
idSegmentClosestPoseOnPathsegment id where closestPoseOnPath belongs. WARNING: if the new pose very close to segment0.pose, then the new pose belongs segment0!
poseOutpose on the path at distance distance from starting segment, oriented as line segment where it lays.
travelledDistanceOnPathactual distance travelled on the path from projected pose to poseOut.
idSegmentPoseOutid of the segment where poseOut belongs to.
Returns
true if it was possible to move along path, false in case of error.
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::removeSegment ( unsigned int  id)

Remove a segment from the path.

Parameters
idid of the segment to remove.
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::removeSegmentAtEnd ( )

Remove the segment at the end of the path.

template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::removeSegmentAtStart ( )

Remove the segment at the start of the path.

template<typename PathSegmentType>
template<enum navigation_common::DimensionType Dim>
bool navigation_common::PathBase< PathSegmentType >::removeSegmentsBehindPose ( const PoseStamped pose)

Remove segments that are behind the passed pose. Closest segment to pose, in the given dimension, is cut and has as new origin the closest pose on the path, respect to the given pose.

Parameters
posepose behind which segments should be removed. If pose is not on the path, then closest pose on the path is computed.
Returns
true if 0 or more segments were successfully removed.
template<typename PathSegmentType>
void navigation_common::PathBase< PathSegmentType >::removeSegmentStartingFromEnd ( unsigned int  id)

Remove a segment from the path.

Parameters
idid of the segment to remove, counted from the last to the first segment.

Member Data Documentation

template<typename PathSegmentType>
Header navigation_common::PathBase< PathSegmentType >::header_

Header.

template<typename PathSegmentType>
std::deque<PathSegmentTypePtr> navigation_common::PathBase< PathSegmentType >::pathSegments_

Array of segments.


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