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

#include <TraversabilityManager.hpp>

Classes

struct  TraversabilityResult
 Holds results indicating traversability of a path. More...
 

Public Types

enum  State {
  State::Idle, State::WaitingForTraversablePath, State::NewTraversablePathIsReady, State::CheckingPath,
  State::PathIsChecked
}
 State of the traversability manager. More...
 
using TraversabilityManagerPtr = std::unique_ptr< TraversabilityManager >
 

Public Member Functions

bool updatePath (const PathTypePtr &path)
 
bool isPathTraversable ()
 
bool askIfPathIsTraversable ()
 
bool askTraversablePathToReachEndPose ()
 
PathTypePtr getTraversablePath ()
 
bool getTraversabilityRequestResults (bool *pathIsSafeToTraverse, double *traversability, double *footprintPathArea)
 
State getState () const
 
std::string stateToString () const
 
traversability_msgs::FootprintPath currentPathToFootPrintPathMsg () const
 

Static Public Member Functions

static TraversabilityManagerPtr create (ros::NodeHandle &nodeHandle, const PathTypePtr &path, const std::string &traversabilityMapFrameId, const std::string &robotFrameId)
 

Private Types

using TraversabilityPlannerActionClient = actionlib::SimpleActionClient< navigation_msgs::PlanPathAction >
 
using TraversabilityPlannerActionClientPtr = std::shared_ptr< TraversabilityPlannerActionClient >
 
using TraversabilityPlannerActionResultConstPtr = navigation_msgs::PlanPathResultConstPtr
 
using TraversabilityPlannerActionFeedbackConstPtr = navigation_msgs::PlanPathFeedbackConstPtr
 
using CheckFootprintPathActionClient = actionlib::SimpleActionClient< navigation_msgs::CheckFootprintPathAction >
 
using CheckFootprintPathActionClientPtr = std::shared_ptr< CheckFootprintPathActionClient >
 
using CheckFootprintPathActionResultConstPtr = navigation_msgs::CheckFootprintPathResultConstPtr
 
using CheckFootprintPathActionFeedbackConstPtr = navigation_msgs::CheckFootprintPathFeedbackConstPtr
 

Private Member Functions

 TraversabilityManager (ros::NodeHandle &nodeHandle, std::string traversabilityMapFrameId, std::string robotFrameId)
 
bool init (const PathTypePtr &path)
 
bool readPersistentParameters ()
 
void subscribeToServers ()
 
void planPathActionDoneCb (const actionlib::SimpleClientGoalState &state, const TraversabilityPlannerActionResultConstPtr &result)
 
void planPathActionActiveCb ()
 
void feedbackPlanPathCb (const TraversabilityPlannerActionFeedbackConstPtr &feedback)
 
void checkFootprintPathActionDoneCb (const actionlib::SimpleClientGoalState &state, const CheckFootprintPathActionResultConstPtr &result)
 
void checkFootprintPathActionActiveCb ()
 
void feedbackCheckFootprintPathCb (const CheckFootprintPathActionFeedbackConstPtr &feedback)
 

Private Attributes

ros::NodeHandle & nodeHandle_
 Ros node handle. More...
 
PathTypePtr currentPath_
 Current path to check. More...
 
PathTypePtr traversablePathToReachPose_
 Current path to reach goal pose. More...
 
ros::ServiceClient traversabilityPlannerCheckPathClient_
 Server client to check traversability of a (sub)path. More...
 
const std::string traversabilityMapFrameId_ {"map"}
 frame_id of the traversability_map More...
 
const std::string robotFrameId_ {"base"}
 frame_id of the robot More...
 
bool useFootprintCircularRadius_ {true}
 Tells if circular radius should be used to check traversability. More...
 
double footprintCircularRadius_ {0.55}
 Circular radius used for traversability if useFootprintCircularRadius_ is true. More...
 
geometry_msgs::PolygonStamped footprintPolygonPoints_
 Vertices of the footprint polygon in base frame. More...
 
bool useConservativePolygonFootprint_ {false}
 Tells whether conservative footprint should be used. Only available if polygon is used. More...
 
TraversabilityPlannerActionClientPtr traversabilityPlannerActionClient_
 Action client to get a new traversable path, being action client. More...
 
std::string traversabilityPlannerActionTopicName_
 Name of the action to get a new traversable path. More...
 
CheckFootprintPathActionClientPtr checkFootprintPathActionClient_
 Action to check if a footprint path is traversable. More...
 
std::string checkFootprintPathActionTopicName_
 Name of the action to check footprint path. More...
 
State state_ {State::Idle}
 State of the manager. More...
 
TraversabilityResult latestTraversabilityResult_ {false, 0, 0}
 Holds results from latest traversability check of footprint path. More...
 

Member Typedef Documentation

◆ CheckFootprintPathActionClient

