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 ()
	Default constructor for Stanley Controller.

	StanleyController (const double dKp, const double dDistToFrontAxle, const double dYawTolerance)
	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 dBearing)
	Calculate the steering control adjustment for an agent using the Stanley method.

double	Calculate (const geoops::GPSCoordinate &stGPSCurrPos, const double dVelocity, const double dBearing)
	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 (std::vector< geoops::UTMCoordinate > &vUTMPath)
	Setter for path.

void	SetPath (std::vector< geoops::GPSCoordinate > &vGPSPath)
	Setter for path.

void	ClearPath ()

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 dBearing) 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 bearing 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 dBearing) 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/4]

controllers::StanleyController::StanleyController ( )

Default constructor for Stanley Controller.

Author: Kai Shafe (kasq5.nosp@m.m@um.nosp@m.syste.nosp@m.m.ed.nosp@m.u)

Date: 2024-04-28

    {
        // Nothing to do.
    }

◆ StanleyController() [2/4]

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

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() [3/4]

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() [4/4]

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	dBearing
	)

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.
dBearing	- 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 bearing is within 0-360 degrees.
        if (dBearing < 0 || dBearing > 360)
        {
            LOG_ERROR(logging::g_qSharedLogger, "StanleyController::Calculate bearing must be in the interval [0-360]. Received: {}", dBearing);
        }
        // Verify a path has been loaded into the Stanley Controller
        if (m_vUTMPath.empty())
        {
            LOG_ERROR(logging::g_qSharedLogger, "StanleyController::Calculate No path has been loaded.");
        }
 
        // Calculate the position for the center of the front axle.
        geoops::UTMCoordinate stUTMFrontAxlePos = CalculateFrontAxleCoordinate(stUTMCurrPos, dBearing);
 
        // 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 - dBearing, -180.0, 180.0);
 
        // Calculate the change in yaw needed to correct for the cross track error
        double dCrossTrackError = CalculateCrossTrackError(stUTMFrontAxlePos, unTargetIdx, dBearing);
        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>(dBearing + 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	dBearing
	)

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.
dBearing	- 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, dBearing);
    }

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/2]

void controllers::StanleyController::SetPath ( 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:

Here is the caller graph for this function:

◆ SetPath() [2/2]

void controllers::StanleyController::SetPath ( 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:

◆ ClearPath()

void controllers::StanleyController::ClearPath ( )

    {
        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	dBearing
	)		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.
dBearing	- The current bearing 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 bearing 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 = dBearing <= 180 ? dBearing : dBearing - 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 bearing 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 (unTargetIdx >= m_vUTMPath.size())
        {
            LOG_ERROR(logging::g_qSharedLogger, "StanleyController::CalculateTargetBearing target {} does not exist.", unTargetIdx);
        }
 
        // The yaw is calculated by finding the bearing 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 bearing in degrees required to navigate to the next point on the path.
        dDisplacementNorth /= dDisplacementMagnitude;
        double dBearing = (std::acos(dDisplacementNorth) / M_PI) * 180;
        if (dDisplacementEast < 0)
        {
            dBearing = 360 - dBearing;
        }
 
        // Return the relative bearing needed to get from the target point to the next point in the path.
        return dBearing;
    }

Here is the caller graph for this function:

◆ CalculateCrossTrackError()

double controllers::StanleyController::CalculateCrossTrackError	(	const geoops::UTMCoordinate &	stUTMFrontAxlePos,
		const unsigned int	unTargetIdx,
		const double	dBearing
	)		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.

The agent is far from the target point (a large displacement from the path).
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.
dBearing	- The current bearing 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

    {
        // Convert the bearing 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 = dBearing <= 180 ? dBearing : dBearing - 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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ StanleyController() [1/4]

◆ StanleyController() [2/4]

◆ StanleyController() [3/4]

◆ StanleyController() [4/4]

◆ ~StanleyController()

Member Function Documentation

◆ Calculate() [1/2]

◆ Calculate() [2/2]

◆ ResetProgress()

◆ SetSteeringControlGain()

◆ SetDistanceToFrontAxle()

◆ SetYawTolerance()

◆ SetPath() [1/2]

◆ SetPath() [2/2]

◆ ClearPath()

◆ GetSteeringControlGain()

◆ GetDistanceToFrontAxle()

◆ GetYawTolerance()

◆ GetLastTargetIdx()

◆ GetPathUTM()

◆ GetPathGPS()

◆ CalculateFrontAxleCoordinate()

◆ IdentifyTargetIdx()

◆ CalculateTargetBearing()

◆ CalculateCrossTrackError()