Class NavFn

Class Documentation

class NavFn

Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.

Public Functions

NavFn(int nx, int ny)

Constructs the planner.

Parameters:

  • nx – The x size of the map

  • ny – The y size of the map

~NavFn()
void setNavArr(int nx, int ny)

Sets or resets the size of the map.

Parameters:

  • nx – The x size of the map

  • ny – The y size of the map

void setCostmap(const COSTTYPE *cmap, bool isROS = true, bool allow_unknown = true)

Set up the cost array for the planner, usually from ROS.

Parameters:

  • cmap – The costmap

  • isROS – Whether or not the costmap is coming in in ROS format

  • allow_unknown – Whether or not the planner should be allowed to plan through unknown space

bool calcNavFnAstar(std::function<bool()> cancelChecker)

Calculates a plan using the A* heuristic, returns true if one is found.

Parameters:

cancelChecker – Function to check if the task has been canceled

Returns:

True if a plan is found, false otherwise

bool calcNavFnDijkstra(std::function<bool()> cancelChecker, bool atStart = false)

Calculates the full navigation function using Dijkstra.

Parameters:

cancelChecker – Function to check if the task has been canceled

float *getPathX()

Accessor for the x-coordinates of a path.

Returns:

The x-coordinates of a path

float *getPathY()

Accessor for the y-coordinates of a path.

Returns:

The y-coordinates of a path

int getPathLen()

Accessor for the length of a path.

Returns:

The length of a path, 0 if not found

float getLastPathCost()

Gets the cost of the path found the last time a navigation function was computed.

Returns:

The cost of the last path found

void setGoal(int *goal)

Sets the goal position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start.

goal and start positions

Parameters:

goal – the goal position

void setStart(int *start)

Sets the start position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start.

Parameters:

start – the start position

void initCost(int k, float v)

Initialize cell k with cost v for propagation.

Parameters:

  • k – the cell to initialize

  • v – the cost to give to the cell

void updateCell(int n)

Updates the cell at index n.

propagation

Parameters:

n – The index to update

void updateCellAstar(int n)

Updates the cell at index n using the A* heuristic.

Parameters:

n – The index to update

void setupNavFn(bool keepit = false)

Set up navigation potential arrays for new propagation.

Parameters:

keepit – whether or not use COST_NEUTRAL

bool propNavFnDijkstra(int cycles, std::function<bool()> cancelChecker, bool atStart = false)

Run propagation for <cycles> iterations, or until start is reached using breadth-first Dijkstra method.

Parameters:

  • cycles – The maximum number of iterations to run for

  • cancelChecker – Function to check if the task has been canceled

  • atStart – Whether or not to stop when the start point is reached

Returns:

true if the start point is reached

bool propNavFnAstar(int cycles, std::function<bool()> cancelChecker)

Run propagation for <cycles> iterations, or until start is reached using the best-first A* method with Euclidean distance heuristic.

Parameters:

  • cycles – The maximum number of iterations to run for

  • cancelChecker – Function to check if the task has been canceled

Returns:

true if the start point is reached

int calcPath(int n, int *st = NULL)

Calculates the path for at mose <n> cycles.

Parameters:

n – The maximum number of cycles to run for

Returns:

The lenght of the path found, 0 if none

float gradCell(int n)

Calculate gradient at a cell.

Parameters:

n – Cell number <n>

Returns:

float norm calculates gradient at cell <n>, returns norm

Public Members

int nx
int ny
int ns

size of grid, in pixels

COSTTYPE *costarr

cell arrays cost array in 2D configuration space

float *potarr

potential array, navigation function potential

bool *pending

pending cells during propagation

int nobs

number of obstacle cells

int *pb1

block priority buffers

int *pb2
int *pb3

storage buffers for priority blocks

int *curP
int *nextP
int *overP

priority buffer block ptrs

int curPe
int nextPe
int overPe

end points of arrays

float curT

block priority thresholds current threshold

float priInc

priority threshold increment number of cycles between checks for cancellation

int goal[2]
int start[2]
float *gradx

gradient and paths

float *grady

gradient arrays, size of potential array

float *pathx
float *pathy

path points, as subpixel cell coordinates

int npath

number of path points

int npathbuf

size of pathx, pathy buffers

float last_path_cost_

Holds the cost of the path found the last time A* was called

float pathStep

step size for following gradient

Public Static Attributes

static constexpr int terminal_checking_interval = 5000