pathplanners::GeoPlanner Class Reference

This class implements a geospatial path planner that uses a discrete 2.5D Costmap and a fast Weighted A* algorithm with Kinematic Constraints to find the optimal path through complex terrain between two coordinates. All tunable heuristics and parameters are accessible via constructor initialization. More...

#include <GeoPlanner.h>

Collaboration diagram for pathplanners::GeoPlanner:

Classes
struct	GridCell
	Represents a single cell in our 2.5D discretized Costmap grid. This is used to aggregate sparse point cloud data into a unified, easy-to-search surface for the A* algorithm. More...
struct	PlannerState
	This struct represents the state of a node in the A* algorithm. Optimized to work with a flat 1D vector instead of a hash map to prevent heavy heap allocations and memory fragmentation. More...
struct	PlannerStateCompare
	This struct is used to compare two PlannerState objects based on their cost to properly sort the priority queue. More...
struct	TileKey
	This struct represents a tile key in the implicit graph representation. We divide the plan into a regular grid of tiles of size dTileSize*dTileSize meters. More...
struct	TileKeyEqual
	This struct is used to compare two TileKey objects for equality after a hash collision is triggered. More...
struct	TileKeyHash
	This struct is used to hash TileKey objects for use in unordered maps. More...

Public Member Functions
	GeoPlanner (double dTileSize=50.0, double dGridResolution=0.5, double dHeuristicWeight=1.5, double dBetaBias=1.0, double dMinTravScore=0.0, int nDilationPasses=2, double dSafeTravScoreThreshold=0.5, int nMaxSpiralSearchRadius=20, size_t siMaxPlotPointsPerTile=10, double dPenaltyScalingFactor=10.0, double dPenaltyPower=2.0, double dPathWaypointTolerance=0.5)
	Construct a new Geo Planner:: Geo Planner object. Initializes all complex algorithms and mathematical hyperparameters to avoid explicitly hardcoded magic numbers logic.
	~GeoPlanner ()
	Destroy the Geo Planner:: Geo Planner object.
std::vector< geoops::Waypoint >	PlanPath (LiDARHandler *pLiDARHandler, const geoops::UTMCoordinate &stStart, const geoops::UTMCoordinate &stEnd, double dSearchRadius=3.0, double dMaxSearchTimeSeconds=120.0, double dCorridorPadding=100.0)
	Plan an optimal trajectory path from the start UTM to the end UTM coordinate utilizing a 2.5D hierarchical Costmap grid representation.
void	ClearGeoCache ()
	Explicitly zeroes and reclaims internal spatial database mapping arrays.
void	UnloadLiDARTiles (double minX, double maxX, double minY, double maxY)
	Unload tile LiDAR data.
void	SetTileSize (double dTileSize)
	Sets the dimensional size of database tiles mapped during planning.
void	SetMinTravScore (double dMinTravScore)
	Sets the absolute minimum travel score required for a valid cell.
void	SetBetaBias (double dBetaBias)
	Sets the algorithm's penalty sensitivity bias multiplier.
double	GetTileSize () const
	Retrieves the currently configured database tile size constraint.
double	GetMinTravScore () const
	Retrieves the actively configured minimum travel score parameter limit.
double	GetBetaBias () const
	Retrieves the beta heuristic bias factor configured dynamically.

Private Member Functions
bool	PreloadCorridorAndBuildGrid (const geoops::UTMCoordinate &stStart, const geoops::UTMCoordinate &stEnd)
	Calculates a bounding box based on Start and End coordinates, preloads LiDAR chunks, and structures them down into a contiguous 1D Costmap vector.
void	SearchAStar ()
	Actively runs a highly optimized Weighted A* pathfinding search upon the 1D Costmap grid utilizing kinematic 3D spatial awareness.
std::vector< geoops::Waypoint >	ReconstructPath () const
	Transforms the abstract 1D computational grid configurations logically backward to rebuild a continuous stream of actionable UTM Geographic sequences.
void	CheckAndLoadTile (int nTileX, int nTileY)
	Checks if a specific tile is loaded in the cache, and if not, fetches the relevant LiDAR point cloud data into memory.
void	FillGridHoles ()
	Performs morphological dilation on the costmap to fill in structural voids or gaps caused by sparse LiDAR data collections.
int	FindNearestValidCell (int nStartIndex) const
	Expands outward concentrically from a target grid cell to locate the nearest neighboring cell that contains valid and safe traversal structures.
double	EuclideanDistance (double dEasting1, double dNorthing1, double dEasting2, double dNorthing2) const
	Calculates the standard 2D Euclidean distance between two geographic coordinates. This establishes mathematically admissible baseline heuristic bounds for A*.
int	GetGridIndex (int nX, int nY) const
	Inline helper to convert 2D grid coordinates to a 1D vector index.
void	GetGridCoords (int nIndex, int &nX, int &nY) const
	Inline helper to convert a 1D vector index back into 2D grid coordinates.

Private Attributes
const std::function< void(const rovecomm::RoveCommPacket< float > &, const sockaddr_in &)>	fnMinTravScoreCallback
	Callback function used to set the minimum travel score for path planning.
const std::function< void(const rovecomm::RoveCommPacket< float > &, const sockaddr_in &)>	fnBetaBiasCallback
	Callback function used to set the beta bias for travel scores in path planning.
double	m_dTileSize
double	m_dGridResolution
double	m_dHeuristicWeight
double	m_dBeta
double	m_dMinTravScore
int	m_nDilationPasses
double	m_dSafeTravScoreThreshold
int	m_nMaxSpiralSearchRadius
size_t	m_siMaxPlotPointsPerTile
double	m_dPenaltyScalingFactor
double	m_dPenaltyPower
double	m_dPathWaypointTolerance
double	m_dSearchRadius
double	m_dMaxSearchTimeSeconds
double	m_dCorridorPadding
LiDARHandler *	m_pLiDARHandler
std::mutex	m_muPathGenMutex
double	m_dGridOriginEasting
double	m_dGridOriginNorthing
int	m_nGridWidth
int	m_nGridHeight
int	m_nStartIndex
int	m_nEndIndex
std::vector< GridCell >	m_vCostmap
std::priority_queue< PlannerState, std::vector< PlannerState >, PlannerStateCompare >	m_pqOpenSetNextBest
std::vector< int >	m_vPredecessors
std::vector< bool >	m_vbClosedSet
std::vector< double >	m_vdGCosts
std::unordered_map< TileKey, std::vector< LiDARHandler::PointRow >, TileKeyHash, TileKeyEqual >	m_umTileMapCache

