controllers::PurePursuitController Class Reference

This class implements the Pure Pursuit Controller. This controller is used to follow a path by calculating the steering angle required to reach a lookahead point a set distance down the reference path. More...

#include <PurePursuitController.h>

Classes
struct	DriveVector
	The struct for the drive vector that includes heading and velocity. More...

Public Member Functions
	PurePursuitController (const double dLookaheadDistance=2.0, const int nLookaheadIndex=5)
	Construct a new Pure Pursuit Controller:: Pure Pursuit Controller object.
DriveVector	Calculate (const geoops::RoverPose &stCurrentPose, const double dMaxSpeed)
	Calculate an updated drive vector for the rover based on the current pose using the pure pursuit control law.
void	SetReferencePath (const std::vector< geoops::Waypoint > &vReferencePath)
	Set the Reference Path object.
void	SetLookaheadDistance (const double dLookaheadDistance)
	Set the Lookahead Distance object.
void	SetLookaheadIndex (const int nLookaheadIndex)
	This will allow us to set the max amount of indices to lookahead.
std::vector< geoops::Waypoint >	GetReferencePath () const
	Get the Reference Path object.
double	GetLookaheadDistance () const
	Get the Lookahead Distance object.
int	GetReferencePathTargetIndex () const
	Get the Reference Path Target Index object.

Private Member Functions
int	FindClosestWaypointIndex (const geoops::UTMCoordinate &stCurrentPosition)
	Finds the closest waypoint index in the path to the current position, enforcing topological blinders to prevent jumping across overlapping paths.
geoops::Waypoint	FindLookaheadWaypoint (const geoops::UTMCoordinate &stCurrentPosition)
	Searches forward from the current target index to find a point that is at least the lookahead distance away from the rover.

Private Attributes
double	m_dLookaheadDistance
int	m_nLookaheadIndex
int	m_nCurrentReferencePathTargetIndex
std::vector< geoops::Waypoint >	m_vReferencePath

Detailed Description

This class implements the Pure Pursuit Controller. This controller is used to follow a path by calculating the steering angle required to reach a lookahead point a set distance down the reference path.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

Constructor & Destructor Documentation

PurePursuitController()

controllers::PurePursuitController::PurePursuitController	(	const double	dLookaheadDistance = 2.0,
		const int	nLookaheadIndex = 5
	)

Construct a new Pure Pursuit Controller:: Pure Pursuit Controller object.

Parameters

dLookaheadDistance	- The distance in meters to look ahead of current position.
dLookaheadIndex	- The number of waypoints the rover will look ahead.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        // Initialize member variables.
        m_dLookaheadDistance               = dLookaheadDistance;
        m_nLookaheadIndex                  = nLookaheadIndex;
        m_nCurrentReferencePathTargetIndex = 0;
    }

Member Function Documentation

Calculate()

PurePursuitController::DriveVector controllers::PurePursuitController::Calculate	(	const geoops::RoverPose &	stCurrentPose,
		const double	dMaxSpeed
	)

Calculate an updated drive vector for the rover based on the current pose using the pure pursuit control law.

Parameters

stCurrentPose	- The current pose of the rover.
dMaxSpeed	- The maximum speed the rover can travel.

Returns

