controllers::StanleyController Class Reference

Provides an implementation of a lightweight lateral StanleyController. This algorithm is used to precisely control a different drive robot to follow a given path. More...

#include <StanleyController.h>

Public Member Functions
	StanleyController (const double dKp=1.0, const double dDistToFrontAxle=1.0, const double dYawTolerance=1.0)
	Construct a new Stanley Contoller:: Stanley Contoller object.
	StanleyController (const std::vector< geoops::UTMCoordinate > &vUTMPath, const double dKp, const double dDistToFrontAxle, const double dYawTolerance)
	Construct a new Stanley Contoller:: Stanley Contoller object.
	StanleyController (const std::vector< geoops::GPSCoordinate > &vGPSPath, const double dKp, const double dDistToFrontAxle, const double dYawTolerance)
	Construct a new Stanley Contoller:: Stanley Contoller object.
	~StanleyController ()
	Destroy the Stanley Contoller:: Stanley Contoller object.
double	Calculate (const geoops::UTMCoordinate &stUTMCurrPos, const double dVelocity, const double dCurrentHeading)
	Calculate the steering control adjustment for an agent using the Stanley method.
double	Calculate (const geoops::GPSCoordinate &stGPSCurrPos, const double dVelocity, const double dCurrentHeading)
	Calculate the steering control adjustment for an agent using the Stanley method.
void	ResetProgress ()
	Resets the progress on the current path.
void	SetSteeringControlGain (const double dKpp)
	Setter for steering control gain.
void	SetDistanceToFrontAxle (const double dDistToFrontAxle)
	Setter for distance to front axle.
void	SetYawTolerance (const double dYawTolerance)
	Setter for yaw tolerance.
void	SetPath (const std::vector< geoops::UTMCoordinate > &vUTMPath)
	Setter for path.
void	SetPath (const std::vector< geoops::GPSCoordinate > &vGPSPath)
	Setter for path.
void	SetPath (const std::vector< geoops::Waypoint > &vWaypointsPath)
	Setter for path.
void	ClearPath ()
	Clear the current path.
double	GetSteeringControlGain () const
	Getter for steering control gain.
double	GetDistanceToFrontAxle () const
	Getter for distance to the front axle.
double	GetYawTolerance () const
	Getter for yaw tolerance.
unsigned int	GetLastTargetIdx () const
	Getter for the index of the last target point on the path.
std::vector< geoops::UTMCoordinate >	GetPathUTM () const
	Getter for path.
std::vector< geoops::GPSCoordinate >	GetPathGPS () const
	Getter for path.

Private Member Functions
geoops::UTMCoordinate	CalculateFrontAxleCoordinate (const geoops::UTMCoordinate &stUTMCurrPos, const double dCurrentHeading) const
	Calculate the UTM coordinate of the center of the agent's front axle.
unsigned int	IdentifyTargetIdx (const geoops::UTMCoordinate &stUTMFrontAxlePos) const
	Identifies the closest point to the center of the agent's front axle on the path.
double	CalculateTargetBearing (const unsigned int unTargetIdx) const
	Calculate the required heading to navigate from the current target point to the subsequent point on the path.
double	CalculateCrossTrackError (const geoops::UTMCoordinate &stUTMFrontAxlePos, const unsigned int unTargetIdx, const double dCurrentHeading) const
	Calculate the cross track error. This error expresses how far off the agent is from the path (lateral distance).

Private Attributes
double	m_dKp
double	m_dDistToFrontAxle
double	m_dYawTolerance
unsigned int	m_unLastTargetIdx
std::vector< geoops::UTMCoordinate >	m_vUTMPath
std::vector< geoops::GPSCoordinate >	m_vGPSPath

Detailed Description

Provides an implementation of a lightweight lateral StanleyController. This algorithm is used to precisely control a different drive robot to follow a given path.

Note

In this class heading/bearing refers to the absolute orientation of a rover (N,E,S,W). While yaw refers to an angle relative to the agent's current orientation.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-01

Constructor & Destructor Documentation

StanleyController() [1/3]

controllers::StanleyController::StanleyController	(	const double	dKp = 1.0,
		const double	dDistToFrontAxle = 1.0,
		const double	dYawTolerance = 1.0
	)

