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

#include <SubPathsManager.hpp>

Classes

struct  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)
 

Static Public Member Functions

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

Private Member Functions

 SubPathsManager (ros::NodeHandle &nodeHandle, const bool &checkTraversability)
 
bool init (const PathType &fullPath)
 
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 ()
 

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...
 
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...
 
TraversabilityManagerPtr traversabilityManager_
 Manages traversability of current sub-path. More...
 
bool checkTraversability_ {false}
 Tells whether traversability should be checked for the current sub-path. More...
 

Member Typedef Documentation

Constructor & Destructor Documentation

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

Constructor

Parameters
[in]nodeHandleROS node handler.
[in]checkTraversabilitytells if current sub-path should be checked for traversability.

Member Function Documentation

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.
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.
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.
static SubPathsManagerPtr local_guidance_path_manager::SubPathsManager::create ( ros::NodeHandle &  nodeHandle,
const PathType fullPath,
const bool &  checkTraversability 
)
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.
Returns
pointer to created object in cas eof success, nullptr otherwise.
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]
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.
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.
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.
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.
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.
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.
PoseStampedType local_guidance_path_manager::SubPathsManager::getRobotPose ( ) const
private

Returns robot's pose.

Returns
robot's pose in traversabilityMapFrameId_.
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.
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.
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.
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.

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.
void local_guidance_path_manager::SubPathsManager::printGlobalPath ( ) const

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

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().

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.
bool local_guidance_path_manager::SubPathsManager::readPersistentParameters ( )
private

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

Returns
true if successful.
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

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

Tells if all the sub-paths are completed.

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

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

CurrentSubPath local_guidance_path_manager::SubPathsManager::currentSubPath_
private

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

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.

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

Ros node handle.

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

Contains the frame_id of the robot.

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.

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

TF listener.

TraversabilityManagerPtr local_guidance_path_manager::SubPathsManager::traversabilityManager_
private

Manages traversability of current sub-path.

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: