LocalGuidance
A library that generates velocity references to follow a path.
local_guidance_path_manager::SubPathsManager Class Reference

#include <SubPathsManager.hpp>

Classes

class  CurrentSubPath
 

Public Types

using SubPathsManagerPtr = std::unique_ptr< SubPathsManager >
 

Public Member Functions

PathTypePtr getCurrentSubPathInOutputFrame () const
 
PathTypePtr getCurrentSubPathInFrameId (const std::string &frame_id) const
 
int getIndexCurrentSubPath () const
 
int getNumberRemainingSubPathsToFollow () const
 
void notifyCompletionCurrentSubPath ()
 
void printGlobalPath () const
 
void printGlobalPathInDesiredOutputFrameId () const
 
bool areAllSubPathsFollowed () const
 
void manageCurrentPathTraversability (bool *currentSubPathWasUpdated, bool *stopRobotNow)
 
bool setLocalGuidanceModeUpToLookaheadPose (const std::string &requestedLocalGuidanceMode, const double &lookaheadDistanceAlongPath, std::string *message=nullptr)
 

Static Public Member Functions

static SubPathsManagerPtr create (ros::NodeHandle &nodeHandle, const PathType &fullPath, const bool &checkTraversability, const std::string &traversabilityMapFrameId)
 

Private Member Functions

 SubPathsManager (ros::NodeHandle &nodeHandle, const bool &checkTraversability, const std::string &traversabilityMapFrameId)
 
bool init (const PathType &fullPath)
 
void clearInternalState ()
 
bool readPersistentParameters ()
 
PathTypePtr convertPathInFrameId (const PathTypePtr &originalPath, const std::string &frame_id) const
 
PathTypePtr convertPathInFrameId (const PathType &originalPath, const std::string &frame_id) const
 
bool splitPath (const PathType &fullPath)
 
void printPath (const PathType &path, const std::string &info) const
 
TraversabilityManagerPtr createTraversabilityManager (const PathTypePtr &path) const
 
PathTypePtr getPathAheadRobot (const PathTypePtr &path) const
 
PoseStampedType getRobotPose () const
 
void extractPortionSubPathAheadRobot ()
 
navigation_msgs::PathLocalGuidance portionSubPathAheadRobotToRosMsg () const
 
bool mergeTraversableSubPathInCurrentOne ()
 
bool isInputPathValid (const PathType &fullPath) const
 

Private Attributes

ros::NodeHandle & nodeHandle_
 Ros node handle. More...
 
std::vector< PathTypesubPaths_
 
CurrentSubPath currentSubPath_
 Holds data about the current sub-path that is being followed. More...
 
TraversabilityManagerPtr traversabilityManager_ {nullptr}
 Manages traversability of current sub-path. More...
 
bool checkTraversability_ {false}
 Tells whether traversability should be checked for the current sub-path. More...
 
std::string desiredOutputFrameId_ {"odom"}
 Contains the frame_id which is used to convert the current sub-path into another frame. More...
 
std::string traversabilityMapFrameId_ {"map"}
 Contains the frame_id of the traversability map. More...
 
std::string robotFrameId_ {"base"}
 Contains the frame_id of the robot. More...
 
tf::TransformListener tfListener_
 TF listener. More...
 
bool allSubPathsAreFollowed_ {false}
 Tells if all the sub-paths are completed. More...
 
PathTypePtr originalFullPath_ {nullptr}
 Holds the original full path used to initialize this SubPathsManager object. More...
 

Member Typedef Documentation

◆ SubPathsManagerPtr

Constructor & Destructor Documentation

◆ SubPathsManager()

local_guidance_path_manager::SubPathsManager::SubPathsManager ( ros::NodeHandle &  nodeHandle,
const bool &  checkTraversability,
const std::string &  traversabilityMapFrameId 
)
explicitprivate

Constructor

