#include <map_ray_caster.h>
Public Member Functions | |
const std::vector< size_t > & | getRayCastToMapBorder (const double angle, const size_t nrow, const size_t ncol, const double tolerance=0) |
void | laserScanCast (const nav_msgs::OccupancyGrid &map, sensor_msgs::LaserScan &scan) |
size_t | lookupSize () const |
MapRayCaster (const int occupied_threshold=60) | |
Private Member Functions | |
RayLookup::const_iterator | angleLookup (const double angle, const double tolerance) |
Private Attributes | |
size_t | ncol_ |
Map width used in the cache. | |
size_t | nrow_ |
Map height used in the cache. | |
int | occupied_threshold_ |
RayLookup | raycast_lookup_ |
Definition at line 22 of file map_ray_caster.h.
map_ray_caster::MapRayCaster::MapRayCaster | ( | const int | occupied_threshold = 60 | ) |
Definition at line 6 of file map_ray_caster.cpp.
RayLookup::const_iterator map_ray_caster::MapRayCaster::angleLookup | ( | const double | angle, |
const double | tolerance | ||
) | [private] |
Return an iterator to the closest key (angle) in the cache.
Definition at line 166 of file map_ray_caster.cpp.
const std::vector< size_t > & map_ray_caster::MapRayCaster::getRayCastToMapBorder | ( | const double | angle, |
const size_t | nrow, | ||
const size_t | ncol, | ||
const double | tolerance = 0 |
||
) |
Return the list of pixel indexes from map center to pixel at map border and given angle
The Bresenham algorithm is used.
[in] | angle | beam angle. |
[in] | nrow | image height. |
[in] | ncol | image width. |
Definition at line 71 of file map_ray_caster.cpp.
void map_ray_caster::MapRayCaster::laserScanCast | ( | const nav_msgs::OccupancyGrid & | map, |
sensor_msgs::LaserScan & | scan | ||
) |
Fill the ranges attributes with distances to obstacle
The ray casting will be from scan.angle_min to scan.angle_max, so that the scan message must be initialized with non-default values.
[in] | map | occupancy grid. |
[in,out] | scan | LaserScan. scan.angle_min, scan.angle_max, scan.increment, scan.range_max will be used as input and must have non-null values. scan.ranges will set as output. |
Definition at line 29 of file map_ray_caster.cpp.
size_t map_ray_caster::MapRayCaster::lookupSize | ( | ) | const [inline] |
Definition at line 32 of file map_ray_caster.h.
size_t map_ray_caster::MapRayCaster::ncol_ [private] |
Map width used in the cache.
Definition at line 39 of file map_ray_caster.h.
size_t map_ray_caster::MapRayCaster::nrow_ [private] |
Map height used in the cache.
Definition at line 40 of file map_ray_caster.h.
int map_ray_caster::MapRayCaster::occupied_threshold_ [private] |
Definition at line 38 of file map_ray_caster.h.
Definition at line 41 of file map_ray_caster.h.