LocalGuidance
A library that generates velocity references to follow a path.
local_guidance_path_follower::LocalGuidancePathFollower Class Reference

#include <LocalGuidancePathFollower.hpp>

Inheritance diagram for local_guidance_path_follower::LocalGuidancePathFollower:
Inheritance graph

Public Member Functions

 LocalGuidancePathFollower ()=delete
 
 LocalGuidancePathFollower (any_node::Node::NodeHandlePtr nodeHandle)
 
 ~LocalGuidancePathFollower () 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 FollowPathActionServerType = actionlib::SimpleActionServer< navigation_msgs::FollowPathLocalGuidanceAction >
 
using FollowPathActionServerTypePtr = std::shared_ptr< FollowPathActionServerType >
 
using FollowPathActionFeedbackType = navigation_msgs::FollowPathLocalGuidanceFeedback
 

Private Member Functions

bool readPersistentParameters ()
 
bool createLocalGuidanceController (const std::string &localGuidanceControllerName, const LocalGuidanceControllerTypes &localGuidanceControllerType)
 
bool createNopController (const std::string &localGuidanceControllerName, const std::string &pathYamlFileDefaultParams, const std::string &pathYamlFileOverlyingParams)
 
bool createPurePursuitController (const std::string &localGuidanceControllerName, const std::string &pathYamlFileDefaultParams, const std::string &pathYamlFileOverlyingParams)
 
bool createDefaultController (const std::string &localGuidanceControllerName, const std::string &pathYamlFileDefaultParams, const std::string &pathYamlFileOverlyingParams)
 
void advertiseTopics ()
 
void advertiseServers ()
 
void subscribeToTopics ()
 
void subscribeToServers ()
 
void initLocomotionControllerInterface ()
 
void followPathActionRequest (const navigation_msgs::FollowPathLocalGuidanceGoalConstPtr &pFollowPathGoal)
 
bool followPath (const FollowPathDataPtr &pFollowPathData)
 
bool followPathActionPreemptRequested () const
 
FollowPathDataPtr setupFollowPath (const navigation_msgs::FollowPathLocalGuidanceGoalConstPtr &pFollowPathGoal)
 
bool startLocomotionController (const local_guidance_loco_ctrl_manager::LocomotionController locomotionController) const
 
bool stopLocomotionController () const
 
void preemptFollowPathActionRequest (std::string message="", std::function< void(void)> fcnToExecute=nullptr)
 
void completeFollowSubPath (const FollowPathDataPtr &pFollowPathData)
 
void publishVelocityCommand (const local_guidance_control_interface::Twist &referenceTwist)
 
void filterAndPublishVelocityCommand (const local_guidance_control_interface::Twist &referenceTwist)
 
void resetVelocityCommand ()
 
bool goalSubPathHasBeenReached (const FollowPathDataPtr &pFollowPathData) const
 
void visualizeControllerReference (const FollowPathDataPtr &pFollowPathData, int32_t action=visualization_msgs::Marker::ADD)
 
void publishCompletedFollowPathFeedback (const FollowPathDataPtr &pFollowPathData)
 
void publishFollowPathFeedback (const FollowPathDataPtr &pFollowPathData)
 
void publishActionFeedback (uint16_t remainingNumberOfSegments, uint16_t initialNumberOfSegments, double remainingDistance, double initialDistance)
 
void shutdownPublishers ()
 
bool safetyTransitionWalkToStand ()
 
void saturateTwist (const local_guidance_control_interface::Twist &inTwist, local_guidance_control_interface::Twist *pOutTwist) const
 
void resetBeforeFollowing ()
 
template<typename T >
bool limitAcceleration (const double maxAcceleration, const double deltaTime, const T &velocityTimeKm1, const T &velocityTimeK, T *pLimitedVelocityTimeK) const
 
bool loadLocalGuidanceMode (const std::string &localGuidanceMode, local_guidance_loco_ctrl_manager::LocomotionController *pLocomotionController, LocalGuidanceControllerTypes *pLocalGuidanceControllerType, std::string *pLocalGuidanceControllerName)
 
bool setForcingZeroTwistPublishing (std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response)
 
void publishZeroVelocityCommand ()
 

Private Attributes

ros::Publisher twistCommandPub_
 Twist command publisher. More...
 
ros::Publisher controllerReferenceMarkerPub_
 Controller's reference marker publisher. More...
 