Parameters
[in]nodeHandleROS node handler.
[in]checkTraversabilitytells if current sub-path should be checked for traversability.
[in]traversabilityMapFrameIdholds 'frame_id' of traversability map. Used in case is true.

Member Function Documentation

◆ areAllSubPathsFollowed()

bool local_guidance_path_manager::SubPathsManager::areAllSubPathsFollowed ( ) const

Tells whether all the sub-paths are already completed. I.e. function notifyCompletionCurrentSubPath() has been called as many times as the number of sub-paths.

Returns
true if all the sub-paths were completed.

◆ clearInternalState()

void local_guidance_path_manager::SubPathsManager::clearInternalState ( )
private

Clears internal state of the SubPathsManager.

◆ convertPathInFrameId() [1/2]

PathTypePtr local_guidance_path_manager::SubPathsManager::convertPathInFrameId ( const PathTypePtr originalPath,
const std::string &  frame_id 
) const
private

Returns a copy of the specified path. The path is converted in the specified "frame_id". This function checks for the latest TF transformation between path.header.frame_id and "frame_id before converting (if necessary) the path to the latter frame.

Parameters
[in]originalPathpath to be converted.
[in]frame_idtarget frame_id to convert he sub-path to.
Returns
path converted expressed in "frame_id", nullptr in case of error.

◆ convertPathInFrameId() [2/2]

PathTypePtr local_guidance_path_manager::SubPathsManager::convertPathInFrameId ( const PathType originalPath,
const std::string &  frame_id 
) const
private

Returns a copy of the specified path. The path is converted in the specified "frame_id". This function checks for the latest TF transformation between path.header.frame_id and "frame_id before converting (if necessary) the path to the latter frame.

Parameters
[in]originalPathpath to be converted.
[in]frame_idtarget frame_id to convert he sub-path to.
Returns
path converted expressed in "frame_id", nullptr in case of error.

◆ create()

static SubPathsManagerPtr local_guidance_path_manager::SubPathsManager::create ( ros::NodeHandle &  nodeHandle,
const PathType fullPath,
const bool &  checkTraversability,
const std::string &  traversabilityMapFrameId 
)
static

Factory function to create a SubPathsManager object from a full path that needs to be splitted in sub-paths, based on local guidance mode.

Parameters
[in]nodeHandleROS node handle.
[in]fullPathfull path that should be splitted in sub-paths, based on local guidance mode.
[in]checkTraversabilitytells if current sub-path should be checked for traversability.
[in]traversabilityMapFrameIdholds 'frame_id' of traversability map. Used in case is true.
Returns
pointer to created object in cas eof success, nullptr otherwise.

◆ createTraversabilityManager()

TraversabilityManagerPtr local_guidance_path_manager::SubPathsManager::createTraversabilityManager ( const PathTypePtr path) const
private

Creates a TraversabilityManager used to check the given sub-path that needs to be followed.

Parameters
[in]pathinput path used to initialize the SubPathsManager object.
Returns
updated traversability manager in case of success, nullptr in case of error.

◆ extractPortionSubPathAheadRobot()

void local_guidance_path_manager::SubPathsManager::extractPortionSubPathAheadRobot ( )
private

Extracts a portion of the sub-path ahead of the current robot's pose and store it internally. See function

See also
isCurrentPortionSubPathAheadRobotTraversable and
getPortionSubPathAheadRobot.

◆ getCurrentSubPathInFrameId()

PathTypePtr local_guidance_path_manager::SubPathsManager::getCurrentSubPathInFrameId ( const std::string &  frame_id) const

Returns a copy of the current sub-path that should be processed (i.e. followed). The sub-path is converted in the specified "frame_id". This function checks for the latest TF transformation between the full path frame_id and "frame_id" before converting (if necessary) the sub-path to the latter frame. If all the sub-paths present in the manager are already followed, then it returns the last segment.

Parameters
[in]frame_idtarget frame_id to convert he sub-path to.
Returns
sub-path converted expressed in the specified "frame_id", nullptr in case of error.