◆ CheckFootprintPathActionClientPtr

◆ CheckFootprintPathActionFeedbackConstPtr

◆ CheckFootprintPathActionResultConstPtr

◆ TraversabilityManagerPtr

◆ TraversabilityPlannerActionClient

◆ TraversabilityPlannerActionClientPtr

◆ TraversabilityPlannerActionFeedbackConstPtr

◆ TraversabilityPlannerActionResultConstPtr

Member Enumeration Documentation

◆ State

State of the traversability manager.

Enumerator
Idle 
WaitingForTraversablePath 
NewTraversablePathIsReady 
CheckingPath 
PathIsChecked 

Constructor & Destructor Documentation

◆ TraversabilityManager()

local_guidance_path_manager::TraversabilityManager::TraversabilityManager ( ros::NodeHandle &  nodeHandle,
std::string  traversabilityMapFrameId,
std::string  robotFrameId 
)
private

Constructor.

Parameters
nodeHandleROS node handle.
traversabilityMapFrameIdframe_id of the traversability map.
robotFrameIdframe_id of the robot, used to ask TF where it is.

Member Function Documentation

◆ askIfPathIsTraversable()

bool local_guidance_path_manager::TraversabilityManager::askIfPathIsTraversable ( )

Starts action to check if stored path is traversable. This is a NON-blocking function that asks for traversability. To get the state of the manager use function

See also
getState.
Returns
true if the request was sent successfully.

◆ askTraversablePathToReachEndPose()

bool local_guidance_path_manager::TraversabilityManager::askTraversablePathToReachEndPose ( )

Starts action to ask a new traversable path from start pose to end pose of the stored path. This is a NON-blocking function. To get the state of the manager use function

See also
getState.
Returns
true if the request was sent successfully.

◆ checkFootprintPathActionActiveCb()

void local_guidance_path_manager::TraversabilityManager::checkFootprintPathActionActiveCb ( )
private

Called once when the goal of the check footprint path action becomes active.

◆ checkFootprintPathActionDoneCb()

void local_guidance_path_manager::TraversabilityManager::checkFootprintPathActionDoneCb ( const actionlib::SimpleClientGoalState &  state,
const CheckFootprintPathActionResultConstPtr result 
)
private

Called once when the goal of the check footprint path action is completed.

Parameters
state
result

◆ create()

static TraversabilityManagerPtr local_guidance_path_manager::TraversabilityManager::create ( ros::NodeHandle &  nodeHandle,
const PathTypePtr path,
const std::string &  traversabilityMapFrameId,
const std::string &  robotFrameId 
)
static

Factory function to create a TraversabilityManager object to ask for traversability of the given path.

Parameters
[in]nodeHandleROS node handle.
[in]pathpath against which check traversability.
[in]traversabilityMapFrameIdframe_id of the expected traversability map.
[in]robotFrameIdframe_id of the robot that is following the given path.
Returns
pointer to created object in cas eof success, nullptr otherwise.

◆ currentPathToFootPrintPathMsg()

traversability_msgs::FootprintPath local_guidance_path_manager::TraversabilityManager::currentPathToFootPrintPathMsg ( ) const

Converts current path to check, to ROS message traversability_msgs::FootprintPath.

Returns
current path expressed as traversability_msgs::FootprintPath.

◆ feedbackCheckFootprintPathCb()

void local_guidance_path_manager::TraversabilityManager::feedbackCheckFootprintPathCb ( const CheckFootprintPathActionFeedbackConstPtr feedback)
private

Called every time feedback from check footprint path action is received for the set goal.

Parameters
feedbackfeedback from the action check footprint path, received from the traversability planner.

◆ feedbackPlanPathCb()

void local_guidance_path_manager::TraversabilityManager::feedbackPlanPathCb ( const TraversabilityPlannerActionFeedbackConstPtr feedback)
private

Called every time feedback from plan path action is received for the set goal.

Parameters
feedbackfeedback from the action plan path, received from the traversability planner.

◆ getState()

State local_guidance_path_manager::TraversabilityManager::getState ( ) const

Returns the current state of the traversability manager.

Returns
current state.

◆ getTraversabilityRequestResults()

bool local_guidance_path_manager::TraversabilityManager::getTraversabilityRequestResults ( bool *  pathIsSafeToTraverse,
double *  traversability,
double *  footprintPathArea 
)

Returns the result to traversability request sent using function

See also
askIfPathIsTraversable.
Parameters
pathIsSafeToTraversetrue if path is safe to traverse.
traversabilityranges from 0 to 1 where 0 means not traversable and 1 highly traversable.
footprintPathAreaarea of the footprint path.
Returns
true if results were retrieved correctly.

◆ getTraversablePath()

PathTypePtr local_guidance_path_manager::TraversabilityManager::getTraversablePath ( )