Construct a new Stanley Contoller:: Stanley Contoller object.

Parameters

dKp	- Steering control gain.
dDistToFrontAxle	- Distance between the position sensor (GPS) and the front axle.
dYawTolerance	- Minimum yaw change threshold for execution.

Note

The higher the steering control gain the more reactive the rover will be to changes in yaw.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-03

    {
        // Initialize member variables
        m_dKp              = dKp;
        m_dDistToFrontAxle = dDistToFrontAxle;
        m_dYawTolerance    = dYawTolerance;
        m_unLastTargetIdx  = 0;
    }

StanleyController() [2/3]

controllers::StanleyController::StanleyController	(	const std::vector< geoops::UTMCoordinate > &	vUTMPath,
		const double	dKp,
		const double	dDistToFrontAxle,
		const double	dYawTolerance
	)

Construct a new Stanley Contoller:: Stanley Contoller object.

Parameters

vUTMPath	- Vector of UTM coordinates describing path to follow.
dKp	- Steering control gain.
dDistToFrontAxle	- Distance between the position sensor (GPS) and the front axle.
dYawTolerance	- Minimum yaw change threshold for execution.

Note

The higher the steering control gain the more reactive the rover will be to changes in yaw.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-03

    {
        // Initialize member variables
        m_dKp              = dKp;
        m_dDistToFrontAxle = dDistToFrontAxle;
        m_dYawTolerance    = dYawTolerance;
        m_unLastTargetIdx  = 0;
        m_vUTMPath         = vUTMPath;
 
        // Convert the UTM path to a GPS path and save it.
        m_vGPSPath.clear();
        std::vector<geoops::UTMCoordinate>::const_iterator itrUTM = vUTMPath.begin();
        while (itrUTM != vUTMPath.end())
        {
            m_vGPSPath.push_back(geoops::ConvertUTMToGPS(*itrUTM));
            ++itrUTM;
        }
    }

Here is the call graph for this function:

StanleyController() [3/3]

controllers::StanleyController::StanleyController	(	const std::vector< geoops::GPSCoordinate > &	vGPSPath,
		const double	dKp,
		const double	dDistToFrontAxle,
		const double	dYawTolerance
	)

Construct a new Stanley Contoller:: Stanley Contoller object.

Parameters

vGPSPath	- Vector of GPS coordinates describing path to follow.
dKp	- Steering control gain.
dDistToFrontAxle	- Distance between the position sensor (GPS) and the front axle.
dYawTolerance	- Minimum yaw change threshold for execution.

Note

The higher the steering control gain the more reactive the rover will be to changes in yaw.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-17-03

    {
        // Initialize member variables
        m_dKp              = dKp;
        m_dDistToFrontAxle = dDistToFrontAxle;
        m_dYawTolerance    = dYawTolerance;
        m_unLastTargetIdx  = 0;
 
        // For each GPS coordinate convert it to UTM and save it to the path.
        std::vector<geoops::GPSCoordinate>::const_iterator itrGPS = vGPSPath.begin();
        while (itrGPS != vGPSPath.end())
        {
            m_vUTMPath.push_back(geoops::ConvertGPSToUTM(*itrGPS));
            ++itrGPS;
        }
        m_vGPSPath = vGPSPath;
    }

Here is the call graph for this function:

~StanleyController()

controllers::StanleyController::~StanleyController

(

)

Destroy the Stanley Contoller:: Stanley Contoller object.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-01

    {
        // Nothing to destroy yet.
    }

Member Function Documentation

Calculate() [1/2]

double controllers::StanleyController::Calculate	(	const geoops::UTMCoordinate &	stUTMCurrPos,
		const double	dVelocity,
		const double	dCurrentHeading
	)

Calculate the steering control adjustment for an agent using the Stanley method.

This function computes the necessary change in yaw (steering angle) to align the agent with the predetermined path. The steering angle is limited by the proportional gain constant to prevent excessively sharp turns.

Parameters

stUTMCurrPos	- The agent's current position in the UTM coordinate space.
dVelocity	- The agent's current magnitude of velocity.
dCurrentHeading	- The agent's current yaw angle (heading).

Returns