FollowPathActionServerTypePtr pFollowPathActionServer_
 Follow Path action server. More...
 
ros::ServiceServer setForcingZeroTwistSrv_
 Server to enable/disable forcing zero twist publishing. More...
 
navigation_msgs::FollowPathLocalGuidanceResult followPathResult_
 Result (to be sent to client) of the follow global path action, being action server. More...
 
bool simulation_ {false}
 Simulation mode. More...
 
std::string robotFrameId_
 Robot frame id. More...
 
double frequency_ {1.0}
 Action execution frequency. More...
 
std::set< std::string > locomotionControllerNames_
 Locomotion controller names. More...
 
std::unique_ptr< local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManagerpLocomotionControllerManager_
 Locomotion controller manager. More...
 
std::unique_ptr< local_guidance_control_interface::LocalGuidanceControlInterfacepLocalGuidanceController_
 Controller interface to generate velocity commands. More...
 
LocalGuidanceControllerTypes currentLocalGuidanceControllerType_ {LocalGuidanceControllerTypes::kUnknown}
 Current local guidance controller type. More...
 
visualization_msgs::Marker controllerReferenceMarker_
 Controller's reference marker. More...
 
double goalToleranceTranslation_ {kGoalToleranceTranslation}
 Goal tolerance of last goal to consider subpath completed. More...
 
double goalToleranceRotation_ {kGoalToleranceRotation}
 
geometry_msgs::TwistStamped lastPublishedTwistCommand_
 Previous twist given as reference to the robot. More...
 
double maxLinearAcceleration_ {1.0}
 Max linear acceleration. More...
 
double maxRotationalAcceleration_ {M_PI}
 Max rotational acceleration. More...
 
std::set< std::string > localGuidanceControllerNames_
 Names of local guidance controllers specified in modes file. More...
 
std::set< std::string > localGuidanceModeNames_
 Names of local guidance modes specified in modes file. More...
 
FollowPathActionFeedbackType followPathFeedback_
 Action data. More...
 
bool forceZeroTwistPublishing_ {false}
 Indicates if zero twist should be published, no matter of commands from follow path action. More...
 
std::vector< robot_utils::FilteredDoublefilteredVelocities_
 Filtered linear velocities (longitudinal, lateral, yaw rate). 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_follower::LocalGuidancePathFollower::LocalGuidancePathFollower ( )
delete
local_guidance_path_follower::LocalGuidancePathFollower::LocalGuidancePathFollower ( any_node::Node::NodeHandlePtr  nodeHandle)
explicit
local_guidance_path_follower::LocalGuidancePathFollower::~LocalGuidancePathFollower ( )
overridedefault

Member Function Documentation

void local_guidance_path_follower::LocalGuidancePathFollower::advertiseServers ( )
private

Advertises ROS service and action servers.

void local_guidance_path_follower::LocalGuidancePathFollower::advertiseTopics ( )
private

Advertises ROS topics.

void local_guidance_path_follower::LocalGuidancePathFollower::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_follower::LocalGuidancePathFollower::completeFollowSubPath ( const FollowPathDataPtr pFollowPathData)
private

Finishes the current following sub-path request.

Parameters
[in]pFollowPathDatadata of the path to follow.
bool local_guidance_path_follower::LocalGuidancePathFollower::createDefaultController ( const std::string &  localGuidanceControllerName,
const std::string &  pathYamlFileDefaultParams,
const std::string &  pathYamlFileOverlyingParams 
)
private

Creates a 'default' local guidance controller.

Parameters
[in]localGuidanceControllerNamename of the local guidance controller.
[in]pathYamlFileDefaultParamspath of the file containing default parameters.
[in]pathYamlFileOverlyingParamspath of the file containing parameters to overly on top default ones.
Returns
true if successful.
bool local_guidance_path_follower::LocalGuidancePathFollower::createLocalGuidanceController ( const std::string &  localGuidanceControllerName,
const LocalGuidanceControllerTypes localGuidanceControllerType 
)
private

Creates the desired local guidance controller.

Parameters
[in]localGuidanceControllerNamename of the local guidance controller.
[in]localGuidanceControllerTypetype of the local guidance controller to create.
Returns
true if successful.
bool local_guidance_path_follower::LocalGuidancePathFollower::createNopController ( const std::string &  localGuidanceControllerName,
const std::string &  pathYamlFileDefaultParams,
const std::string &  pathYamlFileOverlyingParams 
)
private