Returns a traversable path that can be used to reach desired pose, if it's available. To get the state of the manager use function

See also
getState. To ask for a new traversable path
askIfPathIsTraversable.
Returns
traversable path to reach desired pose.

◆ init()

bool local_guidance_path_manager::TraversabilityManager::init ( const PathTypePtr path)
private

Initializes object.

Parameters
pathpath against which check traversability.
Returns
true in case the object was successfully initialized.

◆ isPathTraversable()

bool local_guidance_path_manager::TraversabilityManager::isPathTraversable ( )

Checks if stored path is traversable. This is a blocking function that asks for traversability.

Returns
true if path is traversable.

◆ planPathActionActiveCb()

void local_guidance_path_manager::TraversabilityManager::planPathActionActiveCb ( )
private

Called once when the goal of the plan path action becomes active.

◆ planPathActionDoneCb()

void local_guidance_path_manager::TraversabilityManager::planPathActionDoneCb ( const actionlib::SimpleClientGoalState &  state,
const TraversabilityPlannerActionResultConstPtr result 
)
private

Called once when the goal of the plan path action is completed.

Parameters
state
result

◆ readPersistentParameters()

bool local_guidance_path_manager::TraversabilityManager::readPersistentParameters ( )
private

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

Returns
true if successful.

◆ stateToString()

std::string local_guidance_path_manager::TraversabilityManager::stateToString ( ) const

Converts current state to std::string.

Returns
std::string of current state.

◆ subscribeToServers()

void local_guidance_path_manager::TraversabilityManager::subscribeToServers ( )
private

Subscribes to ROS service and action servers.

◆ updatePath()

bool local_guidance_path_manager::TraversabilityManager::updatePath ( const PathTypePtr path)

Updates the path hold by the Traversability Manager.

Parameters
[in]pathnew path against which check traversability.
Returns
true if path was update correctly.

Member Data Documentation

◆ checkFootprintPathActionClient_

CheckFootprintPathActionClientPtr local_guidance_path_manager::TraversabilityManager::checkFootprintPathActionClient_
private

Action to check if a footprint path is traversable.

◆ checkFootprintPathActionTopicName_

std::string local_guidance_path_manager::TraversabilityManager::checkFootprintPathActionTopicName_
private

Name of the action to check footprint path.

◆ currentPath_

PathTypePtr local_guidance_path_manager::TraversabilityManager::currentPath_
private

Current path to check.

◆ footprintCircularRadius_

double local_guidance_path_manager::TraversabilityManager::footprintCircularRadius_ {0.55}
private

Circular radius used for traversability if useFootprintCircularRadius_ is true.

◆ footprintPolygonPoints_

geometry_msgs::PolygonStamped local_guidance_path_manager::TraversabilityManager::footprintPolygonPoints_
private

Vertices of the footprint polygon in base frame.

◆ latestTraversabilityResult_

TraversabilityResult local_guidance_path_manager::TraversabilityManager::latestTraversabilityResult_ {false, 0, 0}
private

Holds results from latest traversability check of footprint path.

◆ nodeHandle_

ros::NodeHandle& local_guidance_path_manager::TraversabilityManager::nodeHandle_
private

Ros node handle.

◆ robotFrameId_

const std::string local_guidance_path_manager::TraversabilityManager::robotFrameId_ {"base"}
private

frame_id of the robot

◆ state_

State local_guidance_path_manager::TraversabilityManager::state_ {State::Idle}
private

State of the manager.

◆ traversabilityMapFrameId_

const std::string local_guidance_path_manager::TraversabilityManager::traversabilityMapFrameId_ {"map"}
private

frame_id of the traversability_map

◆ traversabilityPlannerActionClient_

TraversabilityPlannerActionClientPtr local_guidance_path_manager::TraversabilityManager::traversabilityPlannerActionClient_
private

Action client to get a new traversable path, being action client.

◆ traversabilityPlannerActionTopicName_

std::string local_guidance_path_manager::TraversabilityManager::traversabilityPlannerActionTopicName_
private

Name of the action to get a new traversable path.

◆ traversabilityPlannerCheckPathClient_

ros::ServiceClient local_guidance_path_manager::TraversabilityManager::traversabilityPlannerCheckPathClient_
private

Server client to check traversability of a (sub)path.

◆ traversablePathToReachPose_

PathTypePtr local_guidance_path_manager::TraversabilityManager::traversablePathToReachPose_
private

Current path to reach goal pose.

◆ useConservativePolygonFootprint_

bool local_guidance_path_manager::TraversabilityManager::useConservativePolygonFootprint_ {false}
private

Tells whether conservative footprint should be used. Only available if polygon is used.

◆ useFootprintCircularRadius_

bool local_guidance_path_manager::TraversabilityManager::useFootprintCircularRadius_ {true}
private

Tells if circular radius should be used to check traversability.


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