LocalGuidance
A library that generates velocity references to follow a path.
local_guidance_pure_pursuit::LocalGuidancePurePursuit Class Reference

#include <LocalGuidancePurePursuit.hpp>

Inheritance diagram for local_guidance_pure_pursuit::LocalGuidancePurePursuit:
Inheritance graph

Classes

class  PreciseReachingControllerData
 
class  PurePursuitControllerData
 

Public Member Functions

 LocalGuidancePurePursuit (const std::string &localGuidanceControllerName)
 
 LocalGuidancePurePursuit ()=delete
 
 ~LocalGuidancePurePursuit () override=default
 
bool loadParametersFromFile (const std::string &pathYamlFileDefaultParams, const std::string &pathYamlFileOverlyingParams) override
 
bool setDesiredPath (const local_guidance_control_interface::PathConstPtr &pDesiredPath) override
 
bool updateCurrentPath (const local_guidance_control_interface::PathConstPtr &pDesiredPath) override
 
bool setRobotPose (const local_guidance_control_interface::PoseStampedConstPtr &pRobotPose) override
 
void computeControlAction (local_guidance_control_interface::Twist *pTwist) override
 
void reset () override
 
bool getControllerReference (local_guidance_control_interface::PoseStamped *pCurrentReference) const override
 
void getFollowingFeedback (uint16_t *remainingNumberOfSegments, uint16_t *initialNumberOfSegments, double *remainingDistance, double *initialDistance) const override
 
- Public Member Functions inherited from local_guidance_control_interface::LocalGuidanceControlInterface
 LocalGuidanceControlInterface ()=delete
 
 LocalGuidanceControlInterface (const std::string &localGuidanceControllerName)
 
virtual ~LocalGuidanceControlInterface ()=default
 
std::string getLocalGuidanceControllerName () const
 

Protected Member Functions

void computePurePursuitControlAction (local_guidance_control_interface::Twist *pTwist)
 
void computePreciseReachingManeuverControlAction (local_guidance_control_interface::Twist *pTwist)
 
void getLookaheadPoint ()
 
void getHeadingDifferenceControlFrameToLookaheadPoint ()
 
template<enum navigation_common::DimensionType Dim>
double getDistanceFromPoses (const local_guidance_control_interface::PoseStamped &pose1, const local_guidance_control_interface::PoseStamped &pose2) const
 
template<enum navigation_common::DimensionType Dim>
double getDistanceFromLastGoalOfPath () const
 
double saturate (double valueIn, double absMaxValue) const
 
double getYawDifferenceBetweenPoses (const local_guidance_control_interface::PoseStamped &pose1, const local_guidance_control_interface::PoseStamped &pose2) const
 
void getPositionErrorInControlFrame (const kindr::VectorTypeless3D &positionInput_World, kindr::VectorTypeless3D *errorVector_ControlFrame) const
 
double getYawDifferenceInRobotControlFrame (const kindr::VectorTypeless3D &positionInput_World) const
 
kindr::VectorTypeless3D transformFromWorldToCurrentControlFrame (const kindr::VectorTypeless3D &vector) const
 
local_guidance_control_interface::PoseStamped transformFromWorldToCurrentControlFrame (const local_guidance_control_interface::PoseStamped &pose) const
 
void fillTwistCommandControlFrameToBodyFrame (double linearVelocityX, double linearVelocityY, double angularVelocity, local_guidance_control_interface::Twist *pTwist)
 
bool loadPurePursuitParametersFromFile (const std::string &pathYamlFileDefaultParams, const std::string &pathYamlFileOverlyingParams, const std::string &namePurePursuitController, int maximumNumberOfNestedSearch=10)
 
bool loadPurePursuitParametersInMemory (const std::string &pathYamlFileDefaultParams, const std::string &pathYamlFileOverlyingParams, const std::string &namePurePursuitController)
 
bool getPurePursuitParameters (bool verbose=true)
 