Creates a NOP (No OPeration) local guidance controller. Used as dummy controller to start following a sub-path and complete it immediately.

Parameters
[in]localGuidanceControllerNamename of the local guidance controller.
[in]pathYamlFileDefaultParamspath of the file containing default parameters.
[in]pathYamlFileOverlyingParamspath of the file containing parameters to overly on top default ones.
Returns
true if successful.
bool local_guidance_path_follower::LocalGuidancePathFollower::createPurePursuitController ( const std::string &  localGuidanceControllerName,
const std::string &  pathYamlFileDefaultParams,
const std::string &  pathYamlFileOverlyingParams 
)
private

Creates a Pure Pursuit local guidance controller.

Parameters
[in]localGuidanceControllerNamename of the local guidance controller.
[in]pathYamlFileDefaultParamspath of the file containing default parameters.
[in]pathYamlFileOverlyingParamspath of the file containing parameters to overly on top default ones.
Returns
true if successful.
void local_guidance_path_follower::LocalGuidancePathFollower::filterAndPublishVelocityCommand ( const local_guidance_control_interface::Twist referenceTwist)
private

Filters and publishes velocity reference for locomotion controller.

Parameters
[in]referenceTwisttwist (containing linear and angular velocity command) that will be filtered and published.
bool local_guidance_path_follower::LocalGuidancePathFollower::followPath ( const FollowPathDataPtr pFollowPathData)
private

Follows a path.

Parameters
[in,out]pFollowPathDatapath data ready to be used, containing path to follow.
Returns
true if path was followed correctly.
bool local_guidance_path_follower::LocalGuidancePathFollower::followPathActionPreemptRequested ( ) const
private

Checks if followPathAction preempt has been requested.

Returns
true if preempt has been requested.
void local_guidance_path_follower::LocalGuidancePathFollower::followPathActionRequest ( const navigation_msgs::FollowPathLocalGuidanceGoalConstPtr pFollowPathGoal)
private

Follow path action execution callback.

Parameters
[in]pFollowPathGoalgoal of the action.
bool local_guidance_path_follower::LocalGuidancePathFollower::goalSubPathHasBeenReached ( const FollowPathDataPtr pFollowPathData) const
private

Checks if goal of the sub-path has been reached.

Parameters
[in]pFollowPathDatadata of the path to follow.
Returns
true if goal has been reached.
bool local_guidance_path_follower::LocalGuidancePathFollower::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.

void local_guidance_path_follower::LocalGuidancePathFollower::initLocomotionControllerInterface ( )
private

Initializes the interfaces locomotion controller and low level controller.

template<typename T >
bool local_guidance_path_follower::LocalGuidancePathFollower::limitAcceleration ( const double  maxAcceleration,
const double  deltaTime,
const T &  velocityTimeKm1,
const T &  velocityTimeK,
T *  pLimitedVelocityTimeK 
) const
inlineprivate

Limits velocity based on max acceleration from previous velocity.

Template Parameters
Ttype of the velocity vector.
Parameters
[in]maxAccelerationmax acceleration allowed.
[in]deltaTimedelta time between current and previous velocity vector.
[in]velocityTimeKm1velocity vector at time K-1 (previous).
[in]velocityTimeKvelocity vector at time (current).
[out]pLimitedVelocityTimeKlimited velocity vector at time K (current time).
Returns
true if the velocity at time K was limited based on maxAcceleration
bool local_guidance_path_follower::LocalGuidancePathFollower::loadLocalGuidanceMode ( const std::string &  localGuidanceMode,
local_guidance_loco_ctrl_manager::LocomotionController pLocomotionController,
LocalGuidanceControllerTypes pLocalGuidanceControllerType,
std::string *  pLocalGuidanceControllerName 
)
private

Loads a specific local guidance mode from param file.

Parameters
[in]localGuidanceModemode to load from param file.
[out]pLocomotionControllerlocomotion controller to be used.
[out]pLocalGuidanceControllerTypelocal guidance controller to be used.
[out]pLocalGuidanceControllerNamelocal guidance controller name as specified in the parameter section.
Returns
true if output data very initialized correctly.
void local_guidance_path_follower::LocalGuidancePathFollower::preemptFollowPathActionRequest ( std::string  message = "",
std::function< void(void)>  fcnToExecute = nullptr 
)
private