double - The calculated change in yaw needed to align the agent with the path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-03

    {
        // Verify the given heading is within 0-360 degrees.
        if (dCurrentHeading < 0 || dCurrentHeading > 360)
        {
            LOG_ERROR(logging::g_qSharedLogger, "Heading must be in the interval [0-360]. Received: {}", dCurrentHeading);
            return 0;
        }
        // Verify a path has been loaded into the Stanley Controller
        if (m_vUTMPath.empty())
        {
            LOG_ERROR(logging::g_qSharedLogger, "No path has been loaded.");
            return 0;
        }
 
        // Calculate the position for the center of the front axle.
        geoops::UTMCoordinate stUTMFrontAxlePos = CalculateFrontAxleCoordinate(stUTMCurrPos, dCurrentHeading);
 
        // Find the point on the path closest to the front axle center
        unsigned int unTargetIdx = IdentifyTargetIdx(stUTMFrontAxlePos);
 
        // Make sure the agent proceeds forward through the path
        unTargetIdx       = std::max(unTargetIdx, m_unLastTargetIdx);
        m_unLastTargetIdx = unTargetIdx;
 
        // Calculate the change in yaw needed to go from the target to the next point
        double dTargetYaw = CalculateTargetBearing(unTargetIdx);
 
        // Calculate the difference between the rover's yaw and the desired path yaw
        double dYawError = numops::InputAngleModulus<double>(dTargetYaw - dCurrentHeading, -180.0, 180.0);
 
        // Calculate the change in yaw needed to correct for the cross track error
        double dCrossTrackError = CalculateCrossTrackError(stUTMFrontAxlePos, unTargetIdx, dCurrentHeading);
        double dDeltaYaw        = dYawError + std::atan2(m_dKp * dCrossTrackError, dVelocity);
 
        // If a rotation is small enough we will just go ahead and skip it
        if (std::abs(dDeltaYaw) < m_dYawTolerance)
        {
            dDeltaYaw = 0;
        }
 
        // Here we translate the relative change in yaw to an absolute heading
        return numops::InputAngleModulus<double>(dCurrentHeading + dDeltaYaw, 0, 360);
    }

Here is the call graph for this function:

Here is the caller graph for this function:

Calculate() [2/2]

double controllers::StanleyController::Calculate	(	const geoops::GPSCoordinate &	stGPSCurrPos,
		const double	dVelocity,
		const double	dCurrentHeading
	)

Calculate the steering control adjustment for an agent using the Stanley method.

This function computes the necessary change in yaw (steering angle) to align the agent with the predetermined path. The steering angle is limited by the proportional gain constant to prevent excessively sharp turns.

Parameters

stGPSCurrPos	- The agent's current position in the GPS coordinate space.
dVelocity	- The agent's current magnitude of velocity.
dCurrentHeading	- The agent's current yaw angle (heading).

Returns

double - The calculated change in yaw needed to align the agent with the path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-17

    {
        geoops::UTMCoordinate stUTMCurrPos = geoops::ConvertGPSToUTM(stGPSCurrPos);
        return Calculate(stUTMCurrPos, dVelocity, dCurrentHeading);
    }

Here is the call graph for this function:

ResetProgress()

void controllers::StanleyController::ResetProgress

(

)

Resets the progress on the current path.

The stanley control algorithm prevents points that have been pass to be revisited to motivate the agent to go further along the path. Although sometimes it may be necessary to restart an agents progress if it deviates far enough from the path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        m_unLastTargetIdx = 0;
    }

Here is the caller graph for this function:

SetSteeringControlGain()

void controllers::StanleyController::SetSteeringControlGain

(

const double

dKp

)

Setter for steering control gain.

Parameters

dKp	- Steering control gain.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        m_dKp = dKp;
    }

SetDistanceToFrontAxle()

void controllers::StanleyController::SetDistanceToFrontAxle

(

const double

dDistToFrontAxle

)

Setter for distance to front axle.

Parameters

dDistToFrontAxle

- Distance between the position sensor (GPS) and the front axle.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        m_dDistToFrontAxle = dDistToFrontAxle;
    }

SetYawTolerance()

void controllers::StanleyController::SetYawTolerance

(

const double

dYawTolerance

)

Setter for yaw tolerance.

Parameters

dYawTolerance

- Minimum yaw change threshold for execution.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        m_dYawTolerance = dYawTolerance;
    }

SetPath() [1/3]

void controllers::StanleyController::SetPath

(

const std::vector< geoops::UTMCoordinate > &

vUTMPath

)

Setter for path.

Parameters

vUTMPath

- Vector of UTM coordinates describing path to follow.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        m_vUTMPath        = vUTMPath;
        m_unLastTargetIdx = 0;
 
        // Convert the UTM path to a GPS path and save it.
        m_vGPSPath.clear();
        std::vector<geoops::UTMCoordinate>::const_iterator itrUTM = vUTMPath.begin();
        while (itrUTM != vUTMPath.end())
        {
            m_vGPSPath.push_back(geoops::ConvertUTMToGPS(*itrUTM));
            ++itrUTM;
        }
    }

Here is the call graph for this function:

SetPath() [2/3]

void controllers::StanleyController::SetPath

(

const std::vector< geoops::GPSCoordinate > &

vGPSPath

)

Setter for path.

Parameters

vGPSPath

- Vector of GPS coordinates describing path to follow.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-17

    {
        m_vUTMPath.clear();
 
        // For each GPS coordinate convert it to UTM and save it to the path.
        std::vector<geoops::GPSCoordinate>::const_iterator itrGPS = vGPSPath.begin();
        while (itrGPS != vGPSPath.end())
        {
            m_vUTMPath.push_back(geoops::ConvertGPSToUTM(*itrGPS));
            ++itrGPS;
        }
        m_vGPSPath        = vGPSPath;
 
        m_unLastTargetIdx = 0;
    }

Here is the call graph for this function:

SetPath() [3/3]

void controllers::StanleyController::SetPath

(

const std::vector< geoops::Waypoint > &

vWaypointsPath

)

Setter for path.

Parameters

vWaypointsPath

- Vector of waypoints describing path to follow.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-07

    {
        // Clear the current paths.
        m_vUTMPath.clear();
        m_vGPSPath.clear();
 
        // For each GPS coordinate convert it to UTM and save it to the path.
        std::vector<geoops::Waypoint>::const_iterator itrWaypoint = vWaypointsPath.begin();
        while (itrWaypoint != vWaypointsPath.end())
        {
            m_vUTMPath.push_back(itrWaypoint->GetUTMCoordinate());
            m_vGPSPath.push_back(itrWaypoint->GetGPSCoordinate());
            ++itrWaypoint;
        }
 
        // Reset the last target index.
        m_unLastTargetIdx = 0;
    }

ClearPath()

void controllers::StanleyController::ClearPath

(

)

Clear the current path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-07

    {
        m_vUTMPath.clear();
        m_vGPSPath.clear();
    }

GetSteeringControlGain()

double controllers::StanleyController::GetSteeringControlGain

(

)

const

Getter for steering control gain.

Returns

double - Steering control gain.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        return m_dKp;
    }

GetDistanceToFrontAxle()

double controllers::StanleyController::GetDistanceToFrontAxle

(

)

const

Getter for distance to the front axle.

Returns

double - Distance between the position sensor (GPS) and the front axle.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        return m_dDistToFrontAxle;
    }

GetYawTolerance()

double controllers::StanleyController::GetYawTolerance

(

)

const

Getter for yaw tolerance.

Returns

double - Minimum yaw change threshold for execution.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        return m_dYawTolerance;
    }

GetLastTargetIdx()

unsigned int controllers::StanleyController::GetLastTargetIdx

(

)

const

Getter for the index of the last target point on the path.

Returns

unsigned int - Index of the last target point on the path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        return m_unLastTargetIdx;
    }

Here is the caller graph for this function:

GetPathUTM()

std::vector< geoops::UTMCoordinate > controllers::StanleyController::GetPathUTM

(

)

const

Getter for path.

Returns

std::vector<geoops::UTMCoordinate> - Sequence of UTM coordinates defining the navigational path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-16

    {
        return m_vUTMPath;
    }

GetPathGPS()

std::vector< geoops::GPSCoordinate > controllers::StanleyController::GetPathGPS

(

)

const

Getter for path.

Returns

