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

Member Enumeration Documentation

State of the traversability manager.

Enumerator
Idle 
WaitingForTraversablePath 
NewTraversablePathIsReady 
CheckingPath 
PathIsChecked 

Constructor & Destructor Documentation

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

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.
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.
void local_guidance_path_manager::TraversabilityManager::checkFootprintPathActionActiveCb ( )
private

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

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
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.
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.
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.
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.
State local_guidance_path_manager::TraversabilityManager::getState ( ) const

Returns the current state of the traversability manager.

Returns
current state.
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.
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.
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.
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.
void local_guidance_path_manager::TraversabilityManager::planPathActionActiveCb ( )
private

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

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

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

Returns
true if successful.
std::string local_guidance_path_manager::TraversabilityManager::stateToString ( ) const

Converts current state to std::string.

Returns
std::string of current state.
void local_guidance_path_manager::TraversabilityManager::subscribeToServers ( )
private

Subscribes to ROS service and action servers.

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

CheckFootprintPathActionClientPtr local_guidance_path_manager::TraversabilityManager::checkFootprintPathActionClient_
private

Action to check if a footprint path is traversable.

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

Name of the action to check footprint path.

PathTypePtr local_guidance_path_manager::TraversabilityManager::currentPath_
private

Current path to check.

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

Circular radius used for traversability if useFootprintCircularRadius_ is true.

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

Vertices of the footprint polygon in base frame.

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

Holds results from latest traversability check of footprint path.

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

Ros node handle.

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

frame_id of the robot

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

State of the manager.

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

frame_id of the traversability_map

TraversabilityPlannerActionClientPtr local_guidance_path_manager::TraversabilityManager::traversabilityPlannerActionClient_
private

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

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

Name of the action to get a new traversable path.

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

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

PathTypePtr local_guidance_path_manager::TraversabilityManager::traversablePathToReachPose_
private

Current path to reach goal pose.

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

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

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: