Classes | |
class | GenericGridPublisher |
An interface for publishing NavGridOfX/OccupancyGrid msgs and their updates periodically. More... | |
class | NavGridPublisher |
An interface for publishing NavGridOfChars/OccupancyGrid msgs and their updates periodically. More... | |
class | NavGridSubscriber |
class | ScaleGridPublisher |
An interface for publishing NavGridOfDoubles/OccupancyGrid msgs and their updates periodically. More... | |
Functions | |
void | applyInterpretation (nav_grid::NavGrid< unsigned char > &grid, const std::vector< unsigned char > &cost_interpretation_table) |
Apply a given interpretation to the provided nav grid. | |
template<typename NumericType > | |
void | getExtremeValues (const nav_grid::NavGrid< NumericType > &grid, const NumericType unknown_value, NumericType &min_val, NumericType &max_val) |
Retrieve the minimum and maximum values from a grid, ignoring the unknown_value. | |
std::vector< unsigned char > | getOccupancyInput (bool trinary=false, bool use_unknown_value=false) |
Above 100 is occupied, -1 is sometimes unknown, and otherwise it is either scaled or freespace. | |
std::vector< unsigned char > | grayScaleInterpretation (const double free_threshold, const double occupied_threshold) |
Above occupied_threshold is occupied, below free_threshold is free, and the middle is in between. | |
unsigned char | interpretCost (unsigned char original_value, const std::vector< unsigned char > &cost_interpretation_table) |
return cost_interpretation_table[original_value] (or original_value if not a valid index) | |
template<typename NumericType > | |
unsigned char | interpretValue (const NumericType value, const NumericType min_value, const NumericType denominator, const NumericType unknown_value) |
Scale the given value to fit within [0, 100] (unless its ignore_value, then its -1) | |
geometry_msgs::Point32 | makePoint (const nav_grid::NavGridInfo &info, int x, int y) |
std::vector< unsigned char > | pixelColoringInterpretation (const double free_threshold, const double occupied_threshold) |
Above occupied_threshold is occupied, below free_threshold is free, and the middle gray is unknown. | |
static std::vector< unsigned char > | RAW (0) |
Every value is untouched. | |
template<typename ROSMsgType , typename NumericType > | |
ROSMsgType | toMsg (const nav_grid::NavGrid< NumericType > &grid, const ros::Time &stamp) |
Utilities for converting NavGrid objects to NavGridOfX messages and updates. | |
nav_2d_msgs::NavGridOfChars | toMsg (const nav_grid::NavGrid< unsigned char > &grid, const ros::Time &stamp=ros::Time(0)) |
NavGrid<unsigned char> to NavGridOfChars. | |
nav_2d_msgs::NavGridOfDoubles | toMsg (const nav_grid::NavGrid< double > &grid, const ros::Time &stamp=ros::Time(0)) |
NavGrid<double> to NavGridOfDoubles. | |
nav_2d_msgs::NavGridOfDoubles | toMsg (const nav_grid::NavGrid< float > &grid, const ros::Time &stamp=ros::Time(0)) |
NavGrid<float> to NavGridOfDoubles. | |
nav_msgs::OccupancyGrid | toOccupancyGrid (const nav_grid::NavGrid< unsigned char > &grid, const ros::Time &stamp=ros::Time(0), const std::vector< unsigned char > cost_interpretation_table=std::vector< unsigned char >()) |
NavGrid<unsigned char> to OccupancyGrid using cost_interpretation_table. | |
template<typename NumericType > | |
nav_msgs::OccupancyGrid | toOccupancyGrid (const nav_grid::NavGrid< NumericType > &grid, const NumericType min_value, const NumericType max_value, const NumericType unknown_value, const ros::Time &stamp=ros::Time(0)) |
generic NavGrid to OccupancyGrid using scaling. Min and max explicitly provided. | |
template<typename NumericType > | |
nav_msgs::OccupancyGrid | toOccupancyGrid (const nav_grid::NavGrid< NumericType > &grid, const NumericType unknown_value=std::numeric_limits< NumericType >::max(), const ros::Time &stamp=ros::Time(0)) |
generic NavGrid to OccupancyGrid using scaling. Min and max not provided, so they are determined first. | |
map_msgs::OccupancyGridUpdate | toOccupancyGridUpdate (const nav_grid::NavGrid< unsigned char > &grid, const nav_core2::UIntBounds &bounds, const ros::Time &stamp=ros::Time(0), const std::vector< unsigned char > cost_interpretation_table=std::vector< unsigned char >()) |
NavGrid<unsigned char> to OccupancyGridUpdate using cost_interpretation_table. | |
template<typename NumericType > | |
map_msgs::OccupancyGridUpdate | toOccupancyGridUpdate (const nav_grid::NavGrid< NumericType > &grid, const nav_core2::UIntBounds &bounds, const NumericType min_value, const NumericType max_value, const NumericType unknown_value, const ros::Time &stamp=ros::Time(0)) |
generic NavGrid to OccupancyGridUpdate using scaling. Min and max explicitly provided. | |
template<typename ROSMsgType , typename NumericType > | |
ROSMsgType | toUpdate (const nav_grid::NavGrid< NumericType > &grid, const nav_core2::UIntBounds &bounds, const ros::Time &stamp) |
Generic conversion from a portion of a grid of arbitrary type to an update message of arbitrary type. | |
nav_2d_msgs::NavGridOfCharsUpdate | toUpdate (const nav_grid::NavGrid< unsigned char > &grid, const nav_core2::UIntBounds &bounds, const ros::Time &stamp=ros::Time(0)) |
NavGrid<unsigned char> to NavGridOfCharsUpdate. | |
nav_2d_msgs::NavGridOfDoublesUpdate | toUpdate (const nav_grid::NavGrid< double > &grid, const nav_core2::UIntBounds &bounds, const ros::Time &stamp=ros::Time(0)) |
NavGrid<double> to NavGridOfDoublesUpdate. | |
nav_2d_msgs::NavGridOfDoublesUpdate | toUpdate (const nav_grid::NavGrid< float > &grid, const nav_core2::UIntBounds &bounds, const ros::Time &stamp=ros::Time(0)) |
NavGrid<float> to NavGridOfDoublesUpdate. | |
Variables | |
static std::vector< unsigned char > | NEGATE |
Every value is 255 - i value. | |
static std::vector< unsigned char > | OCC_GRID_PUBLISHING |
Scale [0, 255] down to [0, 100] (except for a few special values) | |
static std::vector< unsigned char > | SCALE_SAVE |
0 becomes 254 (white), 100 becomes 0 (black) and everything in between is gray | |
static std::vector< unsigned char > | TRINARY_SAVE |
0 becomes 254 (white), 100 becomes 0 (black) and everything else is 205 (gray) |
void nav_grid_pub_sub::applyInterpretation | ( | nav_grid::NavGrid< unsigned char > & | grid, |
const std::vector< unsigned char > & | cost_interpretation_table | ||
) | [inline] |
Apply a given interpretation to the provided nav grid.
Definition at line 63 of file cost_interpretation.h.
void nav_grid_pub_sub::getExtremeValues | ( | const nav_grid::NavGrid< NumericType > & | grid, |
const NumericType | unknown_value, | ||
NumericType & | min_val, | ||
NumericType & | max_val | ||
) | [inline] |
Retrieve the minimum and maximum values from a grid, ignoring the unknown_value.
Definition at line 138 of file occ_grid_message_utils.h.
std::vector< unsigned char > nav_grid_pub_sub::getOccupancyInput | ( | bool | trinary = false , |
bool | use_unknown_value = false |
||
) |
Above 100 is occupied, -1 is sometimes unknown, and otherwise it is either scaled or freespace.
Expects values from [-1, 100] and outputs [0, 255]
Definition at line 86 of file cost_interpretation_tables.cpp.
std::vector< unsigned char > nav_grid_pub_sub::grayScaleInterpretation | ( | const double | free_threshold, |
const double | occupied_threshold | ||
) |
Above occupied_threshold is occupied, below free_threshold is free, and the middle is in between.
Expects values from [0, 255] and flattens them to [0, 100]
Definition at line 63 of file cost_interpretation_tables.cpp.
unsigned char nav_grid_pub_sub::interpretCost | ( | unsigned char | original_value, |
const std::vector< unsigned char > & | cost_interpretation_table | ||
) | [inline] |
return cost_interpretation_table[original_value] (or original_value if not a valid index)
Definition at line 47 of file cost_interpretation.h.
unsigned char nav_grid_pub_sub::interpretValue | ( | const NumericType | value, |
const NumericType | min_value, | ||
const NumericType | denominator, | ||
const NumericType | unknown_value | ||
) | [inline] |
Scale the given value to fit within [0, 100] (unless its ignore_value, then its -1)
Note denominator is used instead of max_value to avoid recalculating the difference each time this is called.
value | Value to interpret |
min_value | Minimum value that will correspond to 0 |
denominator | The result of max_value - min_value, where max_value corresponds with 100 |
unknown_value | If the value is equal to this value, return -1 |
Definition at line 83 of file cost_interpretation.h.
geometry_msgs::Point32 nav_grid_pub_sub::makePoint | ( | const nav_grid::NavGridInfo & | info, |
int | x, | ||
int | y | ||
) | [inline] |
Definition at line 54 of file nav_grid_publisher.h.
std::vector< unsigned char > nav_grid_pub_sub::pixelColoringInterpretation | ( | const double | free_threshold, |
const double | occupied_threshold | ||
) |
Above occupied_threshold is occupied, below free_threshold is free, and the middle gray is unknown.
Expects values from [0, 255] and flattens them to 0, 100, and -1
Definition at line 41 of file cost_interpretation_tables.cpp.
static std::vector<unsigned char> nav_grid_pub_sub::RAW | ( | 0 | ) | [static] |
Every value is untouched.
ROSMsgType nav_grid_pub_sub::toMsg | ( | const nav_grid::NavGrid< NumericType > & | grid, |
const ros::Time & | stamp | ||
) |
Utilities for converting NavGrid objects to NavGridOfX messages and updates.
Generic conversion from grid of arbitrary type to message of arbitrary type
Note that the ROSMsgType must be provided since return types are not used in template type deduction.
Definition at line 59 of file nav_grid_message_utils.h.
nav_2d_msgs::NavGridOfChars nav_grid_pub_sub::toMsg | ( | const nav_grid::NavGrid< unsigned char > & | grid, |
const ros::Time & | stamp = ros::Time(0) |
||
) | [inline] |
NavGrid<unsigned char> to NavGridOfChars.
Definition at line 102 of file nav_grid_message_utils.h.
nav_2d_msgs::NavGridOfDoubles nav_grid_pub_sub::toMsg | ( | const nav_grid::NavGrid< double > & | grid, |
const ros::Time & | stamp = ros::Time(0) |
||
) | [inline] |
NavGrid<double> to NavGridOfDoubles.
Definition at line 111 of file nav_grid_message_utils.h.
nav_2d_msgs::NavGridOfDoubles nav_grid_pub_sub::toMsg | ( | const nav_grid::NavGrid< float > & | grid, |
const ros::Time & | stamp = ros::Time(0) |
||
) | [inline] |
NavGrid<float> to NavGridOfDoubles.
Definition at line 119 of file nav_grid_message_utils.h.
nav_msgs::OccupancyGrid nav_grid_pub_sub::toOccupancyGrid | ( | const nav_grid::NavGrid< unsigned char > & | grid, |
const ros::Time & | stamp = ros::Time(0) , |
||
const std::vector< unsigned char > | cost_interpretation_table = std::vector<unsigned char>() |
||
) | [inline] |
NavGrid<unsigned char> to OccupancyGrid using cost_interpretation_table.
Definition at line 56 of file occ_grid_message_utils.h.
nav_msgs::OccupancyGrid nav_grid_pub_sub::toOccupancyGrid | ( | const nav_grid::NavGrid< NumericType > & | grid, |
const NumericType | min_value, | ||
const NumericType | max_value, | ||
const NumericType | unknown_value, | ||
const ros::Time & | stamp = ros::Time(0) |
||
) |
generic NavGrid to OccupancyGrid using scaling. Min and max explicitly provided.
NumericType can be inferred, so explicitly specifying it is generally not needed.
Definition at line 108 of file occ_grid_message_utils.h.
nav_msgs::OccupancyGrid nav_grid_pub_sub::toOccupancyGrid | ( | const nav_grid::NavGrid< NumericType > & | grid, |
const NumericType | unknown_value = std::numeric_limits<NumericType>::max() , |
||
const ros::Time & | stamp = ros::Time(0) |
||
) |
generic NavGrid to OccupancyGrid using scaling. Min and max not provided, so they are determined first.
NumericType can be inferred, so explicitly specifying it is generally not needed.
Definition at line 159 of file occ_grid_message_utils.h.
map_msgs::OccupancyGridUpdate nav_grid_pub_sub::toOccupancyGridUpdate | ( | const nav_grid::NavGrid< unsigned char > & | grid, |
const nav_core2::UIntBounds & | bounds, | ||
const ros::Time & | stamp = ros::Time(0) , |
||
const std::vector< unsigned char > | cost_interpretation_table = std::vector<unsigned char>() |
||
) | [inline] |
NavGrid<unsigned char> to OccupancyGridUpdate using cost_interpretation_table.
Definition at line 78 of file occ_grid_message_utils.h.
map_msgs::OccupancyGridUpdate nav_grid_pub_sub::toOccupancyGridUpdate | ( | const nav_grid::NavGrid< NumericType > & | grid, |
const nav_core2::UIntBounds & | bounds, | ||
const NumericType | min_value, | ||
const NumericType | max_value, | ||
const NumericType | unknown_value, | ||
const ros::Time & | stamp = ros::Time(0) |
||
) |
generic NavGrid to OccupancyGridUpdate using scaling. Min and max explicitly provided.
NumericType can be inferred, so explicitly specifying it is generally not needed.
Definition at line 174 of file occ_grid_message_utils.h.
ROSMsgType nav_grid_pub_sub::toUpdate | ( | const nav_grid::NavGrid< NumericType > & | grid, |
const nav_core2::UIntBounds & | bounds, | ||
const ros::Time & | stamp | ||
) |
Generic conversion from a portion of a grid of arbitrary type to an update message of arbitrary type.
Note that the ROSMsgType must be provided since return types are not used in template type deduction.
Definition at line 82 of file nav_grid_message_utils.h.
nav_2d_msgs::NavGridOfCharsUpdate nav_grid_pub_sub::toUpdate | ( | const nav_grid::NavGrid< unsigned char > & | grid, |
const nav_core2::UIntBounds & | bounds, | ||
const ros::Time & | stamp = ros::Time(0) |
||
) | [inline] |
NavGrid<unsigned char> to NavGridOfCharsUpdate.
Definition at line 127 of file nav_grid_message_utils.h.
nav_2d_msgs::NavGridOfDoublesUpdate nav_grid_pub_sub::toUpdate | ( | const nav_grid::NavGrid< double > & | grid, |
const nav_core2::UIntBounds & | bounds, | ||
const ros::Time & | stamp = ros::Time(0) |
||
) | [inline] |
NavGrid<double> to NavGridOfDoublesUpdate.
Definition at line 136 of file nav_grid_message_utils.h.
nav_2d_msgs::NavGridOfDoublesUpdate nav_grid_pub_sub::toUpdate | ( | const nav_grid::NavGrid< float > & | grid, |
const nav_core2::UIntBounds & | bounds, | ||
const ros::Time & | stamp = ros::Time(0) |
||
) | [inline] |
NavGrid<float> to NavGridOfDoublesUpdate.
Definition at line 145 of file nav_grid_message_utils.h.
std::vector<unsigned char> nav_grid_pub_sub::NEGATE [static] |
{ 255, 254, 253, 252, 251, 250, 249, 248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 237, 236, 235, 234, 233, 232, 231, 230, 229, 228, 227, 226, 225, 224, 223, 222, 221, 220, 219, 218, 217, 216, 215, 214, 213, 212, 211, 210, 209, 208, 207, 206, 205, 204, 203, 202, 201, 200, 199, 198, 197, 196, 195, 194, 193, 192, 191, 190, 189, 188, 187, 186, 185, 184, 183, 182, 181, 180, 179, 178, 177, 176, 175, 174, 173, 172, 171, 170, 169, 168, 167, 166, 165, 164, 163, 162, 161, 160, 159, 158, 157, 156, 155, 154, 153, 152, 151, 150, 149, 148, 147, 146, 145, 144, 143, 142, 141, 140, 139, 138, 137, 136, 135, 134, 133, 132, 131, 130, 129, 128, 127, 126, 125, 124, 123, 122, 121, 120, 119, 118, 117, 116, 115, 114, 113, 112, 111, 110, 109, 108, 107, 106, 105, 104, 103, 102, 101, 100, 99, 98, 97, 96, 95, 94, 93, 92, 91, 90, 89, 88, 87, 86, 85, 84, 83, 82, 81, 80, 79, 78, 77, 76, 75, 74, 73, 72, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39, 38, 37, 36, 35, 34, 33, 32, 31, 30, 29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 }
Every value is 255 - i value.
map from [0, 255] to [255, 0]
Definition at line 52 of file cost_interpretation_tables.h.
std::vector<unsigned char> nav_grid_pub_sub::OCC_GRID_PUBLISHING [static] |
Scale [0, 255] down to [0, 100] (except for a few special values)
Expects values from [0, 255] and flattens them to [-1, 100] There are a few special values that only have one input value mapping to them. 0, 253, 254 and 255 always map to 0, 99, 100, and 255 (and are the only values that do).
Definition at line 97 of file cost_interpretation_tables.h.
std::vector<unsigned char> nav_grid_pub_sub::SCALE_SAVE [static] |
0 becomes 254 (white), 100 becomes 0 (black) and everything in between is gray
Translates from a trinary occupancy grid to an image pixel color
Definition at line 198 of file cost_interpretation_tables.h.
std::vector<unsigned char> nav_grid_pub_sub::TRINARY_SAVE [static] |
0 becomes 254 (white), 100 becomes 0 (black) and everything else is 205 (gray)
Translates from a trinary occupancy grid to an image pixel color
Definition at line 156 of file cost_interpretation_tables.h.