std::vector<geoops::GPSCoordinate> - Sequence of GPS coordinates defining the navigational path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-17

    {
        return m_vGPSPath;
    }

CalculateFrontAxleCoordinate()

geoops::UTMCoordinate controllers::StanleyController::CalculateFrontAxleCoordinate ( const geoops::UTMCoordinate &  stUTMCurrPos, const double  dCurrentHeading  ) const

private

Calculate the UTM coordinate of the center of the agent's front axle.

Parameters

stUTMCurrPos	- The agent's current position in the UTM coordinate space.
dCurrentHeading	- The current heading of the agent, measured in degrees from 0 to 360.

Returns

UTMCoordinate - The UTM coordinate of the center of the agent's front axle.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-08

    {
        // Convert the heading to a change in degrees of yaw relative to the north axis
        // Here a positive degree represents a change in yaw towards the East.
        // Here a negative degree represents a change in yaw towards the West.
        double dChangeInYawRelToNorth = dCurrentHeading <= 180 ? dCurrentHeading : dCurrentHeading - 360;
 
        // Convert to radians.
        dChangeInYawRelToNorth = (dChangeInYawRelToNorth / 180) * M_PI;
 
        // Calculate the unit vector for the agent's orientation in terms of East and North.
        double dOrientEast  = std::sin(dChangeInYawRelToNorth);
        double dOrientNorth = std::cos(dChangeInYawRelToNorth);
 
        // Calculate the UTM coordinate for the center of the agent's front axle.
        geoops::UTMCoordinate stUTMFrontAxlePos = geoops::UTMCoordinate(stUTMCurrPos);
        stUTMFrontAxlePos.dEasting              = stUTMCurrPos.dEasting + m_dDistToFrontAxle * dOrientEast;
        stUTMFrontAxlePos.dNorthing             = stUTMCurrPos.dNorthing + m_dDistToFrontAxle * dOrientNorth;
 
        // Return the UTM coordinate of the center of the agent's front axle.
        return stUTMFrontAxlePos;
    }

Here is the caller graph for this function:

IdentifyTargetIdx()

unsigned int controllers::StanleyController::IdentifyTargetIdx ( const geoops::UTMCoordinate &  stUTMFrontAxlePos ) const

private

Identifies the closest point to the center of the agent's front axle on the path.

This function scans through a set of path points to find the one that is nearest to the center of the agent's front axle, providing a crucial reference for any path corrections.

Parameters

stUTMFrontAxlePos

- The UTM coordinate of the center of the agent's front axle.

Returns

unsigned int - Index of the target point on the path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-03

    {
        // Search for the nearest point in the path
        unsigned int unTargetIdx                                  = 0;
        double dMinDistance                                       = std::numeric_limits<double>::max();
        std::vector<geoops::UTMCoordinate>::const_iterator itPath = this->m_vUTMPath.begin();
        while (itPath != this->m_vUTMPath.end())
        {
            // Find the distance in meters between the center of the front axle and a point on the path.
            double dDistanceFromPoint = std::hypot(stUTMFrontAxlePos.dNorthing - itPath->dNorthing, stUTMFrontAxlePos.dEasting - itPath->dEasting);
 
            // Update the closest point to the front axle's center if the current distance is the shortest recorded.
            // Save both the point's index and the distance.
            if (dDistanceFromPoint < dMinDistance)
            {
                unTargetIdx  = std::distance(this->m_vUTMPath.begin(), itPath);
                dMinDistance = dDistanceFromPoint;
            }
 
            // Move the iterator to the next point in the path
            ++itPath;
        }
 
        return unTargetIdx;
    }

Here is the caller graph for this function:

CalculateTargetBearing()

double controllers::StanleyController::CalculateTargetBearing ( const unsigned int  unTargetIdx ) const

private

Calculate the required heading to navigate from the current target point to the subsequent point on the path.

Parameters

unTargetIdx

- Index of the target point on the path.

Returns

