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

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

Returns:

True if a plan is found, false otherwise

bool calcNavFnDijkstra(bool atStart = false)

Caclulates the full navigation function using Dijkstra.

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, 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

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

Returns:

true if the start point is reached

bool propNavFnAstar(int cycles)

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

Returns:

true if the start point is reached returns true if start point found

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

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