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

#include <LocalGuidancePathManager.hpp>

Inheritance diagram for local_guidance_path_manager::LocalGuidancePathManager:
Inheritance graph

Public Member Functions

 LocalGuidancePathManager ()=delete
 
 LocalGuidancePathManager (any_node::Node::NodeHandlePtr nodeHandle)
 
 ~LocalGuidancePathManager () override=default
 
bool init () override
 
void cleanup () override
 
- Public Member Functions inherited from any_node::Node
 Node ()=delete
 
 Node (NodeHandlePtr nh)
 
virtual ~Node ()=default
 
virtual void preCleanup ()
 
void shutdown ()
 
template<class T >
bool addWorker (const std::string &name, const double timestep, bool(T::*fp)(const any_worker::WorkerEvent &), T *obj, const int priority=0)
 
bool addWorker (const any_worker::WorkerOptions &options)
 
bool hasWorker (const std::string &name)
 
void cancelWorker (const std::string &name, const bool wait=true)
 
void stopAllWorkers ()
 
ros::NodeHandle & getNodeHandle () const
 
template<typename msg >
ros::Publisher advertise (const std::string &name, const std::string &defaultTopic, uint32_t queue_size, bool latch=false)
 
template<typename msg >
ThreadedPublisherPtr< msg > threadedAdvertise (const std::string &name, const std::string &defaultTopic, uint32_t queue_size, bool latch=false, unsigned int maxMessageBufferSize=10)
 
template<class M , class T >
ros::Subscriber subscribe (const std::string &name, const std::string &defaultTopic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr< M const > &), T *obj, const ros::TransportHints &transport_hints=ros::TransportHints())
 
template<class M , class T >
ThrottledSubscriberPtr< M, T > throttledSubscribe (double timeStep, const std::string &name, const std::string &defaultTopic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr< M const > &), T *obj, const ros::TransportHints &transport_hints=ros::TransportHints())
 
template<class T , class MReq , class MRes >
ros::ServiceServer advertiseService (const std::string &name, const std::string &defaultService, bool(T::*srv_func)(MReq &, MRes &), T *obj)
 
template<class MReq , class MRes >
ros::ServiceClient serviceClient (const std::string &name, const std::string &defaultService, const ros::M_string &header_values=ros::M_string())
 
template<class Service >
ros::ServiceClient serviceClient (const std::string &name, const std::string &defaultService, const ros::M_string &header_values=ros::M_string())
 
template<typename ParamT >
bool getParam (const std::string &key, ParamT &param_val)
 
template<typename ParamT >
ParamT param (const std::string &key, const ParamT &defaultValue)
 
template<typename ParamT >
void setParam (const std::string &key, const ParamT &param)
 

Private Types

using FollowPathActionClient = actionlib::SimpleActionClient< navigation_msgs::FollowPathLocalGuidanceAction >
 
using FollowPathActionClientPtr = std::shared_ptr< FollowPathActionClient >
 
using FollowPathActionResultConstPtr = navigation_msgs::FollowPathLocalGuidanceResultConstPtr
 
using FollowPathActionFeedbackConstPtr = navigation_msgs::FollowPathLocalGuidanceFeedbackConstPtr
 
using FollowPathActionServerType = actionlib::SimpleActionServer< navigation_msgs::FollowPathLocalGuidanceAction >
 
using FollowPathActionServerTypePtr = std::shared_ptr< FollowPathActionServerType >
 
using FollowPathActionServerResult = navigation_msgs::FollowPathLocalGuidanceResult
 
using FollowPathActionFeedbackType = navigation_msgs::FollowPathLocalGuidanceFeedback
 

Private Member Functions

bool readPersistentParameters ()
 
void advertiseServers ()
 
void advertiseTopics ()
 
void subscribeToServers ()
 
void followLocalPathActionDoneCb (const actionlib::SimpleClientGoalState &state, const FollowPathActionResultConstPtr &result)
 
void followLocalPathActionActiveCb ()
 
void feedbackFollowLocalPathCb (const FollowPathActionFeedbackConstPtr &feedback)
 
void followGlobalPathActionRequest (const navigation_msgs::FollowPathLocalGuidanceGoalConstPtr &followPathGoal)
 
bool publishCurrentLocalSubPathBasedOnTime ()
 
bool publishCurrentLocalSubPathNow ()
 
bool followPathActionPreemptRequested () const
 
bool traversabilityCheckerWorker (const any_worker::WorkerEvent &event)
 
void setupFollowingPath ()
 
void stopFollowingPath ()
 
bool setForcingZeroTwist (bool forceZeroTwist)
 

Private Attributes

FollowPathActionClientPtr followLocalPathActionClient_
 Used as action client to set a local path to the path follower, being action client. More...
 
std::string followLocalPathActionTopicName_
 Name of the action to follow sub path. More...
 
