Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down. More...
#include <navfn.h>
Public Member Functions | |
bool | calcNavFnAstar () |
Calculates a plan using the A* heuristic, returns true if one is found. | |
bool | calcNavFnDijkstra (bool atStart=false) |
Caclulates the full navigation function using Dijkstra. | |
int | calcPath (int n, int *st=NULL) |
Calculates the path for at mose <n> cycles. | |
void | display (void fn(NavFn *nav), int n=100) |
float | getLastPathCost () |
Gets the cost of the path found the last time a navigation function was computed. | |
int | getPathLen () |
Accessor for the length of a path. | |
float * | getPathX () |
Accessor for the x-coordinates of a path. | |
float * | getPathY () |
Accessor for the y-coordinates of a path. | |
float | gradCell (int n) |
void | initCost (int k, float v) |
Initialize cell k with cost v for propagation. | |
NavFn (int nx, int ny) | |
Constructs the planner. | |
bool | propNavFnAstar (int cycles) |
Run propagation for <cycles> iterations, or until start is reached using the best-first A* method with Euclidean distance heuristic. | |
bool | propNavFnDijkstra (int cycles, bool atStart=false) |
Run propagation for <cycles> iterations, or until start is reached using breadth-first Dijkstra method. | |
void | savemap (const char *fname) |
void | setCostmap (const COSTTYPE *cmap, bool isROS=true, bool allow_unknown=true) |
Set up the cost array for the planner, usually from ROS. | |
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. | |
void | setNavArr (int nx, int ny) |
Sets or resets the size of the map. | |
void | setObs () |
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. | |
void | setupNavFn (bool keepit=false) |
void | updateCell (int n) |
Updates the cell at index n. | |
void | updateCellAstar (int n) |
Updates the cell at index n using the A* heuristic. | |
~NavFn () | |
Public Attributes | |
COSTTYPE * | costarr |
int * | curP |
int | curPe |
float | curT |
void(* | displayFn )(NavFn *nav) |
int | displayInt |
int | goal [2] |
float * | gradx |
float * | grady |
float | last_path_cost_ |
int * | nextP |
int | nextPe |
int | nobs |
int | npath |
int | npathbuf |
int | ns |
int | nx |
int | ny |
COSTTYPE * | obsarr |
int * | overP |
int | overPe |
float | pathStep |
float * | pathx |
float * | pathy |
int * | pb1 |
int * | pb2 |
int * | pb3 |
bool * | pending |
float * | potarr |
float | priInc |
int | start [2] |
Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.
navfn::NavFn::NavFn | ( | int | nx, |
int | ny | ||
) |
bool navfn::NavFn::calcNavFnAstar | ( | ) |
bool navfn::NavFn::calcNavFnDijkstra | ( | bool | atStart = false | ) |
int navfn::NavFn::calcPath | ( | int | n, |
int * | st = NULL |
||
) |
void navfn::NavFn::display | ( | void | fnNavFn *nav, |
int | n = 100 |
||
) |
float navfn::NavFn::getLastPathCost | ( | ) |
int navfn::NavFn::getPathLen | ( | ) |
float * navfn::NavFn::getPathX | ( | ) |
float * navfn::NavFn::getPathY | ( | ) |
float navfn::NavFn::gradCell | ( | int | n | ) |
void navfn::NavFn::initCost | ( | int | k, |
float | v | ||
) |
bool navfn::NavFn::propNavFnAstar | ( | int | cycles | ) |
Run propagation for <cycles> iterations, or until start is reached using the best-first A* method with Euclidean distance heuristic.
cycles | The maximum number of iterations to run for |
bool navfn::NavFn::propNavFnDijkstra | ( | int | cycles, |
bool | atStart = false |
||
) |
Run propagation for <cycles> iterations, or until start is reached using breadth-first Dijkstra method.
cycles | The maximum number of iterations to run for |
atStart | Whether or not to stop when the start point is reached |
void navfn::NavFn::savemap | ( | const char * | fname | ) |
void navfn::NavFn::setCostmap | ( | const COSTTYPE * | cmap, |
bool | isROS = true , |
||
bool | allow_unknown = true |
||
) |
void navfn::NavFn::setGoal | ( | int * | goal | ) |
void navfn::NavFn::setNavArr | ( | int | nx, |
int | ny | ||
) |
void navfn::NavFn::setObs | ( | ) |
void navfn::NavFn::setStart | ( | int * | start | ) |
void navfn::NavFn::setupNavFn | ( | bool | keepit = false | ) |
void navfn::NavFn::updateCell | ( | int | n | ) | [inline] |
void navfn::NavFn::updateCellAstar | ( | int | n | ) | [inline] |
int* navfn::NavFn::curP |
float navfn::NavFn::curT |
void(* navfn::NavFn::displayFn)(NavFn *nav) |
int navfn::NavFn::goal[2] |
float* navfn::NavFn::gradx |
float * navfn::NavFn::grady |
int * navfn::NavFn::nextP |
int navfn::NavFn::ns |
int navfn::NavFn::nx |
int navfn::NavFn::ny |
int * navfn::NavFn::overP |
float navfn::NavFn::pathStep |
float* navfn::NavFn::pathx |
float * navfn::NavFn::pathy |
int* navfn::NavFn::pb1 |
int * navfn::NavFn::pb2 |
int * navfn::NavFn::pb3 |
bool* navfn::NavFn::pending |
float* navfn::NavFn::potarr |
float navfn::NavFn::priInc |
int navfn::NavFn::start[2] |