double - Bearing required to get from the target point to the subsequent point on the path.

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-03

    {
        // Verify the target index is a valid point on the path.
        if (m_vUTMPath.empty() || unTargetIdx >= m_vUTMPath.size())
        {
            LOG_ERROR(logging::g_qSharedLogger, "Target index {} does not exist on path.", unTargetIdx);
            return 0;
        }
 
        // The yaw is calculated by finding the heading needed to navigate from the
        // target point to the next point in the path.
        geoops::UTMCoordinate stUTMTargetPoint = m_vUTMPath[unTargetIdx];
        geoops::UTMCoordinate stUTMNextPoint   = m_vUTMPath[unTargetIdx + 1];
 
        // Calculate the displacement from the target point to the next point in the path.
        double dDisplacementEast  = stUTMNextPoint.dEasting - stUTMTargetPoint.dEasting;
        double dDisplacementNorth = stUTMNextPoint.dNorthing - stUTMTargetPoint.dNorthing;
 
        // Calculate the magnitude of the displacement.
        double dDisplacementMagnitude = std::hypot(dDisplacementEast, dDisplacementNorth);
 
        // Calculate the heading in degrees required to navigate to the next point on the path.
        dDisplacementNorth /= dDisplacementMagnitude;
        double dCurrentHeading = (std::acos(dDisplacementNorth) / M_PI) * 180;
        if (dDisplacementEast < 0)
        {
            dCurrentHeading = 360 - dCurrentHeading;
        }
 
        // Return the relative heading needed to get from the target point to the next point in the path.
        return dCurrentHeading;
    }

Here is the caller graph for this function:

CalculateCrossTrackError()

double controllers::StanleyController::CalculateCrossTrackError ( const geoops::UTMCoordinate &  stUTMFrontAxlePos, const unsigned int  unTargetIdx, const double  dCurrentHeading  ) const

private

Calculate the cross track error. This error expresses how far off the agent is from the path (lateral distance).

Note

Here is what causes a high magnitude for cross track error.

1. The agent is far from the target point (a large displacement from the path).
2. The agent is driving parallel to the path. The reason for number 2 is if the agent is far from the path and driving parallel it will continue to stay off course. However, if the agent is on the path and driving parallel the displacement vector will be negligible and the error will likewise be negligible.

Note that the significance of the error is its magnitude. Positive and negative are the same amount of error just in opposing directions.

Parameters

stUTMFrontAxlePos	- The UTM coordinate of the center of the agent's front axle.
unTargetIdx	- Index of the target point on the path.
dCurrentHeading	- The current heading of the agent, measured in degrees from 0 to 360.

Returns

double - The cross track error (CTE).

Author

JSpencerPittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-02-09

    {
        // Check if the target index is valid and the path is not empty.
        if (m_vUTMPath.empty() || unTargetIdx >= m_vUTMPath.size())
        {
            LOG_ERROR(logging::g_qSharedLogger, "Target index {} does not exist on path.", unTargetIdx);
            return 0;
        }
 
        // Convert the heading to a change in degrees of yaw relative to the north axis
        // Here a positive degree represents a change in yaw towards the East.
        // Here a negative degree represents a change in yaw towards the West.
        double dChangeInYawRelToNorth = dCurrentHeading <= 180 ? dCurrentHeading : dCurrentHeading - 360;
        dChangeInYawRelToNorth        = (dChangeInYawRelToNorth / 180) * M_PI;
        dChangeInYawRelToNorth -= M_PI / 2;
 
        // Calculate the front axle vector.
        // This vector will point perpendicular to the orientation of the agent (representing the front drive axle).
        double dFrontDriveAxleX = std::sin(dChangeInYawRelToNorth);
        double dFrontDriveAxleY = std::cos(dChangeInYawRelToNorth);
 
        // Find the displacement between the target and the center of the front drive axle.
        geoops::UTMCoordinate stUTMTargetPos = m_vUTMPath[unTargetIdx];
        double dDisplacementX                = stUTMFrontAxlePos.dEasting - stUTMTargetPos.dEasting;
        double dDisplacementY                = stUTMFrontAxlePos.dNorthing - stUTMTargetPos.dNorthing;
 
        // Calculate the cross track error as a dot product of our front drive axle and the displacement vector.
        return (dDisplacementX * dFrontDriveAxleX) + (dDisplacementY * dFrontDriveAxleY);
    }

Here is the caller graph for this function:

---

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

• src/algorithms/controllers/StanleyController.h
• src/algorithms/controllers/StanleyController.cpp