#include <cost_interpreter.h>
Public Types | |
typedef std::shared_ptr< CostInterpreter > | Ptr |
Public Member Functions | |
float | getCost (const unsigned int x, const unsigned int y) const |
unsigned char | getNeutralCost () const |
void | initialize (ros::NodeHandle &nh, nav_core2::Costmap::Ptr costmap) |
float | interpretCost (const unsigned char cost) const |
bool | isLethal (const float cost) const |
void | setConfiguration (const unsigned char neutral_cost, const float scale, const UnknownInterpretation mode) |
Protected Attributes | |
std::array< float, 256 > | cached_costs_ |
nav_core2::Costmap::Ptr | costmap_ |
unsigned char | neutral_cost_ |
There is a fundamental tradeoff in planning between the length of the path and the costs contained in the costmap. The neutral cost is a penalty for moving from one cell to an adjacent cell. The raw values in the costmap can be weighted as well. See the README for more conceptual explanations.
Practically speaking, we take the cost for each cell (c) and generally interpret it as a float: c' = neutral_cost + scale * c
The caveat is that c' should not be greater than the the lethal cost, so non-lethal costs c with c' >= lethal_cost will be interpreted as c' = lethal_cost -1
A plot of this can be seen here: https://www.desmos.com/calculator/2haig4ki8h
The special case is when c==255, i.e. NO_INFORMATION. There are three intepretations possible here.
Definition at line 66 of file cost_interpreter.h.
typedef std::shared_ptr<CostInterpreter> dlux_global_planner::CostInterpreter::Ptr |
Definition at line 89 of file cost_interpreter.h.
|
inline |
Definition at line 79 of file cost_interpreter.h.
|
inline |
Definition at line 72 of file cost_interpreter.h.
void dlux_global_planner::CostInterpreter::initialize | ( | ros::NodeHandle & | nh, |
nav_core2::Costmap::Ptr | costmap | ||
) |
Definition at line 43 of file cost_interpreter.cpp.
|
inline |
Definition at line 74 of file cost_interpreter.h.
|
inline |
Definition at line 84 of file cost_interpreter.h.
void dlux_global_planner::CostInterpreter::setConfiguration | ( | const unsigned char | neutral_cost, |
const float | scale, | ||
const UnknownInterpretation | mode | ||
) |
Definition at line 90 of file cost_interpreter.cpp.
|
protected |
Definition at line 92 of file cost_interpreter.h.
|
protected |
Definition at line 95 of file cost_interpreter.h.
|
protected |
Definition at line 93 of file cost_interpreter.h.