◆ getCurrentSubPathInOutputFrame()

PathTypePtr local_guidance_path_manager::SubPathsManager::getCurrentSubPathInOutputFrame ( ) const

Returns a copy of the current sub-path that should be processed (i.e. followed). The sub-path is converted in the "desiredOutputFrameId" set during creation in function create(). This function checks for the latest TF transformation between the full path frame_id and "desiredOutputFrameId" before converting the sub-path to the latter frame. If all the sub-paths present in the manager are already followed, then it returns the last segment.

Returns
sub-path converted expressed in "desiredOutputFrameId", nullptr in case of error.

◆ getIndexCurrentSubPath()

int local_guidance_path_manager::SubPathsManager::getIndexCurrentSubPath ( ) const

Returns the index [0, total_number_of_sub_paths] of the current sub-path. An index equal to total_number_of_sub_paths indicates all paths were followed.

Returns
index of the current sub-path.

◆ getNumberRemainingSubPathsToFollow()

int local_guidance_path_manager::SubPathsManager::getNumberRemainingSubPathsToFollow ( ) const

Returns the number of sub-paths which should be still followed. Current sub-path is NOT considered as a remaining one to be followed.

Returns
number of sub-paths which should be still followed.

◆ getPathAheadRobot()

PathTypePtr local_guidance_path_manager::SubPathsManager::getPathAheadRobot ( const PathTypePtr path) const
private

Returns the path that is ahead the robot, i.e. that has to be check for traversability.

Returns
path ahead of the robot, from current robot's pose.

◆ getRobotPose()

PoseStampedType local_guidance_path_manager::SubPathsManager::getRobotPose ( ) const
private

Returns robot's pose.

Returns
robot's pose in traversabilityMapFrameId_.

◆ init()

bool local_guidance_path_manager::SubPathsManager::init ( const PathType fullPath)
private

Initializes object.

Parameters
fullPathpath to start with.
Returns
true in case the object was successfully initialized.

◆ isInputPathValid()

bool local_guidance_path_manager::SubPathsManager::isInputPathValid ( const PathType fullPath) const
private

Checks whether the input path that has to be used to create the SubPathsManager object is valid.

Parameters
[in]fullPathinput path used to initialize the SubPathsManager object.
Returns
true if input valid is considered valid.

◆ manageCurrentPathTraversability()

void local_guidance_path_manager::SubPathsManager::manageCurrentPathTraversability ( bool *  currentSubPathWasUpdated,
bool *  stopRobotNow 
)

Manages the traversability of the current sub-path, i.e. checks if it's traversable and if it's not then asks for a new traversable portion to reach end of current sub-path.

Parameters
[out]currentSubPathWasUpdatedtrue if current sub-path was updated with a new traversable portion.
[out]stopRobotNowtells whther the robot should be stopped while a traversable path is being computed.

◆ mergeTraversableSubPathInCurrentOne()

bool local_guidance_path_manager::SubPathsManager::mergeTraversableSubPathInCurrentOne ( )
private

Merges the latest retrieved traversable path (

See also
isNewTraversableSubPathReady) in the current sub-path.
Returns
true on success.

◆ notifyCompletionCurrentSubPath()

void local_guidance_path_manager::SubPathsManager::notifyCompletionCurrentSubPath ( )

Notifies that the current sub-path, which was being followed, is completed and we should switch to the next one, if available.

◆ portionSubPathAheadRobotToRosMsg()

navigation_msgs::PathLocalGuidance local_guidance_path_manager::SubPathsManager::portionSubPathAheadRobotToRosMsg ( ) const
private

Returns the latest portion of the sub-path ahead of the robot, extracted by calling

See also
extractPortionSubPathAheadRobot.
Returns
ROS message of the portion of the sub-path ahead of the robot.

◆ printGlobalPath()

void local_guidance_path_manager::SubPathsManager::printGlobalPath ( ) const

Prints on terminal the global path that was set during creation with function create().