- Protected Member Functions inherited from local_guidance_control_interface::LocalGuidanceControlInterface
bool loadParametersSectionFromFile (const std::string &pathYamlFileDefaultParams="", const std::string &pathYamlFileOverlyingParams="", const std::string &sectionName="")
 
virtual void printLoadedParameters () const
 
template<typename T >
bool getParameter (const std::string &key, T *pParameter, bool verbose=true) const
 
template<typename T >
parameter (const std::string &key) const
 

Protected Attributes

PurePursuitControllerData purePursuitData_
 Data for Pure Pursuit Controller. More...
 
PreciseReachingControllerData preciseReachingData_
 Data for controller to precisely reach last goal if the robot is close enough. More...
 
local_guidance_control_interface::PathConstPtr pDesiredPath_
 Desired path the robot should follow. More...
 
bool pathReceivedOnce_
 
local_guidance_control_interface::PoseStampedPtr pCurrentPose2d_
 Current pose of the robot. More...
 
local_guidance_control_interface::PoseStampedPtr pLastPose2d_
 Last pose of the robot. More...
 
local_guidance_control_interface::PoseStampedPtr pClosestPoseOnPath_
 Closest pose fon path from the robot. More...
 
unsigned int idSegmentClosestPoseOnPath_
 Segment id on the path where the closest pose from the robot lays on. More...
 
local_guidance_control_interface::PoseStampedPtr pLastGoalOnPath_
 Last goal on the path. More...
 
local_guidance_control_interface::PoseStampedConstPtr pCurrentControllerGoal_
 Current goal of the controller. More...
 
local_guidance_control_interface::PoseStampedPtr pInitialPose2d_
 Initial pose of the robot. More...
 
bool initialPoseReceivedOnce_
 
double initialDistanceRobotLastGoal_
 Initial distance of the robot from last goal of the path. More...
 
bool initialDistanceRobotLastGoalComputedOnce_
 
local_guidance_control_interface::EulerAnglesZyx rotation_ControlFrame_Body_
 
local_guidance_control_interface::Twist lastComputedTwist_
 Holds last computed twist command. More...
 

Constructor & Destructor Documentation

local_guidance_pure_pursuit::LocalGuidancePurePursuit::LocalGuidancePurePursuit ( const std::string &  localGuidanceControllerName)
explicit
local_guidance_pure_pursuit::LocalGuidancePurePursuit::LocalGuidancePurePursuit ( )
delete
local_guidance_pure_pursuit::LocalGuidancePurePursuit::~LocalGuidancePurePursuit ( )
overridedefault

Member Function Documentation

void local_guidance_pure_pursuit::LocalGuidancePurePursuit::computeControlAction ( local_guidance_control_interface::Twist pTwist)
overridevirtual

Computes control action.

Parameters
[out]pTwistpointers to resulting control action (expressed as a twist).

Implements local_guidance_control_interface::LocalGuidanceControlInterface.

void local_guidance_pure_pursuit::LocalGuidancePurePursuit::computePreciseReachingManeuverControlAction ( local_guidance_control_interface::Twist pTwist)
protected

Computes angular and linear velocity commands to precisely reach last goal: (a) point towards last goal; (b) reach last goal by moving forward; (c) rotate to match last goal's orientation.

Parameters
[out]pTwistpointers to resulting control action (expressed as a twist).
void local_guidance_pure_pursuit::LocalGuidancePurePursuit::computePurePursuitControlAction ( local_guidance_control_interface::Twist pTwist)
protected

Computes Pure Pursuit control action. Adapted from Matlab function 'PurePursuitBase'.

Parameters
[out]pTwistpointers to resulting control action (expressed as a twist).
void local_guidance_pure_pursuit::LocalGuidancePurePursuit::fillTwistCommandControlFrameToBodyFrame ( double  linearVelocityX,
double  linearVelocityY,
double  angularVelocity,
local_guidance_control_interface::Twist pTwist 
)
protected

Fills a twist message, making sure commands are expressed in body frame. It converts commands from current control frame to body frame, if necessary.

Parameters
[in]linearVelocityXlinear velocity in X, in current control frame.
[in]linearVelocityYlinear velocity in Y, in current control frame.
[in]angularVelocityangular velocity in yaw, in current control frame.
[out]pTwisttwist command in body frame.
bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::getControllerReference ( local_guidance_control_interface::PoseStamped pCurrentReference) const
overridevirtual

Gets the current 3D reference used by the controller, useful for visualization purposes.

Parameters
[out]pCurrentReference.
Returns
true if p_current_reference can be used.

Reimplemented from local_guidance_control_interface::LocalGuidanceControlInterface.

template<enum navigation_common::DimensionType Dim>
double local_guidance_pure_pursuit::LocalGuidancePurePursuit::getDistanceFromLastGoalOfPath ( ) const
inlineprotected

Returns the distance from the robot's current position to the last goal's position of the path.

Returns
distance of the robot from last goal of the path, in the specified dimension.
template<enum navigation_common::DimensionType Dim>
double local_guidance_pure_pursuit::LocalGuidancePurePursuit::getDistanceFromPoses ( const local_guidance_control_interface::PoseStamped pose1,
const local_guidance_control_interface::PoseStamped pose2 
) const
inlineprotected

Returns the distance from two positions.

Parameters
[in]pose1first pose.
[in]pose2second pose.
Returns
distance in the specified dimension.
void local_guidance_pure_pursuit::LocalGuidancePurePursuit::getFollowingFeedback ( uint16_t *  remainingNumberOfSegments,
uint16_t *  initialNumberOfSegments,
double *  remainingDistance,
double *  initialDistance 
) const
overridevirtual

Returns a feedback about the status of the following action.

Parameters
[out]remainingNumberOfSegments
[out]initialNumberOfSegments
[out]remainingDistance
[out]initialDistance

Implements local_guidance_control_interface::LocalGuidanceControlInterface.

void local_guidance_pure_pursuit::LocalGuidancePurePursuit::getHeadingDifferenceControlFrameToLookaheadPoint ( )
protected

Computes the heading difference from robot's current control frame (see transformFromWorldToCurrentControlFrame) and lookahead's orientation.

void local_guidance_pure_pursuit::LocalGuidancePurePursuit::getLookaheadPoint ( )
protected

Computes the lookahead point on the path: from the projection of the robot's current reference it returns a pose that is at a specified distance on the path.

void local_guidance_pure_pursuit::LocalGuidancePurePursuit::getPositionErrorInControlFrame ( const kindr::VectorTypeless3D &  positionInput_World,
kindr::VectorTypeless3D *  errorVector_ControlFrame 
) const
protected

Computes the position error vector of the specified point from the current robot position.

Parameters
[in]positionInput_Worldinput position, expressed in world (fixed) frame W.
[out]errorVector_ControlFrameerror vector. errorVector_ControlFrame = R_ControlFrame_World * (positionInput_World - robotPosition_World). WARNING: the frame of this output parameter is "current control frame", see function transformFromWorldToCurrentControlFrame().
bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::getPurePursuitParameters ( bool  verbose = true)
protected

Retrieves actual pure pursuit parameters.

Parameters
[in]verbose
Returns
true if all parameters were retrieved.
double local_guidance_pure_pursuit::LocalGuidancePurePursuit::getYawDifferenceBetweenPoses ( const local_guidance_control_interface::PoseStamped pose1,
const local_guidance_control_interface::PoseStamped pose2 
) const
protected

Computes the shortest yaw difference between the orientation of two poses.

Parameters
[in]pose1
[in]pose2
Returns
yaw difference of orientation of pose2 form orientation of pose1: yaw_diff = yaw2 - yaw1.
double local_guidance_pure_pursuit::LocalGuidancePurePursuit::getYawDifferenceInRobotControlFrame ( const kindr::VectorTypeless3D &  positionInput_World) const
protected

Computes the yaw difference of the specified point from the current position of the control frame.

Parameters
[in]positionInput_Worldinput position, expressed in world (fixed) frame W.
Returns
yaw difference expressed w.r.t. current control frame, see function transformFromWorldToCurrentControlFrame().
bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::loadParametersFromFile ( const std::string &  pathYamlFileDefaultParams,
const std::string &  pathYamlFileOverlyingParams 
)
overridevirtual

Loads parameters with a default + overlying mechanism. See https://github.com/ethz-asl/ros_best_practices/wiki#parameter-organisation

Parameters
[in]pathYamlFileDefaultParamspath of the file containing default parameters.
[in]pathYamlFileOverlyingParamspath of the file containing parameters to overly on top default ones.

Implements local_guidance_control_interface::LocalGuidanceControlInterface.

bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::loadPurePursuitParametersFromFile ( const std::string &  pathYamlFileDefaultParams,
const std::string &  pathYamlFileOverlyingParams,
const std::string &  namePurePursuitController,
int  maximumNumberOfNestedSearch = 10 
)
protected

Loads parameters of specified pure pursuit controller. See https://github.com/ethz-asl/ros_best_practices/wiki#parameter-organisation

Parameters
[in]pathYamlFileDefaultParamspath of the file containing default parameters.
[in]pathYamlFileOverlyingParamspath of the file containing parameters to overly on top default ones.
[in]namePurePursuitControllername of the pure pursuit controller to prefix parameters search.
[in]maximumNumberOfNestedSearchlimits the number of nested search in case of default parameters.
bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::loadPurePursuitParametersInMemory ( const std::string &  pathYamlFileDefaultParams,
const std::string &  pathYamlFileOverlyingParams,
const std::string &  namePurePursuitController 
)
protected

Loads actual pure pursuit parameters in local memory, so they get be accessed through getters.

Parameters
[in]pathYamlFileDefaultParamspath of the file containing default parameters.
[in]pathYamlFileOverlyingParamspath of the file containing parameters to overly on top default ones.
[in]namePurePursuitControllername of the controller whose parameters should be loaded.
Returns
true if all parameters were retrieved.
void local_guidance_pure_pursuit::LocalGuidancePurePursuit::reset ( )
overridevirtual

Resets controller internal state.

Implements local_guidance_control_interface::LocalGuidanceControlInterface.

double local_guidance_pure_pursuit::LocalGuidancePurePursuit::saturate ( double  valueIn,
double  absMaxValue 
) const
protected

Saturates input between [-abs_max_value, abs_max_value].

Parameters
[in]valueInunconstrained value.
[in]absMaxValuemaximum positive value
Returns
saturated value.
bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::setDesiredPath ( const local_guidance_control_interface::PathConstPtr pDesiredPath)
overridevirtual

Sets the desired path that should be followed.

Parameters
[in]pDesiredPathdesired path.
Returns
true on success.

Implements local_guidance_control_interface::LocalGuidanceControlInterface.

bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::setRobotPose ( const local_guidance_control_interface::PoseStampedConstPtr pRobotPose)
overridevirtual

Sets current pose of the robot.

Parameters
[in]pRobotPosecurrent pose of the robot.
Returns
true on success.

Implements local_guidance_control_interface::LocalGuidanceControlInterface.

kindr::VectorTypeless3D local_guidance_pure_pursuit::LocalGuidancePurePursuit::transformFromWorldToCurrentControlFrame ( const kindr::VectorTypeless3D &  vector) const
protected

Transforms a vector from World frame to the current control frame. Control frame can either be "body" or "reverse body" (see variable rotation_ReverseBody_Body_). Choice is made based on whether the robot moves forward or backward.