DriveVector - The new output heading and speed for the rover.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        // Check if the reference path is empty.
        if (m_vReferencePath.empty())
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "PurePursuitController::Calculate: Reference path is empty. Cannot calculate drive powers.");
 
            return DriveVector{0.0, 0.0};
        }
 
        // Evaluate the rover position to properly advance the path index based on physical location.
        m_nCurrentReferencePathTargetIndex = FindClosestWaypointIndex(stCurrentPose.GetUTMCoordinate());
 
        // Check if we are at the end of the path.
        if (m_nCurrentReferencePathTargetIndex >= static_cast<int>(m_vReferencePath.size()) - 2)
        {
            // Get the start and end points of the last segment.
            geoops::UTMCoordinate stLastPoint         = m_vReferencePath.back().GetUTMCoordinate();
            geoops::UTMCoordinate stSecondToLastPoint = m_vReferencePath[m_vReferencePath.size() - 2].GetUTMCoordinate();
            geoops::UTMCoordinate stRoverPos          = stCurrentPose.GetUTMCoordinate();
 
            // Calculate vector of the last segment (Start -> End).
            double dSegmentX     = stLastPoint.dEasting - stSecondToLastPoint.dEasting;
            double dSegmentY     = stLastPoint.dNorthing - stSecondToLastPoint.dNorthing;
            double dSegmentLenSq = dSegmentX * dSegmentX + dSegmentY * dSegmentY;
 
            // Calculate vector from Segment Start -> Rover.
            double dRoverVectorX = stRoverPos.dEasting - stSecondToLastPoint.dEasting;
            double dRoverVectorY = stRoverPos.dNorthing - stSecondToLastPoint.dNorthing;
 
            // Project rover onto the segment vector.
            double dNormalDistance = 0.0;
            if (dSegmentLenSq > 1e-6)
            {
                dNormalDistance = (dRoverVectorX * dSegmentX + dRoverVectorY * dSegmentY) / dSegmentLenSq;
            }
 
            // Check if we have passed the end or are very close to it.
            if (dNormalDistance >= 1.0 || std::hypot(dRoverVectorX, dRoverVectorY) < 0.5)
            {
                // We have reached or passed the end. Command the rover to stop.
                double dHeadingToLastWaypoint = geoops::CalculateGeoMeasurement(stRoverPos, stLastPoint).dStartRelativeBearing;
                return DriveVector{dHeadingToLastWaypoint, 0.0};
            }
        }
 
        // Find the lookahead point on the path ahead of the rover.
        geoops::Waypoint stLookaheadWaypoint = FindLookaheadWaypoint(stCurrentPose.GetUTMCoordinate());
 
        // Calculate the required heading to reach the lookahead point.
        double dDeltaX = stLookaheadWaypoint.GetUTMCoordinate().dEasting - stCurrentPose.GetUTMCoordinate().dEasting;
        double dDeltaY = stLookaheadWaypoint.GetUTMCoordinate().dNorthing - stCurrentPose.GetUTMCoordinate().dNorthing;
 
        // Math angle of segment (atan2 gives angle from +X (East), CCW positive) in degrees.
        double dTargetMathDeg = std::atan2(dDeltaY, dDeltaX) * (180.0 / M_PI);
 
        // Convert math angle to COMPASS heading (0 = North, clockwise positive): compass = (90 - math) mod 360.
        double dAbsoluteHeadingGoal = std::fmod(90.0 - dTargetMathDeg + 360.0, 360.0);
 
        return DriveVector{dAbsoluteHeadingGoal, dMaxSpeed};
    }

Here is the call graph for this function:

SetReferencePath()

void controllers::PurePursuitController::SetReferencePath

(

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

vReferencePath

)

Set the Reference Path object.

Parameters

vReferencePath

- The new reference path for the controller to follow.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        // Reset the index and store the new path.
        m_nCurrentReferencePathTargetIndex = 0;
        m_vReferencePath                   = vReferencePath;
    }

SetLookaheadDistance()

void controllers::PurePursuitController::SetLookaheadDistance

(

const double

dLookaheadDistance

)

Set the Lookahead Distance object.

Parameters

dLookaheadDistance

- The distance ahead of the rover to place the target point.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        m_dLookaheadDistance = dLookaheadDistance;
    }

SetLookaheadIndex()

void controllers::PurePursuitController::SetLookaheadIndex

(

const int

nLookAheadIndex

)

This will allow us to set the max amount of indices to lookahead.

Parameters

nLookAheadIndex

- How many waypoints/indices we can look ahead.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-04-14

    {
        m_nLookaheadIndex = nLookAheadIndex;
    }

GetReferencePath()

std::vector< geoops::Waypoint > controllers::PurePursuitController::GetReferencePath

(

)

const

Get the Reference Path object.

Returns

std::vector<geoops::Waypoint> - The current reference path.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        return m_vReferencePath;
    }

GetLookaheadDistance()

double controllers::PurePursuitController::GetLookaheadDistance

(

)

const

Get the Lookahead Distance object.

Returns

double - The current lookahead distance.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        return m_dLookaheadDistance;
    }

GetReferencePathTargetIndex()

int controllers::PurePursuitController::GetReferencePathTargetIndex

(

)

const

Get the Reference Path Target Index object.

Returns

int - The current index of the reference path.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        return m_nCurrentReferencePathTargetIndex;
    }

FindClosestWaypointIndex()

int controllers::PurePursuitController::FindClosestWaypointIndex ( const geoops::UTMCoordinate &  stCurrentPosition )

private