Detailed Description

This class implements a geospatial path planner that uses a discrete 2.5D Costmap and a fast Weighted A* algorithm with Kinematic Constraints to find the optimal path through complex terrain between two coordinates. All tunable heuristics and parameters are accessible via constructor initialization.

Author

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

Date

2025-07-14

Constructor & Destructor Documentation

GeoPlanner()

pathplanners::GeoPlanner::GeoPlanner	(	double	dTileSize = 50.0,
		double	dGridResolution = 0.5,
		double	dHeuristicWeight = 1.5,
		double	dBetaBias = 1.0,
		double	dMinTravScore = 0.0,
		int	nDilationPasses = 2,
		double	dSafeTravScoreThreshold = 0.5,
		int	nMaxSpiralSearchRadius = 20,
		size_t	siMaxPlotPointsPerTile = 10,
		double	dPenaltyScalingFactor = 10.0,
		double	dPenaltyPower = 2.0,
		double	dPathWaypointTolerance = 0.5
	)

Construct a new Geo Planner:: Geo Planner object. Initializes all complex algorithms and mathematical hyperparameters to avoid explicitly hardcoded magic numbers logic.

Parameters

dTileSize	- The size of each database tile block loaded in meters.
dGridResolution	- Metric scale of an individual discrete costmap cell.
dHeuristicWeight	- The A* bias weight to accelerate goal seeking behaviors.
dBetaBias	- Baseline beta algorithmic multiplier for penalizing bad terrain.
dMinTravScore	- Absolute required lower bound on traversal values to be considered usable pathing terrain.
nDilationPasses	- Number of morphological loop passes executed to fill void structures in sparse point clouds.
dSafeTravScoreThreshold	- Baseline score requirement applied when attempting to snap stray origin coordinates.
nMaxSpiralSearchRadius	- Max concentric rings expanded when searching for safe origin snapping structures.
siMaxPlotPointsPerTile	- Rendering density constraint to prevent visualizer overload.
dPenaltyScalingFactor	- Multiplier intensifying standard penalty math strictly during node score resolution.
dPenaltyPower	- Exponential scaler applied universally to mathematically discourage steep or dangerous zones.
dPathWaypointTolerance	- Native radius value embedded into actively returned planned telemetry sequence nodes.

Author

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

Date

2025-09-24

    {
        // Initialize member variables from constructor arguments.
        m_dTileSize               = dTileSize;
        m_dGridResolution         = dGridResolution;
        m_dHeuristicWeight        = dHeuristicWeight;
        m_dBeta                   = dBetaBias;
        m_dMinTravScore           = dMinTravScore;
        m_nDilationPasses         = nDilationPasses;
        m_dSafeTravScoreThreshold = dSafeTravScoreThreshold;
        m_nMaxSpiralSearchRadius  = nMaxSpiralSearchRadius;
        m_siMaxPlotPointsPerTile  = siMaxPlotPointsPerTile;
        m_dPenaltyScalingFactor   = dPenaltyScalingFactor;
        m_dPenaltyPower           = dPenaltyPower;
        m_dPathWaypointTolerance  = dPathWaypointTolerance;
 
        // Initialize resource pointers and request-specific variables.
        m_pLiDARHandler         = nullptr;
        m_dSearchRadius         = 0.0;
        m_dMaxSearchTimeSeconds = 0.0;
        m_dCorridorPadding      = 0.0;
 
        // Bind RoveComm UDP Node network callbacks if available.
        if (network::g_pRoveCommUDPNode != nullptr)
        {
            network::g_pRoveCommUDPNode->AddUDPCallback<float>(fnMinTravScoreCallback, manifest::Autonomy::COMMANDS.find("SETMINTRAVSCORE")->second.DATA_ID);
            network::g_pRoveCommUDPNode->AddUDPCallback<float>(fnBetaBiasCallback, manifest::Autonomy::COMMANDS.find("SETBETABIAS")->second.DATA_ID);
        }
 
        LOG_INFO(logging::g_qSharedLogger, "GeoPlanner initialized successfully with a defined tile fetch size of {} meters.", std::to_string(m_dTileSize));
    }

~GeoPlanner()

pathplanners::GeoPlanner::~GeoPlanner

(

)

Destroy the Geo Planner:: Geo Planner object.

Author

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

Date

2025-09-27

    {
        // Smart pointers and standard containers clean themselves up automatically.
    }

Member Function Documentation

PlanPath()

std::vector< geoops::Waypoint > pathplanners::GeoPlanner::PlanPath	(	LiDARHandler *	pLiDARHandler,
		const geoops::UTMCoordinate &	stStart,
		const geoops::UTMCoordinate &	stEnd,
		double	dSearchRadius = 3.0,
		double	dMaxSearchTimeSeconds = 120.0,
		double	dCorridorPadding = 100.0
	)

Plan an optimal trajectory path from the start UTM to the end UTM coordinate utilizing a 2.5D hierarchical Costmap grid representation.

Parameters

pLiDARHandler	- Pointer to the LiDARHandler database instance for fetching geospatial data.
stStart	- The desired starting UTM geographic coordinate representation.
stEnd	- The ultimate destination UTM geographic coordinate representation.
dSearchRadius	- Padding base radius used to compute bounds.
dMaxSearchTimeSeconds	- The absolute maximum CPU time in seconds allocated to search attempts.
dCorridorPadding	- The extended corridor buffer dimension appended dynamically outside raw bounds.

Returns

std::vector<geoops::Waypoint> - The sequentially planned traversal path configurations.

Author

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

Date