Parameters
vectorvector expressed in world frame to be transformed in control frame.
Returns
vector in control frame.
local_guidance_control_interface::PoseStamped local_guidance_pure_pursuit::LocalGuidancePurePursuit::transformFromWorldToCurrentControlFrame ( const local_guidance_control_interface::PoseStamped pose) const
protected

Transforms a pose from World frame to the current control frame. Control frame can either be "body" or "reverse body" (see variable rotation_ReverseBody_Body_). Choice is made based on whether the robot moves forward or backward.

Parameters
posepose expressed in world frame to be transformed in control frame.
Returns
pose in control frame.
bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::updateCurrentPath ( const local_guidance_control_interface::PathConstPtr pDesiredPath)
overridevirtual

Updates the current path that is being followed. Clients should call this function instead of setDesiredPath() when a path is already being followed and it should be just updated.

Parameters
pDesiredPathnew path
Returns
true on success.

Implements local_guidance_control_interface::LocalGuidanceControlInterface.

Member Data Documentation

unsigned int local_guidance_pure_pursuit::LocalGuidancePurePursuit::idSegmentClosestPoseOnPath_
protected

Segment id on the path where the closest pose from the robot lays on.

double local_guidance_pure_pursuit::LocalGuidancePurePursuit::initialDistanceRobotLastGoal_
protected

Initial distance of the robot from last goal of the path.

bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::initialDistanceRobotLastGoalComputedOnce_
protected
bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::initialPoseReceivedOnce_
protected
local_guidance_control_interface::Twist local_guidance_pure_pursuit::LocalGuidancePurePursuit::lastComputedTwist_
protected

Holds last computed twist command.

bool local_guidance_pure_pursuit::LocalGuidancePurePursuit::pathReceivedOnce_
protected
local_guidance_control_interface::PoseStampedPtr local_guidance_pure_pursuit::LocalGuidancePurePursuit::pClosestPoseOnPath_
protected

Closest pose fon path from the robot.

local_guidance_control_interface::PoseStampedConstPtr local_guidance_pure_pursuit::LocalGuidancePurePursuit::pCurrentControllerGoal_
protected

Current goal of the controller.

local_guidance_control_interface::PoseStampedPtr local_guidance_pure_pursuit::LocalGuidancePurePursuit::pCurrentPose2d_
protected

Current pose of the robot.

local_guidance_control_interface::PathConstPtr local_guidance_pure_pursuit::LocalGuidancePurePursuit::pDesiredPath_
protected

Desired path the robot should follow.

local_guidance_control_interface::PoseStampedPtr local_guidance_pure_pursuit::LocalGuidancePurePursuit::pInitialPose2d_
protected

Initial pose of the robot.

local_guidance_control_interface::PoseStampedPtr local_guidance_pure_pursuit::LocalGuidancePurePursuit::pLastGoalOnPath_
protected

Last goal on the path.

local_guidance_control_interface::PoseStampedPtr local_guidance_pure_pursuit::LocalGuidancePurePursuit::pLastPose2d_
protected

Last pose of the robot.

PreciseReachingControllerData local_guidance_pure_pursuit::LocalGuidancePurePursuit::preciseReachingData_
protected

Data for controller to precisely reach last goal if the robot is close enough.

PurePursuitControllerData local_guidance_pure_pursuit::LocalGuidancePurePursuit::purePursuitData_
protected

Data for Pure Pursuit Controller.

local_guidance_control_interface::EulerAnglesZyx local_guidance_pure_pursuit::LocalGuidancePurePursuit::rotation_ControlFrame_Body_
protected

Holds the rotation matrix to transform from Body frame of robot to "Control Frame". Working in Control Frame allows to have a generic framework to express the preferred "forward" direction of the robot. This can be for example, "backwards direction" of the robot (i.e. in the opposite direction of the forward X axis of body frame), or "left direction" (i.e. in the direction of the positive Y axis of body frame), etc.


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