Preemts the execution of a FollowPathAction, executes an exiting function and prints a message.

Parameters
[in]messagemessage to print in addition to the default one.
[in]fcnToExecutefunction to be called to handle the pre-emption.
void local_guidance_path_follower::LocalGuidancePathFollower::publishActionFeedback ( uint16_t  remainingNumberOfSegments,
uint16_t  initialNumberOfSegments,
double  remainingDistance,
double  initialDistance 
)
private

Publishes action feedback.

Parameters
remainingNumberOfSegments
initialNumberOfSegments
remainingDistance
initialDistance
void local_guidance_path_follower::LocalGuidancePathFollower::publishCompletedFollowPathFeedback ( const FollowPathDataPtr pFollowPathData)
private

Publishes a feedback when a following path request has been completed.

Parameters
[in]pFollowPathDatadata of the path to follow.
void local_guidance_path_follower::LocalGuidancePathFollower::publishFollowPathFeedback ( const FollowPathDataPtr pFollowPathData)
private

Publishes a feedback about the current follow path request.

Parameters
[in]pFollowPathDatadata of the path to follow.
void local_guidance_path_follower::LocalGuidancePathFollower::publishVelocityCommand ( const local_guidance_control_interface::Twist referenceTwist)
private

Publishes velocity reference for locomotion controller.

Parameters
[in]referenceTwisttwist containing linear and angular velocity command.
void local_guidance_path_follower::LocalGuidancePathFollower::publishZeroVelocityCommand ( )
private

Published zero velocity (twist) command. It forces not to check acceleration limits, i.e. zero twist is immediately published, not matter the value of the previous twist.

bool local_guidance_path_follower::LocalGuidancePathFollower::readPersistentParameters ( )
private

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

Returns
true if successful.
void local_guidance_path_follower::LocalGuidancePathFollower::resetBeforeFollowing ( )
private

Resets persistent variables before start following a new path.

void local_guidance_path_follower::LocalGuidancePathFollower::resetVelocityCommand ( )
private

Sets the velocity command to zero.

bool local_guidance_path_follower::LocalGuidancePathFollower::safetyTransitionWalkToStand ( )
private

Makes sure to have a safety transition from walking to standing mode: first publish 0 velocity command, wait a little bit and then switch to stand mode.

Returns
true if it was possible to stop locomotion controller by switching to stand mode.
void local_guidance_path_follower::LocalGuidancePathFollower::saturateTwist ( const local_guidance_control_interface::Twist inTwist,
local_guidance_control_interface::Twist pOutTwist 
) const
private

Saturates twist based on maximum acceleration parameter.

Parameters
[in]pTwistinTwist to saturate.
[out]pOutTwistsaturated twist.
bool local_guidance_path_follower::LocalGuidancePathFollower::setForcingZeroTwistPublishing ( std_srvs::SetBool::Request &  request,
std_srvs::SetBool::Response &  response 
)
private

Service server to enable/disable the forcing of zero twist publishing. This is a sort of soft emergency stop server, which forces to publish zero velocity twist.

Parameters
requestservice request.
responseservice response.
Returns
true if the service succeeded.
FollowPathDataPtr local_guidance_path_follower::LocalGuidancePathFollower::setupFollowPath ( const navigation_msgs::FollowPathLocalGuidanceGoalConstPtr pFollowPathGoal)
private

Initializes variables and gets ready to handle a follow path request. Starts locomotion controller.

Parameters
[in]pFollowPathGoaldata of the path to follow.
Returns
object holding follow path data, nullptr in case of error.
void local_guidance_path_follower::LocalGuidancePathFollower::shutdownPublishers ( )
private

Shutdowns publishers.

bool local_guidance_path_follower::LocalGuidancePathFollower::startLocomotionController ( const local_guidance_loco_ctrl_manager::LocomotionController  locomotionController) const
private

Starts locomotion controller and low level control if not in simulation.

Parameters
[in]locomotionControllerlocomotion controller to be used in this sub-path.
Returns
true on success.
bool local_guidance_path_follower::LocalGuidancePathFollower::stopLocomotionController ( ) const
private

Stops locomotion controller and low level control if not in simulation.