◆ printGlobalPathInDesiredOutputFrameId()

void local_guidance_path_manager::SubPathsManager::printGlobalPathInDesiredOutputFrameId ( ) const

Prints on terminal the global path, converted in the frame 'desiredOutputFrameId' specifiend during creation in function create().

◆ printPath()

void local_guidance_path_manager::SubPathsManager::printPath ( const PathType path,
const std::string &  info 
) const
private

Prints on terminal the given path.

Parameters
pathpath to be printed on screen.
infoadditional info to be printed at the beginning.

◆ readPersistentParameters()

bool local_guidance_path_manager::SubPathsManager::readPersistentParameters ( )
private

Reads and verifies persistent (kept in class memory) ROS parameters.

Returns
true if successful.

◆ setLocalGuidanceModeUpToLookaheadPose()

bool local_guidance_path_manager::SubPathsManager::setLocalGuidanceModeUpToLookaheadPose ( const std::string &  requestedLocalGuidanceMode,
const double &  lookaheadDistanceAlongPath,
std::string *  message = nullptr 
)

Sets the specified local guidance mode from the closest pose of the path from current robot's pose, up to a pose along the path obtained travelling on the path from the closest pose with a specified distance. This function can modify and update the current sub-paths based on the requested mode and lookahead distance.

Parameters
requestedLocalGuidanceModenew local guidance mode to be used. Empty string means current local guidance mode in usage.
lookaheadDistanceAlongPathdistance to be travelled along the path to get last pose to set with specified local guidance mode.
messagemessage with report about the requested mode setting.
Returns
true upon success, false otherwise.

◆ splitPath()

bool local_guidance_path_manager::SubPathsManager::splitPath ( const PathType fullPath)
private

Splits path in sub-paths in case different local guidance modes are specified along the full path.

Parameters
[in]fullPathfull path to split.
Returns
true in case fullPath was successfully splitted in sub-paths.

Member Data Documentation

◆ allSubPathsAreFollowed_

bool local_guidance_path_manager::SubPathsManager::allSubPathsAreFollowed_ {false}
private

Tells if all the sub-paths are completed.

◆ checkTraversability_

bool local_guidance_path_manager::SubPathsManager::checkTraversability_ {false}
private

Tells whether traversability should be checked for the current sub-path.

◆ currentSubPath_

CurrentSubPath local_guidance_path_manager::SubPathsManager::currentSubPath_
private

Holds data about the current sub-path that is being followed.

◆ desiredOutputFrameId_

std::string local_guidance_path_manager::SubPathsManager::desiredOutputFrameId_ {"odom"}
private

Contains the frame_id which is used to convert the current sub-path into another frame.

◆ nodeHandle_

ros::NodeHandle& local_guidance_path_manager::SubPathsManager::nodeHandle_
private

Ros node handle.

◆ originalFullPath_

PathTypePtr local_guidance_path_manager::SubPathsManager::originalFullPath_ {nullptr}
private

Holds the original full path used to initialize this SubPathsManager object.

◆ robotFrameId_

std::string local_guidance_path_manager::SubPathsManager::robotFrameId_ {"base"}
private

Contains the frame_id of the robot.

◆ subPaths_

std::vector<PathType> local_guidance_path_manager::SubPathsManager::subPaths_
private

Holds sub-paths to follow, obtained by splitting global path based on specified local guidance mode. Sub-paths managed with FIFO policy: first sub-path in is followed first.

◆ tfListener_

tf::TransformListener local_guidance_path_manager::SubPathsManager::tfListener_
private

TF listener.

◆ traversabilityManager_

TraversabilityManagerPtr local_guidance_path_manager::SubPathsManager::traversabilityManager_ {nullptr}
private

Manages traversability of current sub-path.

◆ traversabilityMapFrameId_

std::string local_guidance_path_manager::SubPathsManager::traversabilityMapFrameId_ {"map"}
private

Contains the frame_id of the traversability map.


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