FollowPathActionServerTypePtr followGlobalPathActionServer_
 Used to advertise action to follow global path, being action server. More...
 
FollowPathActionFeedbackType followGlobalPathFeedback_
 Feedback (to be sent to client) of the follow global path action, being action server. More...
 
FollowPathActionServerResult followPathResult_
 Result (to be sent to client) of the follow global path action, being action server. More...
 
SubPathsManagerPtr subPathsManager_
 Manages data about global sub-paths to follow. More...
 
std::mutex subPathsManagerMutex_
 Mutex of subPathsManager_ object. More...
 
double timeStepConversionGlobalToLocalPath_ {10.0}
 Time interval used to trigger conversion global to local path. More...
 
actionlib::SimpleClientGoalState latestGoalStateFromFollower_ {actionlib::SimpleClientGoalState::PENDING}
 Holds the latest action client status reported from the path follower module. More...
 
double spinFrequencyActionControlLoop_ {10.0}
 Spin frequency main control loop contained in this node. More...
 
ros::Time lastTimeLocalSubPathWasPublished_ {0.0}
 Holds indication about last time a local sub-path was published to the follower. More...
 
bool checkTraversability_ {false}
 Tells whether traversability should be checked for the current sub-path. More...
 
bool publishSubPathCheckedForTraversability_ {false}
 Tells if the sub-path that is checked for traversability should be published as ROS message (to be visualized in rviz). More...
 
ros::Publisher subPathCheckedForTraversabilityPub_
 Publisher for sub-path currently checked for traversability. More...
 
bool publishLocalSubPath_ {false}
 Tells if the current local sub-path should be published as ROS message (to be visualized in rviz). More...
 
ros::Publisher localSubPathPub_
 Publisher of current local sub-path. More...
 
const std::string traversabilityCheckerWorkerName_ {"traversability_checker_worker"}
 
ros::ServiceClient setForcingZeroTwistClient_
 Server client to force path follower to publish zero velocity twist. This is a sort of soft emergency stop to block the robot. More...
 
bool forceZeroTwistInPathFollower_ {false}
 Indicates if the path follower was forced to publish zero twist. More...
 

Additional Inherited Members

- Public Types inherited from any_node::Node
using NodeHandlePtr = std::shared_ptr< ros::NodeHandle >
 
- Protected Attributes inherited from any_node::Node
NodeHandlePtr nh_
 

Member Typedef Documentation

Constructor & Destructor Documentation

local_guidance_path_manager::LocalGuidancePathManager::LocalGuidancePathManager ( )
delete
local_guidance_path_manager::LocalGuidancePathManager::LocalGuidancePathManager ( any_node::Node::NodeHandlePtr  nodeHandle)
explicit
local_guidance_path_manager::LocalGuidancePathManager::~LocalGuidancePathManager ( )
overridedefault

Member Function Documentation

void local_guidance_path_manager::LocalGuidancePathManager::advertiseServers ( )
private

Advertises ROS service and action servers.

void local_guidance_path_manager::LocalGuidancePathManager::advertiseTopics ( )
private

Advertises ROS topics.

void local_guidance_path_manager::LocalGuidancePathManager::cleanup ( )
overridevirtual

Cleanup function, called by Nodewrap after stopping workers. This function is called even if init() returned false.

Implements any_node::Node.

void local_guidance_path_manager::LocalGuidancePathManager::feedbackFollowLocalPathCb ( const FollowPathActionFeedbackConstPtr feedback)
private

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

Parameters
feedbackfeedback from the action follow local path, received from the path follower.
void local_guidance_path_manager::LocalGuidancePathManager::followGlobalPathActionRequest ( const navigation_msgs::FollowPathLocalGuidanceGoalConstPtr followPathGoal)
private

Receives the action request to follow a global path and sets up and updates a local path to follow.

Parameters
followPathGoalgoal of the action, i.e. a global path to be followed. It can contain sub-paths with different local guidance modes.
void local_guidance_path_manager::LocalGuidancePathManager::followLocalPathActionActiveCb ( )
private

Called once when the goal of the follow local path action becomes active.

void local_guidance_path_manager::LocalGuidancePathManager::followLocalPathActionDoneCb ( const actionlib::SimpleClientGoalState &  state,
const FollowPathActionResultConstPtr result 
)
private

Called once when the goal of the follow local path action is completed.

Parameters
state
result
bool local_guidance_path_manager::LocalGuidancePathManager::followPathActionPreemptRequested ( ) const
private

Checks if followPathAction preempt has been requested.

Returns
true if preempt has been requested.
bool local_guidance_path_manager::LocalGuidancePathManager::init ( )
overridevirtual

Init function, used to initialize all members and starting workers (if any).

Returns
True if successful. Returning false indicates that the node shall shut down.

Implements any_node::Node.

