#include <map_builder.h>
Public Member Functions | |
nav_msgs::OccupancyGrid | getMap () const |
void | grow (const sensor_msgs::LaserScan &scan) |
MapBuilder (int width, int height, double resolution) | |
bool | saveMap (const std::string &name) const |
Save the map on disk. | |
Private Member Functions | |
bool | getRayCastToObstacle (const nav_msgs::OccupancyGrid &map, double angle, double range, vector< size_t > &raycast) |
bool | updateMap (const sensor_msgs::LaserScan &scan, long int dx, long int dy, double theta) |
void | updatePointOccupancy (bool occupied, size_t idx, vector< int8_t > &occupancy, vector< double > &log_odds) const |
void | updatePointsOccupancy (bool occupied, const vector< size_t > &indexes, vector< int8_t > &occupancy, vector< double > &log_odds) |
Private Attributes | |
double | angle_resolution_ |
bool | has_frame_id_ |
true if map frame_id was already set | |
double | large_log_odds_ |
long int | last_xmap_ |
Map integer x position at last map move. | |
long int | last_ymap_ |
Map integer y position at last map move. | |
std::vector< double > | log_odds_ |
nav_msgs::OccupancyGrid | map_ |
local map with fixed orientation | |
std::string | map_frame_id_ |
map frame id in tf | |
double | max_log_odds_for_belief_ |
double | p_occupied_when_laser_ |
double | p_occupied_when_no_laser_ |
map_ray_caster::MapRayCaster | ray_caster_ |
Ray casting with cache. | |
tf::TransformListener | tf_listerner_ |
tf::TransformBroadcaster | tr_broadcaster_ |
To broadcast the transform from LaserScan to local map. | |
std::string | world_frame_id_ |
frame_id of the world frame | |
double | xinit_ |
Map x position at initialization. | |
double | yinit_ |
Map y position at initialization. |
Definition at line 26 of file map_builder.h.
local_map::MapBuilder::MapBuilder | ( | int | width, |
int | height, | ||
double | resolution | ||
) |
Definition at line 116 of file map_builder.cpp.
nav_msgs::OccupancyGrid local_map::MapBuilder::getMap | ( | ) | const [inline] |
Definition at line 36 of file map_builder.h.
bool local_map::MapBuilder::getRayCastToObstacle | ( | const nav_msgs::OccupancyGrid & | map, |
double | angle, | ||
double | range, | ||
vector< size_t > & | raycast | ||
) | [private] |
Return the pixel list by ray casting from map origin to map border, first obstacle.
Return the pixel list by ray casting from map origin to map border or first obstacle, whichever comes first.
[in] | map | occupancy grid |
[in] | angle | laser beam angle |
[in] | range | laser beam range |
[out] | raycast | list of pixel indexes touched by the laser beam |
Definition at line 379 of file map_builder.cpp.
void local_map::MapBuilder::grow | ( | const sensor_msgs::LaserScan & | scan | ) |
Callback for the LaserScan subscriber.
Update (geometrical transformation + probability update) the map with the current scan
Definition at line 247 of file map_builder.cpp.
bool local_map::MapBuilder::saveMap | ( | const std::string & | name | ) | const |
Save the map on disk.
Definition at line 413 of file map_builder.cpp.
bool local_map::MapBuilder::updateMap | ( | const sensor_msgs::LaserScan & | scan, |
long int | dx, | ||
long int | dy, | ||
double | theta | ||
) | [private] |
Definition at line 335 of file map_builder.cpp.
void local_map::MapBuilder::updatePointOccupancy | ( | bool | occupied, |
size_t | idx, | ||
vector< int8_t > & | occupancy, | ||
vector< double > & | log_odds | ||
) | const [private] |
Update occupancy and log odds for a point
[in] | occupied | true if the point was measured as occupied |
[in] | idx | pixel index |
[in] | ncol | map width |
[in,out] | occupancy | occupancy map to update |
[in,out] | log_odds | log odds to update |
Definition at line 192 of file map_builder.cpp.
void local_map::MapBuilder::updatePointsOccupancy | ( | bool | occupied, |
const vector< size_t > & | indexes, | ||
vector< int8_t > & | occupancy, | ||
vector< double > & | log_odds | ||
) | [inline, private] |
Update occupancy and log odds for a list of a points
Definition at line 46 of file map_builder.h.
double local_map::MapBuilder::angle_resolution_ [private] |
Angle resolution for the ray cast lookup (rad). Defaults to 0.25 deg equivalent.
Definition at line 56 of file map_builder.h.
bool local_map::MapBuilder::has_frame_id_ [private] |
true if map frame_id was already set
Definition at line 78 of file map_builder.h.
double local_map::MapBuilder::large_log_odds_ [private] |
Large log odds used with probability 0 and 1. The greater, the more inertia. Defaults to 100.
Definition at line 65 of file map_builder.h.
long int local_map::MapBuilder::last_xmap_ [private] |
Map integer x position at last map move.
Definition at line 82 of file map_builder.h.
long int local_map::MapBuilder::last_ymap_ [private] |
Map integer y position at last map move.
Definition at line 83 of file map_builder.h.
std::vector<double> local_map::MapBuilder::log_odds_ [private] |
log odds ratios for the binary Bayes filter log_odd = log(p(x) / (1 - p(x)))
Definition at line 86 of file map_builder.h.
nav_msgs::OccupancyGrid local_map::MapBuilder::map_ [private] |
local map with fixed orientation
Definition at line 85 of file map_builder.h.
std::string local_map::MapBuilder::map_frame_id_ [private] |
map frame id in tf
Definition at line 79 of file map_builder.h.
double local_map::MapBuilder::max_log_odds_for_belief_ [private] |
Max log odds used to compute the belief (exp(max_log_odds_for_belief) should not overflow). Defaults to 20.
Definition at line 68 of file map_builder.h.
double local_map::MapBuilder::p_occupied_when_laser_ [private] |
Probability that a point is occupied when the laser ranger says so. Defaults to 0.9.
Definition at line 58 of file map_builder.h.
double local_map::MapBuilder::p_occupied_when_no_laser_ [private] |
Probability that a point is occupied when the laser ranger says it's free. Defaults to 0.3.
Definition at line 61 of file map_builder.h.
Ray casting with cache.
Definition at line 88 of file map_builder.h.
Definition at line 75 of file map_builder.h.
To broadcast the transform from LaserScan to local map.
Definition at line 77 of file map_builder.h.
std::string local_map::MapBuilder::world_frame_id_ [private] |
frame_id of the world frame
Definition at line 76 of file map_builder.h.
double local_map::MapBuilder::xinit_ [private] |
Map x position at initialization.
Definition at line 80 of file map_builder.h.
double local_map::MapBuilder::yinit_ [private] |
Map y position at initialization.
Definition at line 81 of file map_builder.h.