Returns
true on success.
void local_guidance_path_follower::LocalGuidancePathFollower::subscribeToServers ( )
private

Subscribes to ROS service and action servers.

void local_guidance_path_follower::LocalGuidancePathFollower::subscribeToTopics ( )
private

Subscribes to ROS topics.

void local_guidance_path_follower::LocalGuidancePathFollower::visualizeControllerReference ( const FollowPathDataPtr pFollowPathData,
int32_t  action = visualization_msgs::Marker::ADD 
)
private

Visualizes the reference point of the controller by publishing an Rviz marker.

Parameters
[in]pFollowPathDatadata of the path to follow.
[in]actionaction for the marker, see ROS message visualization_msgs/Marker.

Member Data Documentation

visualization_msgs::Marker local_guidance_path_follower::LocalGuidancePathFollower::controllerReferenceMarker_
private

Controller's reference marker.

ros::Publisher local_guidance_path_follower::LocalGuidancePathFollower::controllerReferenceMarkerPub_
private

Controller's reference marker publisher.

LocalGuidanceControllerTypes local_guidance_path_follower::LocalGuidancePathFollower::currentLocalGuidanceControllerType_ {LocalGuidanceControllerTypes::kUnknown}
private

Current local guidance controller type.

std::vector<robot_utils::FilteredDouble> local_guidance_path_follower::LocalGuidancePathFollower::filteredVelocities_
private

Filtered linear velocities (longitudinal, lateral, yaw rate).

FollowPathActionFeedbackType local_guidance_path_follower::LocalGuidancePathFollower::followPathFeedback_
private

Action data.

navigation_msgs::FollowPathLocalGuidanceResult local_guidance_path_follower::LocalGuidancePathFollower::followPathResult_
private

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

bool local_guidance_path_follower::LocalGuidancePathFollower::forceZeroTwistPublishing_ {false}
private

Indicates if zero twist should be published, no matter of commands from follow path action.

double local_guidance_path_follower::LocalGuidancePathFollower::frequency_ {1.0}
private

Action execution frequency.

double local_guidance_path_follower::LocalGuidancePathFollower::goalToleranceRotation_ {kGoalToleranceRotation}
private
double local_guidance_path_follower::LocalGuidancePathFollower::goalToleranceTranslation_ {kGoalToleranceTranslation}
private

Goal tolerance of last goal to consider subpath completed.

geometry_msgs::TwistStamped local_guidance_path_follower::LocalGuidancePathFollower::lastPublishedTwistCommand_
private

Previous twist given as reference to the robot.

std::set<std::string> local_guidance_path_follower::LocalGuidancePathFollower::localGuidanceControllerNames_
private

Names of local guidance controllers specified in modes file.

std::set<std::string> local_guidance_path_follower::LocalGuidancePathFollower::localGuidanceModeNames_
private

Names of local guidance modes specified in modes file.

std::set<std::string> local_guidance_path_follower::LocalGuidancePathFollower::locomotionControllerNames_
private

Locomotion controller names.

double local_guidance_path_follower::LocalGuidancePathFollower::maxLinearAcceleration_ {1.0}
private

Max linear acceleration.

double local_guidance_path_follower::LocalGuidancePathFollower::maxRotationalAcceleration_ {M_PI}
private

Max rotational acceleration.

FollowPathActionServerTypePtr local_guidance_path_follower::LocalGuidancePathFollower::pFollowPathActionServer_
private

Follow Path action server.

std::unique_ptr<local_guidance_control_interface::LocalGuidanceControlInterface> local_guidance_path_follower::LocalGuidancePathFollower::pLocalGuidanceController_
private

Controller interface to generate velocity commands.

std::unique_ptr<local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager> local_guidance_path_follower::LocalGuidancePathFollower::pLocomotionControllerManager_
private

Locomotion controller manager.

std::string local_guidance_path_follower::LocalGuidancePathFollower::robotFrameId_
private

Robot frame id.

ros::ServiceServer local_guidance_path_follower::LocalGuidancePathFollower::setForcingZeroTwistSrv_
private

Server to enable/disable forcing zero twist publishing.

bool local_guidance_path_follower::LocalGuidancePathFollower::simulation_ {false}
private

Simulation mode.

ros::Publisher local_guidance_path_follower::LocalGuidancePathFollower::twistCommandPub_
private

Twist command publisher.


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