bool local_guidance_path_manager::LocalGuidancePathManager::publishCurrentLocalSubPathBasedOnTime ( )
private

Sends an updated version of the current local sub path to the follower. WARNING: Actual publishing of the path is throttled within the function: path is actually published only if a certain amount of time (see timeStepConversionGlobalToLocalPath_) is elapsed since last publishing.

Returns
true if new local sub path was published during the function call.
bool local_guidance_path_manager::LocalGuidancePathManager::publishCurrentLocalSubPathNow ( )
private

Sends an updated version of the current local sub path to the follower.

Returns
true if new local sub path was successfully set.
bool local_guidance_path_manager::LocalGuidancePathManager::readPersistentParameters ( )
private

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

Returns
true if successful.
bool local_guidance_path_manager::LocalGuidancePathManager::setForcingZeroTwist ( bool  forceZeroTwist)
private

Calls service to enable/disable forcing zero twist publishing.

void local_guidance_path_manager::LocalGuidancePathManager::setupFollowingPath ( )
private

Executes required actions to be ready to follow a path.

void local_guidance_path_manager::LocalGuidancePathManager::stopFollowingPath ( )
private

Executes required actions to stop to follow a path.

void local_guidance_path_manager::LocalGuidancePathManager::subscribeToServers ( )
private

Subscribes to ROS service and action servers.

bool local_guidance_path_manager::LocalGuidancePathManager::traversabilityCheckerWorker ( const any_worker::WorkerEvent event)
private

Checks, at a certain frequency, the traversability of the current sub-path. Worker is created only if path_traversability is enabled.

Parameters
event
Returns

Member Data Documentation

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

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

FollowPathActionServerTypePtr local_guidance_path_manager::LocalGuidancePathManager::followGlobalPathActionServer_
private

Used to advertise action to follow global path, being action server.

FollowPathActionFeedbackType local_guidance_path_manager::LocalGuidancePathManager::followGlobalPathFeedback_
private

Feedback (to be sent to client) of the follow global path action, being action server.

FollowPathActionClientPtr local_guidance_path_manager::LocalGuidancePathManager::followLocalPathActionClient_
private

Used as action client to set a local path to the path follower, being action client.

std::string local_guidance_path_manager::LocalGuidancePathManager::followLocalPathActionTopicName_
private

Name of the action to follow sub path.

FollowPathActionServerResult local_guidance_path_manager::LocalGuidancePathManager::followPathResult_
private

Result (to be sent to client) of the follow global path action, being action server.

bool local_guidance_path_manager::LocalGuidancePathManager::forceZeroTwistInPathFollower_ {false}
private

Indicates if the path follower was forced to publish zero twist.

ros::Time local_guidance_path_manager::LocalGuidancePathManager::lastTimeLocalSubPathWasPublished_ {0.0}
private

Holds indication about last time a local sub-path was published to the follower.

actionlib::SimpleClientGoalState local_guidance_path_manager::LocalGuidancePathManager::latestGoalStateFromFollower_ {actionlib::SimpleClientGoalState::PENDING}
private

Holds the latest action client status reported from the path follower module.

ros::Publisher local_guidance_path_manager::LocalGuidancePathManager::localSubPathPub_
private

Publisher of current local sub-path.

bool local_guidance_path_manager::LocalGuidancePathManager::publishLocalSubPath_ {false}
private

Tells if the current local sub-path should be published as ROS message (to be visualized in rviz).

bool local_guidance_path_manager::LocalGuidancePathManager::publishSubPathCheckedForTraversability_ {false}
private

Tells if the sub-path that is checked for traversability should be published as ROS message (to be visualized in rviz).

ros::ServiceClient local_guidance_path_manager::LocalGuidancePathManager::setForcingZeroTwistClient_
private

Server client to force path follower to publish zero velocity twist. This is a sort of soft emergency stop to block the robot.

double local_guidance_path_manager::LocalGuidancePathManager::spinFrequencyActionControlLoop_ {10.0}
private

Spin frequency main control loop contained in this node.

ros::Publisher local_guidance_path_manager::LocalGuidancePathManager::subPathCheckedForTraversabilityPub_
private

Publisher for sub-path currently checked for traversability.

SubPathsManagerPtr local_guidance_path_manager::LocalGuidancePathManager::subPathsManager_
private

Manages data about global sub-paths to follow.

std::mutex local_guidance_path_manager::LocalGuidancePathManager::subPathsManagerMutex_
mutableprivate

Mutex of subPathsManager_ object.

double local_guidance_path_manager::LocalGuidancePathManager::timeStepConversionGlobalToLocalPath_ {10.0}
private

Time interval used to trigger conversion global to local path.

const std::string local_guidance_path_manager::LocalGuidancePathManager::traversabilityCheckerWorkerName_ {"traversability_checker_worker"}
private

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