2025-09-28

    {
        // Acquire a thread mutex lock to prevent concurrent path planning operations from corrupting internal state.
        std::lock_guard<std::mutex> lkPathLock(m_muPathGenMutex);
 
        // Secure external variables into local class state for this search pass.
        m_pLiDARHandler         = pLiDARHandler;
        m_dSearchRadius         = dSearchRadius;
        m_dMaxSearchTimeSeconds = dMaxSearchTimeSeconds;
        m_dCorridorPadding      = dCorridorPadding;
 
        LOG_NOTICE(logging::g_qSharedLogger,
                   "Starting GeoPlanner path planning from ({:.2f}, {:.2f}) to ({:.2f}, {:.2f}) with algorithmic beta: {}, minimum score threshold: {}.",
                   stStart.dEasting,
                   stStart.dNorthing,
                   stEnd.dEasting,
                   stEnd.dNorthing,
                   m_dBeta,
                   m_dMinTravScore);
 
        // Sanity check the beta multiplier to avoid dividing by zero or disabling penalty tracking.
        if (m_dBeta <= 0.0)
        {
            m_dBeta = 0.001;
            LOG_WARNING(logging::g_qSharedLogger, "GeoPlanner: supplied dBeta bias {} is mathematically invalid; utilizing fallback logic 0.001.", m_dBeta);
        }
 
        // Store execution start time to profile algorithmic bottlenecks.
        std::chrono::time_point<std::chrono::high_resolution_clock> tmStartTime = std::chrono::high_resolution_clock::now();
 
        // Step 1: Preload the bounding box tiles and construct the dense high-res abstract costmap grid matrices.
        if (!this->PreloadCorridorAndBuildGrid(stStart, stEnd))
        {
            LOG_ERROR(logging::g_qSharedLogger, "Failed to initialize the search grid. Aborting pathfinding routine.");
            return {};
        }
 
        std::chrono::time_point<std::chrono::high_resolution_clock> tmAfterInit = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> dInitDurationSeconds                      = tmAfterInit - tmStartTime;
        LOG_INFO(logging::g_qSharedLogger, "GeoPlanner Grid Generation phase mapped within {:.6f} seconds.", dInitDurationSeconds.count());
 
        // Step 2: Formally run the highly optimized Weighted A* search logic.
        this->SearchAStar();
 
        std::chrono::time_point<std::chrono::high_resolution_clock> tmAfterSearch = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> dSearchDurationSeconds                      = tmAfterSearch - tmAfterInit;
        LOG_INFO(logging::g_qSharedLogger, "GeoPlanner A* Grid Node Expansion executed within {:.6f} seconds.", dSearchDurationSeconds.count());
 
        // Step 3: Integrate and rebuild exact geographic sequences tracking backwards from the goal.
        std::vector<geoops::Waypoint> vPath                                   = this->ReconstructPath();
 
        std::chrono::time_point<std::chrono::high_resolution_clock> tmEndTime = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> dOverallDurationSeconds                 = tmEndTime - tmStartTime;
        LOG_NOTICE(logging::g_qSharedLogger,
                   "GeoPlanner total end-to-end path routing executed within {:.6f} seconds rendering {} waypoints.",
                   dOverallDurationSeconds.count(),
                   vPath.size());
 
        return vPath;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

ClearGeoCache()

void pathplanners::GeoPlanner::ClearGeoCache

(

)

Explicitly zeroes and reclaims internal spatial database mapping arrays.

Author

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

Date

2025-09-27

    {
        // Acquire a thread mutex lock to guarantee safety during cache wipes.
        std::lock_guard<std::mutex> lkResourceLock(m_muPathGenMutex);
        m_umTileMapCache.clear();
    }

UnloadLiDARTiles()

void pathplanners::GeoPlanner::UnloadLiDARTiles	(	double	minX,
		double	maxX,
		double	minY,
		double	maxY
	)

Unload tile LiDAR data.

Parameters

minX	- Minimum x coordinate of tile range.
maxX	- Maximum x coordinate of tile range.
minY	- Minimum y coordinate of tile range.
maxY	- Maximum y coordinate of tile range.

Author

Sam Nolte (samno.nosp@m.lte0.nosp@m.302@g.nosp@m.mail.nosp@m..com)

Date

2025-03-01

    {
        int nMinTileX = static_cast<int>(std::floor(minX / m_dTileSize));
        int nMinTileY = static_cast<int>(std::floor(minY / m_dTileSize));
        int nMaxTileX = static_cast<int>(std::floor(maxX / m_dTileSize));
        int nMaxTileY = static_cast<int>(std::floor(maxY / m_dTileSize));
 
        std::list<TileKey> tileKeys;
        for (int i = 0; i < nMaxTileX - nMinTileX + 1; ++i)
            for (int j = 0; j < nMaxTileY - nMinTileY + 1; ++j)
                tileKeys.push_back(TileKey{nMinTileX + i, nMinTileY + j});
 
        for (std::list<TileKey>::iterator it = tileKeys.begin(); it != tileKeys.end(); ++it)
        {
            // Make sure tile is actually loaded
            if (m_umTileMapCache.find(*it) == m_umTileMapCache.end())
            {
                continue;
            }
 
            m_umTileMapCache.erase(*it);
        }
    }

SetTileSize()

void pathplanners::GeoPlanner::SetTileSize

(

double

dTileSize

)

Sets the dimensional size of database tiles mapped during planning.

Parameters

dTileSize

- The block size in meters.

Author

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

Date

2025-07-14

    {
        m_dTileSize = dTileSize;
    }

SetMinTravScore()

void pathplanners::GeoPlanner::SetMinTravScore

(

double

dMinTravScore

)

Sets the absolute minimum travel score required for a valid cell.

Parameters

dMinTravScore

- Minimum permissible score limit.

Author

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

Date

2025-07-14

    {
        m_dMinTravScore = dMinTravScore;
    }

SetBetaBias()

void pathplanners::GeoPlanner::SetBetaBias

(

double

dBetaBias

)

Sets the algorithm's penalty sensitivity bias multiplier.

Parameters

dBetaBias

- Baseline beta algorithmic multiplier.

Author

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

Date

2025-07-14

    {
        m_dBeta = dBetaBias;
    }

GetTileSize()

double pathplanners::GeoPlanner::GetTileSize

(

)

const

Retrieves the currently configured database tile size constraint.

Returns

double - Tile size mapped dimension in meters.

Author

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

Date

2025-07-14

    {
        return m_dTileSize;
    }

GetMinTravScore()

double pathplanners::GeoPlanner::GetMinTravScore

(

)

const

Retrieves the actively configured minimum travel score parameter limit.

Returns

double - The active lowest traversal score permissible.

Author

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

Date

2025-07-14

    {
        return m_dMinTravScore;
    }

GetBetaBias()

double pathplanners::GeoPlanner::GetBetaBias

(

)

const

Retrieves the beta heuristic bias factor configured dynamically.

Returns

double - Scaler factor penalizing bad terrain paths mathematically.

Author

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

Date

2025-07-14

    {
        return m_dBeta;
    }

PreloadCorridorAndBuildGrid()

bool pathplanners::GeoPlanner::PreloadCorridorAndBuildGrid ( const geoops::UTMCoordinate &  stStart, const geoops::UTMCoordinate &  stEnd  )

private

Calculates a bounding box based on Start and End coordinates, preloads LiDAR chunks, and structures them down into a contiguous 1D Costmap vector.

Parameters

stStart	- The designated starting UTM geographic coordinate.
stEnd	- The designated end UTM geographic coordinate.

Returns

true - Grid initialization succeeded.

false - Total structural invalidation or no usable terrain available.

Author

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

Date

2025-07-14

    {
        // Buffer the search bounds to allow the algorithm lateral space to circumvent large topographic obstructions.
        double dPaddingMeters = m_dSearchRadius + m_dCorridorPadding;
 
        // Establish spatial bounding box limits outlining the entire search corridor.
        double dMinEasting  = std::min(stStart.dEasting, stEnd.dEasting) - dPaddingMeters;
        double dMaxEasting  = std::max(stStart.dEasting, stEnd.dEasting) + dPaddingMeters;
        double dMinNorthing = std::min(stStart.dNorthing, stEnd.dNorthing) - dPaddingMeters;
        double dMaxNorthing = std::max(stStart.dNorthing, stEnd.dNorthing) + dPaddingMeters;
 
        // Compute the tile keys needed to cover the bounding box.
        int nMinTileX = static_cast<int>(std::floor(dMinEasting / m_dTileSize));
        int nMaxTileX = static_cast<int>(std::floor(dMaxEasting / m_dTileSize));
        int nMinTileY = static_cast<int>(std::floor(dMinNorthing / m_dTileSize));
        int nMaxTileY = static_cast<int>(std::floor(dMaxNorthing / m_dTileSize));
 
        // Preload all valid database tiles within this geographic block.
        for (int nX = nMinTileX; nX <= nMaxTileX; ++nX)
        {
            for (int nY = nMinTileY; nY <= nMaxTileY; ++nY)
            {
                this->CheckAndLoadTile(nX, nY);
            }
        }
 
        // Establish the matrix dimensions required for the granular abstract grid.
        m_dGridOriginEasting  = dMinEasting;
        m_dGridOriginNorthing = dMinNorthing;
        m_nGridWidth          = static_cast<int>(std::ceil((dMaxEasting - dMinEasting) / m_dGridResolution));
        m_nGridHeight         = static_cast<int>(std::ceil((dMaxNorthing - dMinNorthing) / m_dGridResolution));
 
        // Pre-allocate the master costmap vector memory capacity and initialize to empty state (-1.0).
        int nTotalCells = m_nGridWidth * m_nGridHeight;
        m_vCostmap.assign(nTotalCells, GridCell());
 
        // Overlay raw sparse LiDAR matrices onto the structured grid mapping.
        for (int nX = nMinTileX; nX <= nMaxTileX; ++nX)
        {
            for (int nY = nMinTileY; nY <= nMaxTileY; ++nY)
            {
                TileKey stKey{nX, nY};
 
                if (m_umTileMapCache.find(stKey) != m_umTileMapCache.end())
                {
                    for (const LiDARHandler::PointRow& stPoint : m_umTileMapCache[stKey])
                    {
                        int nGridX = static_cast<int>((stPoint.dEasting - m_dGridOriginEasting) / m_dGridResolution);
                        int nGridY = static_cast<int>((stPoint.dNorthing - m_dGridOriginNorthing) / m_dGridResolution);
 
                        // Ensure index bounds are safe before memory injection.
                        if (nGridX >= 0 && nGridX < m_nGridWidth && nGridY >= 0 && nGridY < m_nGridHeight)
                        {
                            int nIdx = GetGridIndex(nGridX, nGridY);
 
                            // Pessimistic Data Aggregation: We specifically want to take the worst (lowest) traversal score
                            // to ensure obstacles like trees are not masked by overlapping ground returns.
                            if (m_vCostmap[nIdx].dTravScore < 0.0 || stPoint.dTraversalScore < m_vCostmap[nIdx].dTravScore)
                            {
                                m_vCostmap[nIdx].dTravScore            = stPoint.dTraversalScore;
                                m_vCostmap[nIdx].dAltitude             = stPoint.dAltitude;
                                m_vCostmap[nIdx].nClosestPointID       = stPoint.nID;
                                m_vCostmap[nIdx].nZone                 = std::stoi(stPoint.szZone.substr(0, 2));
                                m_vCostmap[nIdx].bInNorthernHemisphere = (stPoint.dNorthing >= 0);
                            }
                        }
                    }
                }
            }
        }
 
        // Dilate the existing valid cells to bridge structural void gaps in sparse clouds.
        this->FillGridHoles();
 
        // Overlay dynamic obstacles from the WaypointHandler to ensure stuck state and object detection block paths dynamically in memory.
        std::vector<geoops::Waypoint> vObstacles = globals::g_pWaypointHandler->GetAllObstacles();
 
        // Loop through the obstacles.
        for (const geoops::Waypoint& stObstacle : vObstacles)
        {
            // Get obstacle UTM coordinate and radius.
            geoops::UTMCoordinate stObsUTM = stObstacle.GetUTMCoordinate();
            double dRadius                 = stObstacle.dRadius;
 
            // Convert the obstacle's UTM center to grid array coordinates.
            int nObsGridX = static_cast<int>((stObsUTM.dEasting - m_dGridOriginEasting) / m_dGridResolution);
            int nObsGridY = static_cast<int>((stObsUTM.dNorthing - m_dGridOriginNorthing) / m_dGridResolution);
 
            // Determine the bounding box of the obstacle in grid cells.
            int nRadiusCells = static_cast<int>(std::ceil(dRadius / m_dGridResolution));
 
            // Looping through the grid cells of the radius.
            for (int nDx = -nRadiusCells; nDx <= nRadiusCells; ++nDx)
            {
                for (int nDy = -nRadiusCells; nDy <= nRadiusCells; ++nDy)
                {
                    // Increase the X and Y values.
                    int nX = nObsGridX + nDx;
                    int nY = nObsGridY + nDy;
 
                    // Ensure the target indices are within valid 2D grid bounds.
                    if (nX >= 0 && nX < m_nGridWidth && nY >= 0 && nY < m_nGridHeight)
                    {
                        // Make sure that we're checking within the radius with distance formula.
                        double dDistSq = (nDx * m_dGridResolution) * (nDx * m_dGridResolution) + (nDy * m_dGridResolution) * (nDy * m_dGridResolution);
 
                        // Check if the distance is less than radius squared.
                        if (dDistSq <= (dRadius * dRadius))
                        {
                            // If so, set the grid index to a low trav score.
                            int nIdx                    = GetGridIndex(nX, nY);
                            m_vCostmap[nIdx].dTravScore = 0.01;
                        }
                    }
                }
            }
        }
 
        // Calculate starting array indices based on geographic coordinate locations.
        int nStartX = static_cast<int>((stStart.dEasting - m_dGridOriginEasting) / m_dGridResolution);
        int nStartY = static_cast<int>((stStart.dNorthing - m_dGridOriginNorthing) / m_dGridResolution);
        int nEndX   = static_cast<int>((stEnd.dEasting - m_dGridOriginEasting) / m_dGridResolution);
        int nEndY   = static_cast<int>((stEnd.dNorthing - m_dGridOriginNorthing) / m_dGridResolution);
 
        // Enforce strict clamping to prevent edge-case out of bounds index evaluation.
        nStartX = std::clamp(nStartX, 0, m_nGridWidth - 1);
        nStartY = std::clamp(nStartY, 0, m_nGridHeight - 1);
        nEndX   = std::clamp(nEndX, 0, m_nGridWidth - 1);
        nEndY   = std::clamp(nEndY, 0, m_nGridHeight - 1);
 
        // Perform a safe-snap search to ensure origin/destination points don't land exactly in a red zone or void.
        m_nStartIndex = this->FindNearestValidCell(GetGridIndex(nStartX, nStartY));
        m_nEndIndex   = this->FindNearestValidCell(GetGridIndex(nEndX, nEndY));
 
        // Abort if no viable terrain whatsoever exists near the required start/end points.
        if (m_nStartIndex == -1 || m_nEndIndex == -1)
        {
            LOG_ERROR(logging::g_qSharedLogger, "GeoPlanner explicit termination: Geographic endpoints have no safe nearby terrain data.");
            return false;
        }
 
        // Reset fast A* memory trackers to prepare for the active search loop.
        m_vPredecessors.assign(nTotalCells, -1);
        m_vbClosedSet.assign(nTotalCells, false);
        m_vdGCosts.assign(nTotalCells, std::numeric_limits<double>::infinity());
 
        // Empty the priority queue cleanly.
        while (!m_pqOpenSetNextBest.empty())
        {
            m_pqOpenSetNextBest.pop();
        }
 
        LOG_INFO(logging::g_qSharedLogger,
                 "Abstract Search Grid Built. Width {} x Height {} (Total: {} cells). Start Index: {}, End Index: {}",
                 m_nGridWidth,
                 m_nGridHeight,
                 nTotalCells,
                 m_nStartIndex,
                 m_nEndIndex);
        return true;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

SearchAStar()

void pathplanners::GeoPlanner::SearchAStar ( )

private

Actively runs a highly optimized Weighted A* pathfinding search upon the 1D Costmap grid utilizing kinematic 3D spatial awareness.

Author

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

Date

2025-07-14

    {
        std::chrono::high_resolution_clock::time_point tmStartTime = std::chrono::high_resolution_clock::now();
 
        // Initialize the origin node states.
        PlannerState stStartState;
        stStartState.nGridIndex = m_nStartIndex;
        stStartState.dGCost     = 0.0;
 
        int nEndX, nEndY;
        GetGridCoords(m_nEndIndex, nEndX, nEndY);
 
        // Compute baseline Euclidean heuristic scaled up by heuristic weight.
        int nStartX, nStartY;
        GetGridCoords(m_nStartIndex, nStartX, nStartY);
        stStartState.dHCost =
            m_dHeuristicWeight * EuclideanDistance(nStartX * m_dGridResolution, nStartY * m_dGridResolution, nEndX * m_dGridResolution, nEndY * m_dGridResolution);
 
        m_vdGCosts[m_nStartIndex] = 0.0;
        m_pqOpenSetNextBest.push(stStartState);
 
        // Pre-computed lookup tables for rapid 8-way directional neighbor evaluation.
        constexpr int anDx[8]          = {-1, 0, 1, -1, 1, -1, 0, 1};
        constexpr int anDy[8]          = {-1, -1, -1, 0, 0, 1, 1, 1};
        constexpr double adMoveDist[8] = {1.414, 1.0, 1.414, 1.0, 1.0, 1.414, 1.0, 1.414};
 
        // Main algorithm frontier expansion loop.
        while (!m_pqOpenSetNextBest.empty())
        {
            // Acquire the lowest cost node currently located in the Min-Heap.
            PlannerState stCurrentState = m_pqOpenSetNextBest.top();
            m_pqOpenSetNextBest.pop();
 
            // Ignore stale states that were updated with a better path later in the queue.
            if (stCurrentState.dGCost > m_vdGCosts[stCurrentState.nGridIndex])
            {
                continue;
            }
 
            // Immediately exit standard operations if the target goal coordinate was successfully reached.
            if (stCurrentState.nGridIndex == m_nEndIndex)
            {
                LOG_INFO(logging::g_qSharedLogger, "Successfully reached valid goal configuration parameter during A* expansions.");
                return;
            }
 
            // Mark this specific node index as fully evaluated.
            m_vbClosedSet[stCurrentState.nGridIndex] = true;
 
            int nCurrentX, nCurrentY;
            GetGridCoords(stCurrentState.nGridIndex, nCurrentX, nCurrentY);
 
            // Explore all 8 adjacent neighbor grid cells.
            for (int nI = 0; nI < 8; ++nI)
            {
                int nNeighborX = nCurrentX + anDx[nI];
                int nNeighborY = nCurrentY + anDy[nI];
 
                // Block bounds queries outside of the physical map.
                if (nNeighborX < 0 || nNeighborX >= m_nGridWidth || nNeighborY < 0 || nNeighborY >= m_nGridHeight)
                {
                    continue;
                }
 
                int nNeighborIdx = GetGridIndex(nNeighborX, nNeighborY);
 
                // Skip indices that have already been cleanly solved.
                if (m_vbClosedSet[nNeighborIdx])
                {
                    continue;
                }
 
                // Reference cell parameters.
                const GridCell& stCell        = m_vCostmap[nNeighborIdx];
                const GridCell& stCurrentCell = m_vCostmap[stCurrentState.nGridIndex];
 
                // Ensure neighbor inherently contains registered geographic parameters and meets minimum score limits.
                if (stCell.dTravScore < 0.0 || stCell.dTravScore < m_dMinTravScore)
                {
                    continue;
                }
 
                // --- KINEMATIC AWARENESS LOGIC ---
                // Calculate actual true 3D spatial distance incorporating the vertical ascent parameters.
                double dAltDiff    = std::abs(stCell.dAltitude - stCurrentCell.dAltitude);
                double dPlanarDist = adMoveDist[nI] * m_dGridResolution;
                double dTrueDist   = std::sqrt((dPlanarDist * dPlanarDist) + (dAltDiff * dAltDiff));
 
                // Constrain traversal metrics rigidly to [0,1] bounds simply for mathematical safety.
                double dScore = std::clamp(stCell.dTravScore, 0.0, 1.0);
 
                // Generate an exponential scaling multiplier. Low traversal scores (e.g., rigid trees or sharp canyons) are
                // inflated heavily. Empowering the multiplier ensures aggressive physical avoidance of high penalties.
                double dPenaltyWeight = std::pow(1.0 - dScore, m_dPenaltyPower);
                double dMultiplier    = 1.0 + (m_dBeta * m_dPenaltyScalingFactor * dPenaltyWeight);
 
                // Compute exact, penalized aggregate traversal path cost.
                double dTentativeGCost = stCurrentState.dGCost + (dTrueDist * dMultiplier);
 
                // Adopt spatial progression only if it presents a mathematically optimal routing arrangement.
                if (dTentativeGCost < m_vdGCosts[nNeighborIdx])
                {
                    m_vdGCosts[nNeighborIdx]      = dTentativeGCost;
                    m_vPredecessors[nNeighborIdx] = stCurrentState.nGridIndex;
 
                    // Package mathematical layout elements for injection to the heap bounds queue.
                    PlannerState stNeighborState;
                    stNeighborState.nGridIndex = nNeighborIdx;
                    stNeighborState.dGCost     = dTentativeGCost;
                    stNeighborState.dHCost =
                        m_dHeuristicWeight *
                        EuclideanDistance(nNeighborX * m_dGridResolution, nNeighborY * m_dGridResolution, nEndX * m_dGridResolution, nEndY * m_dGridResolution);
 
                    m_pqOpenSetNextBest.push(stNeighborState);
                }
            }
 
            // Consistently measure real-world time elapsed to terminate algorithms forcibly if CPU limits are breached.
            if (std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - tmStartTime).count() >= m_dMaxSearchTimeSeconds)
            {
                LOG_WARNING(logging::g_qSharedLogger, "Grid search forcibly terminated due to exceeding the max search boundary of {} seconds.", m_dMaxSearchTimeSeconds);
                return;
            }
        }
    }

Here is the call graph for this function:

Here is the caller graph for this function:

ReconstructPath()

std::vector< geoops::Waypoint > pathplanners::GeoPlanner::ReconstructPath ( ) const

private

Transforms the abstract 1D computational grid configurations logically backward to rebuild a continuous stream of actionable UTM Geographic sequences.

Returns

std::vector<geoops::Waypoint> - Sequentially ordered map of waypoints establishing optimal path structures.

Author

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

Date

2025-07-14

    {
        std::vector<geoops::Waypoint> vPath;
 
        // Verify that the final path integration chain was actually established to the end node.
        if (m_vPredecessors[m_nEndIndex] == -1)
        {
            LOG_WARNING(logging::g_qSharedLogger, "Algorithmic resolution fault: Final path integration chain was never properly established.");
            return {};
        }
 
        int nCurrentIdx = m_nEndIndex;
 
        // Traverse the hierarchical sequence strictly backwards.
        while (nCurrentIdx != -1)
        {
            int nX, nY;
            GetGridCoords(nCurrentIdx, nX, nY);
 
            const GridCell& stCell = m_vCostmap[nCurrentIdx];
 
            // Reapply coordinate origins to map grid positions back to real-world UTM coordinates.
            double dEasting  = m_dGridOriginEasting + (nX * m_dGridResolution);
            double dNorthing = m_dGridOriginNorthing + (nY * m_dGridResolution);
 
            // Construct waypoint and append to vector.
            vPath.emplace_back(geoops::UTMCoordinate(dEasting, dNorthing, stCell.nZone, stCell.bInNorthernHemisphere, stCell.dAltitude),
                               geoops::WaypointType::eNavigationWaypoint,
                               m_dPathWaypointTolerance,
                               stCell.nClosestPointID);
 
            // Halt backtracing immediately upon intersecting initial origin index.
            if (nCurrentIdx == m_nStartIndex)
            {
                break;
            }
 
            nCurrentIdx = m_vPredecessors[nCurrentIdx];
        }
 
        // Reverse sequence to represent chronology from Start to Goal.
        std::reverse(vPath.begin(), vPath.end());
 
        return vPath;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

CheckAndLoadTile()

void pathplanners::GeoPlanner::CheckAndLoadTile ( int  nTileX, int  nTileY  )

private

Checks if a specific tile is loaded in the cache, and if not, fetches the relevant LiDAR point cloud data into memory.

Parameters

nTileX	- The exact coordinate identifier X block reference naturally mapping structurally.
nTileY	- The exact coordinate identifier Y block reference naturally mapping structurally.

Author

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

Date

2025-07-14

    {
        TileKey stTileKey{nTileX, nTileY};
 
        // Check if the tile is already present in the active cache map.
        if (m_umTileMapCache.find(stTileKey) == m_umTileMapCache.end())
        {
            // Set up point filter parameters logically outlining requested search area blocks.
            LiDARHandler::PointFilter stFilter;
            stFilter.dEasting  = (nTileX + 0.5) * m_dTileSize;
            stFilter.dNorthing = (nTileY + 0.5) * m_dTileSize;
            stFilter.dRadius   = std::sqrt(2) * (m_dTileSize / 2.0);
 
            // Retrieve the point cloud data for the tile from the LiDAR handler.
            std::vector<LiDARHandler::PointRow> vTilePoints = m_pLiDARHandler->GetLiDARData(stFilter);
 
            // Return if no points were found for the requested tile mapping boundaries.
            if (vTilePoints.empty())
            {
                return;
            }
 
            // Store the retrieved points natively into the unordered tile cache matrix.
            m_umTileMapCache[stTileKey] = std::move(vTilePoints);
        }
    }

Here is the call graph for this function:

Here is the caller graph for this function:

FillGridHoles()

void pathplanners::GeoPlanner::FillGridHoles ( )

private

Performs morphological dilation on the costmap to fill in structural voids or gaps caused by sparse LiDAR data collections.

Author

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

Date

2025-07-14

    {
        std::vector<GridCell> vNewCostmap = m_vCostmap;
        constexpr int anDx[8]             = {-1, 0, 1, -1, 1, -1, 0, 1};
        constexpr int anDy[8]             = {-1, -1, -1, 0, 0, 1, 1, 1};
 
        // Perform processing passes to iteratively stretch valid geographic bounds incrementally.
        for (int nPass = 0; nPass < m_nDilationPasses; ++nPass)
        {
            for (int nY = 0; nY < m_nGridHeight; ++nY)
            {
                for (int nX = 0; nX < m_nGridWidth; ++nX)
                {
                    int nIdx = GetGridIndex(nX, nY);
 
                    // Check if the target grid cell is currently an empty void.
                    if (m_vCostmap[nIdx].dTravScore < 0.0)
                    {
                        double dBestScore = -1.0;
                        GridCell stBestCell;
 
                        // Look at neighboring cells to find a valid traversal score to inherit.
                        for (int nI = 0; nI < 8; ++nI)
                        {
                            int nNeighborX = nX + anDx[nI];
                            int nNeighborY = nY + anDy[nI];
 
                            // Ensure neighbor indices are within valid 2D grid bounds.
                            if (nNeighborX >= 0 && nNeighborX < m_nGridWidth && nNeighborY >= 0 && nNeighborY < m_nGridHeight)
                            {
                                int nNeighborIdx = GetGridIndex(nNeighborX, nNeighborY);
                                if (m_vCostmap[nNeighborIdx].dTravScore > dBestScore)
                                {
                                    dBestScore = m_vCostmap[nNeighborIdx].dTravScore;
                                    stBestCell = m_vCostmap[nNeighborIdx];
                                }
                            }
                        }
 
                        // Inherit the best adjacent traversal score if a valid neighbor was discovered.
                        if (dBestScore >= 0.0)
                        {
                            vNewCostmap[nIdx] = stBestCell;
                        }
                    }
                }
            }
            // Update the core system array structures with the applied morphological filter outcomes.
            m_vCostmap = vNewCostmap;
        }
    }

Here is the call graph for this function:

Here is the caller graph for this function:

FindNearestValidCell()

int pathplanners::GeoPlanner::FindNearestValidCell ( int  nStartIndex ) const

private

Expands outward concentrically from a target grid cell to locate the nearest neighboring cell that contains valid and safe traversal structures.

Parameters

nStartIndex

- Origin array structural identifier originally derived from GPS.

Returns

int - Corrected viable mapping ID index, or -1 representing complete absence of viable data.

Author

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

Date

2025-07-14

    {
        // Define an explicit threshold to prevent snapping the origin to extremely hazardous geometry.
        double dSafeThreshold = std::max(m_dMinTravScore, m_dSafeTravScoreThreshold);
 
        // Verify quickly if the requested starting index is already a valid and thoroughly safe cell.
        if (nStartIndex >= 0 && nStartIndex < static_cast<int>(m_vCostmap.size()) && m_vCostmap[nStartIndex].dTravScore >= 0.0 &&
            m_vCostmap[nStartIndex].dTravScore >= dSafeThreshold)
        {
            return nStartIndex;
        }
 
        int nStartX, nStartY;
        GetGridCoords(nStartIndex, nStartX, nStartY);
 
        int nMaxRadius            = m_nMaxSpiralSearchRadius;
        int nBestFallbackIdx      = -1;
        double dBestFallbackScore = -1.0;
 
        // Spiral search radially outward tracking concentric boundary edges locally.
        for (int nRadius = 1; nRadius <= nMaxRadius; ++nRadius)
        {
            int nBestIdxInRing      = -1;
            double dBestScoreInRing = -1.0;
 
            for (int nI = -nRadius; nI <= nRadius; ++nI)
            {
                for (int nJ = -nRadius; nJ <= nRadius; ++nJ)
                {
                    // Restrict processing exclusively to parameters located directly at the current radius boundary.
                    if (std::abs(nI) == nRadius || std::abs(nJ) == nRadius)
                    {
                        int nNeighborX = nStartX + nI;
                        int nNeighborY = nStartY + nJ;
 
                        if (nNeighborX >= 0 && nNeighborX < m_nGridWidth && nNeighborY >= 0 && nNeighborY < m_nGridHeight)
                        {
                            int nIdx      = GetGridIndex(nNeighborX, nNeighborY);
                            double dScore = m_vCostmap[nIdx].dTravScore;
 
                            if (dScore >= 0.0 && dScore >= m_dMinTravScore)
                            {
                                // Track the absolute best viable layout encountered overall.
                                if (dScore > dBestFallbackScore)
                                {
                                    dBestFallbackScore = dScore;
                                    nBestFallbackIdx   = nIdx;
                                }
 
                                // Prioritize and track specific indices exceeding the optimal safe threshold.
                                if (dScore >= dSafeThreshold && dScore > dBestScoreInRing)
                                {
                                    dBestScoreInRing = dScore;
                                    nBestIdxInRing   = nIdx;
                                }
                            }
                        }
                    }
                }
            }
 
            // Immediately yield the optimal safe coordinate natively if one was discovered in this ring.
            if (nBestIdxInRing != -1)
            {
                return nBestIdxInRing;
            }
        }
 
        // Return the highest scoring viable cell found within the search limits, or -1 if no usable data exists.
        return nBestFallbackIdx;
    }

Here is the caller graph for this function:

EuclideanDistance()

double pathplanners::GeoPlanner::EuclideanDistance ( double  dEasting1, double  dNorthing1, double  dEasting2, double  dNorthing2  ) const

private

Calculates the standard 2D Euclidean distance between two geographic coordinates. This establishes mathematically admissible baseline heuristic bounds for A*.

Parameters

dEasting1	- The numeric coordinate east bounds establishing start reference.
dNorthing1	- The numeric coordinate north bounds establishing start reference.
dEasting2	- The numeric coordinate east bounds establishing end reference.
dNorthing2	- The numeric coordinate north bounds establishing end reference.

Returns

double - The calculated spatial euclidean distance dimension.

Author

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

Date

2025-07-14

    {
        // Evaluate numerical differentials mapping bounds simply and efficiently.
        double dDiffX = dEasting1 - dEasting2;
        double dDiffY = dNorthing1 - dNorthing2;
 
        return std::sqrt(dDiffX * dDiffX + dDiffY * dDiffY);
    }

Here is the caller graph for this function:

GetGridIndex()

int pathplanners::GeoPlanner::GetGridIndex ( int  nX, int  nY  ) const

inlineprivate

Inline helper to convert 2D grid coordinates to a 1D vector index.

Parameters

nX	- The X grid coordinate.
nY	- The Y grid coordinate.

Returns

int - The corresponding flat 1D vector index.

Author

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

Date

2025-07-14

{ return nY * m_nGridWidth + nX; }

Here is the caller graph for this function:

GetGridCoords()

void pathplanners::GeoPlanner::GetGridCoords ( int  nIndex, int &  nX, int &  nY  ) const

inlineprivate

Inline helper to convert a 1D vector index back into 2D grid coordinates.

Parameters

nIndex	- The 1D vector index to decode.
nX	- Reference to store the resultant X coordinate.
nY	- Reference to store the resultant Y coordinate.

Author

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

Date

2025-07-14

            {
                nY = nIndex / m_nGridWidth;
                nX = nIndex % m_nGridWidth;
            }

Here is the caller graph for this function:

Member Data Documentation

fnMinTravScoreCallback

const std::function<void(const rovecomm::RoveCommPacket<float>&, const sockaddr_in&)> pathplanners::GeoPlanner::fnMinTravScoreCallback

private

Initial value:

=
                [this](const rovecomm::RoveCommPacket<float>& stPacket, const sockaddr_in& stdAddr)
            {
                (void) stdAddr;
 
                
                if (stPacket.vData.size() > 0)
                {
                    m_dMinTravScore = static_cast<double>(stPacket.vData[0]);
                    this->ClearGeoCache();
 
                    LOG_NOTICE(logging::g_qSharedLogger,
                               "Incoming Packet: Setting GeoPlanner minimum travel score to {}. The tile cache has also been cleared.",
                               this->m_dMinTravScore);
                }
            }

Callback function used to set the minimum travel score for path planning.

Author

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

Date

2024-04-04

            {
                (void) stdAddr;
 
                // Extract minimum travel score from incoming packet.
                if (stPacket.vData.size() > 0)
                {
                    m_dMinTravScore = static_cast<double>(stPacket.vData[0]);
                    this->ClearGeoCache();
 
                    LOG_NOTICE(logging::g_qSharedLogger,
                               "Incoming Packet: Setting GeoPlanner minimum travel score to {}. The tile cache has also been cleared.",
                               this->m_dMinTravScore);
                }
            };

fnBetaBiasCallback

const std::function<void(const rovecomm::RoveCommPacket<float>&, const sockaddr_in&)> pathplanners::GeoPlanner::fnBetaBiasCallback

private

Initial value:

=
                [this](const rovecomm::RoveCommPacket<float>& stPacket, const sockaddr_in& stdAddr)
            {
                (void) stdAddr;
 
                
                if (stPacket.vData.size() > 0)
                {
                    m_dBeta = static_cast<double>(stPacket.vData[0]);
 
                    LOG_NOTICE(logging::g_qSharedLogger, "Incoming Packet: Setting GeoPlanner beta bias to {}", this->m_dBeta);
                }
            }

Callback function used to set the beta bias for travel scores in path planning.

Author

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

Date

2024-04-04

            {
                (void) stdAddr;
 
                // Extract beta bias from incoming packet.
                if (stPacket.vData.size() > 0)
                {
                    m_dBeta = static_cast<double>(stPacket.vData[0]);
 
                    LOG_NOTICE(logging::g_qSharedLogger, "Incoming Packet: Setting GeoPlanner beta bias to {}", this->m_dBeta);
                }
            };

---

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

• src/algorithms/planners/GeoPlanner.h
• src/algorithms/planners/GeoPlanner.cpp