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

#include <SubPathsManager.hpp>

Classes

class  CurrentSubPath
 
class  ReplanPathInfo
 

Public Types

using SubPathsManagerPtr = std::unique_ptr< SubPathsManager >
 

Public Member Functions

PathTypePtr getCurrentSubPathInOutputFrame () const
 
PathTypePtr getCurrentSubPathInFrameId (const std::string &frame_id) const
 
std::string getFrameIdOriginalPath () const
 
int getIndexCurrentSubPath () const
 
int getNumberRemainingSubPathsToFollow () const
 
void notifyCompletionCurrentSubPath ()
 
void printGlobalPath () const
 
void printGlobalPathInDesiredOutputFrameId () const
 
bool areAllSubPathsFollowed () const
 
bool managePathReplanning (bool *replanningPathIsCompleted, bool *stopRobotNow)
 
bool setLocalGuidanceModeUpToLookaheadPose (const std::string &requestedLocalGuidanceMode, const double &lookaheadDistanceAlongPath, std::string *message=nullptr)
 
bool isReplanningRequestInProgress () const
 
bool setLookaheadDistanceReplanningGoal (const double &lookaheadDistanceForReplanningGoal)
 
bool setLocalGuidanceModeReplanning (const std::string &requestedLocalGuidanceMode)
 
void requestStopPlanningIfOngoing ()
 
void stopHereAndErasePath ()
 
std::string getLocalGuidanceModeCurrentSubPath () const
 
PathTypePtr getFullPath () const
 

Static Public Member Functions

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

Private Member Functions

 SubPathsManager (ros::NodeHandle &nodeHandle, 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 getRobotPoseInTraversabilityMapFrameId () const
 
PoseStampedType getRobotPoseInOriginalPathFrameId () const
 
PoseStampedType getRobotPoseInFrameId (const std::string &frameId) const
 
void startReplanRequest ()
 
bool mergeTraversableSubPathInGlobalPath ()
 
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...
 
PathSmartifyPtr pathSmartify_ {nullptr}
 Object to make a path "smarter", i.e. easier for the robot to follow. 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_ {"odom"}
 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...
 
ReplanPathInfo replanPathInfo_
 Holds data for the re-plan request. More...
 
bool parthSmartificationEnabled_ {true}
 Tells is the auto smartification of the path is enabled. More...
 

Member Typedef Documentation

◆ SubPathsManagerPtr

Constructor & Destructor Documentation

◆ SubPathsManager()

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

Constructor

Parameters
[in]nodeHandleROS node handler.
[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 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]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.

◆ 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.

◆ getFrameIdOriginalPath()

std::string local_guidance_path_manager::SubPathsManager::getFrameIdOriginalPath ( ) const

Gets the frame_id of the original path used to initialize the manager.

Returns
frame_id of the original path.

◆ getFullPath()

PathTypePtr local_guidance_path_manager::SubPathsManager::getFullPath ( ) const

Retunrs a copy of the full path hold by this manager.

Returns
copy of the full path.

◆ 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.

◆ getLocalGuidanceModeCurrentSubPath()

std::string local_guidance_path_manager::SubPathsManager::getLocalGuidanceModeCurrentSubPath ( ) const

Gets the local guidance mode of the current sub-path.

Returns
local guidance mode 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.

◆ getRobotPoseInFrameId()

PoseStampedType local_guidance_path_manager::SubPathsManager::getRobotPoseInFrameId ( const std::string &  frameId) const
private

Returns robot's pose in the specified frame id.

Returns
robot's pose expressed in frameId.

◆ getRobotPoseInOriginalPathFrameId()

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

Returns robot's pose in the frame id of the original global path.

Returns
robot's pose.

◆ getRobotPoseInTraversabilityMapFrameId()

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

Returns robot's pose in the frame id of the traversability map.

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.

◆ isReplanningRequestInProgress()

bool local_guidance_path_manager::SubPathsManager::isReplanningRequestInProgress ( ) const

Tells if a re-planning request is in progress at the moment.

Returns
true if a re-planning request is in progress.

◆ managePathReplanning()

bool local_guidance_path_manager::SubPathsManager::managePathReplanning ( bool *  replanningPathIsCompleted,
bool *  stopRobotNow 
)

Manages the re-planning of the current path.

Parameters
[out]re-planningPathIsCompletedtrue if re-planning is completed.
[out]stopRobotNowtells whether the robot should be stopped while a traversable path is being computed.
Returns
true if no error.

◆ mergeTraversableSubPathInGlobalPath()

bool local_guidance_path_manager::SubPathsManager::mergeTraversableSubPathInGlobalPath ( )
private

Merges the latest retrieved traversable path (

See also
isNewTraversableSubPathReady) in the global 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.

◆ 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
[in]pathpath to be printed on screen.
[in]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.

◆ requestStopPlanningIfOngoing()

void local_guidance_path_manager::SubPathsManager::requestStopPlanningIfOngoing ( )

Requests to stop the planning action , if it is on-going.

◆ setLocalGuidanceModeReplanning()

bool local_guidance_path_manager::SubPathsManager::setLocalGuidanceModeReplanning ( const std::string &  requestedLocalGuidanceMode)

Sets the local guidance mode that should be used to follow a path obtained via re-plan request.

Parameters
[in]requestedLocalGuidanceModenew local guidance mode to be used. Empty string means current local guidance mode in usage.
Returns
true if local guidance mode was set correctly

◆ 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.

◆ setLookaheadDistanceReplanningGoal()

bool local_guidance_path_manager::SubPathsManager::setLookaheadDistanceReplanningGoal ( const double &  lookaheadDistanceForReplanningGoal)

Sets the lookahead distance that should be used to compute a goal pose to re-plan a path from current robot's pose.

Parameters
[in]lookaheadDistanceForReplanningGoallookahead distance to place goal for re-planning. 0 means to use default value.
Returns
true if lookahead distance was set correctly.

◆ 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.

◆ startReplanRequest()

void local_guidance_path_manager::SubPathsManager::startReplanRequest ( )
private

Starts a re-plan request by extracting a pose on the path to be used as re-planning goal.

◆ stopHereAndErasePath()

void local_guidance_path_manager::SubPathsManager::stopHereAndErasePath ( )

Erases the path currently hold and places only one node in the current position of the robot, with the mode of the current sub-path.

Member Data Documentation

◆ allSubPathsAreFollowed_

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

Tells if all the sub-paths are completed.

◆ 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.

◆ parthSmartificationEnabled_

bool local_guidance_path_manager::SubPathsManager::parthSmartificationEnabled_ {true}
private

Tells is the auto smartification of the path is enabled.

◆ pathSmartify_

PathSmartifyPtr local_guidance_path_manager::SubPathsManager::pathSmartify_ {nullptr}
private

Object to make a path "smarter", i.e. easier for the robot to follow.

◆ replanPathInfo_

ReplanPathInfo local_guidance_path_manager::SubPathsManager::replanPathInfo_
private

Holds data for the re-plan request.

◆ 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_ {"odom"}
private

Contains the frame_id of the traversability map.


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