Finds the closest waypoint index in the path to the current position, enforcing topological blinders to prevent jumping across overlapping paths.

Parameters

stCurrentPosition

- The current UTM coordinate of the rover.

Returns

int - The index of the closest waypoint.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        if (m_vReferencePath.empty())
        {
            return 0;
        }
 
        int nBestSegmentIndex     = m_nCurrentReferencePathTargetIndex;
        double dClosestDistanceSq = std::numeric_limits<double>::max();
        int nConsecutiveIncreases = 0;
 
        // Search forward starting from the current known index
        for (size_t siIter = static_cast<size_t>(m_nCurrentReferencePathTargetIndex); siIter < m_vReferencePath.size(); ++siIter)
        {
            const geoops::UTMCoordinate& stA = m_vReferencePath[siIter].GetUTMCoordinate();
 
            double dDeltaX                   = stCurrentPosition.dEasting - stA.dEasting;
            double dDeltaY                   = stCurrentPosition.dNorthing - stA.dNorthing;
            double dDistSq                   = dDeltaX * dDeltaX + dDeltaY * dDeltaY;
 
            if (dDistSq < dClosestDistanceSq)
            {
                // We found a closer point. Update best and reset the blinder counter.
                dClosestDistanceSq    = dDistSq;
                nBestSegmentIndex     = static_cast<int>(siIter);
                nConsecutiveIncreases = 0;
            }
            else
            {
                nConsecutiveIncreases++;
 
                // TOPOLOGICAL BLINDER:
                // If the distance increases for N consecutive points, we have found the local minimum.
                // This stops the search so we don't accidentally jump to the adjacent ring of a spiral.
                if (nConsecutiveIncreases >= m_nLookaheadIndex)
                {
                    break;
                }
            }
        }
 
        return nBestSegmentIndex;
    }

Here is the caller graph for this function:

FindLookaheadWaypoint()

geoops::Waypoint controllers::PurePursuitController::FindLookaheadWaypoint ( const geoops::UTMCoordinate &  stCurrentPosition )

private

Searches forward from the current target index to find a point that is at least the lookahead distance away from the rover.

Parameters

stCurrentPosition

- The current UTM coordinate of the rover.

Returns

geoops::Waypoint - The target waypoint to drive towards.

Author

Sam Hajdukiewicz (saman.nosp@m.thah.nosp@m.ajduk.nosp@m.iewi.nosp@m.cz@gm.nosp@m.ail..nosp@m.com)

Date

2026-03-15

    {
        const size_t siWaypoints = m_vReferencePath.size();
 
        // Calculate the distance from the rover to the TRUE closest point on the path
        const geoops::UTMCoordinate& stClosest = m_vReferencePath[m_nCurrentReferencePathTargetIndex].GetUTMCoordinate();
        double dClosestDist                    = std::hypot(stClosest.dEasting - stCurrentPosition.dEasting, stClosest.dNorthing - stCurrentPosition.dNorthing);
 
        // OFF-PATH RECOVERY:
        // If the closest point on the path is further away than your lookahead circle,
        // no point ahead will be strictly < L. To prevent returning a point behind the rover,
        // we must target the closest point to pull the rover back onto the path.
        if (dClosestDist > m_dLookaheadDistance)
        {
            return m_vReferencePath[m_nCurrentReferencePathTargetIndex];
        }
 
        // FIND INTERSECTION AHEAD:
        // Because the closest point is guaranteed to be < L (from the check above),
        // searching forward guarantees we start INSIDE the circle.
        // The first point we find that is >= L represents exiting the circle AHEAD of the rover.
        for (size_t siIter = m_nCurrentReferencePathTargetIndex; siIter < siWaypoints; ++siIter)
        {
            const geoops::UTMCoordinate& stTarget = m_vReferencePath[siIter].GetUTMCoordinate();
 
            double dDeltaX                        = stTarget.dEasting - stCurrentPosition.dEasting;
            double dDeltaY                        = stTarget.dNorthing - stCurrentPosition.dNorthing;
            double dDistance                      = std::hypot(dDeltaX, dDeltaY);
 
            // The moment we cross the lookahead radius boundary, return that point.
            if (dDistance >= m_dLookaheadDistance)
            {
                return m_vReferencePath[siIter];
            }
        }
 
        // If no point is far enough ahead (we are at the end of the path), return the very last point.
        return m_vReferencePath.back();
    }

Here is the caller graph for this function:

---

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

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