Fawkes API
Fawkes Development Version
|
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed purposes. More...
#include <>>
Public Member Functions | |
LaserOccupancyGrid (Laser360Interface *laser, Logger *logger, Configuration *config, tf::Transformer *listener, int width=150, int height=150, int cell_width=5, int cell_height=5) | |
Constructor. More... | |
~LaserOccupancyGrid () | |
Descturctor. More... | |
float | update_occ_grid (int mid_x, int mid_y, float inc, float vx, float vy) |
Put the laser readings in the occupancy grid. More... | |
void | reset_old () |
Reset all old readings and forget about the world state! More... | |
point_t | get_laser_position () |
Get the laser's position in the grid. More... | |
void | set_base_offset (float x, float y) |
Set the offset of base_link from laser. More... | |
colli_cell_cost_t | get_cell_costs () const |
Get cell costs. More... | |
![]() | |
OccupancyGrid (int width, int height, int cell_width=5, int cell_height=5) | |
Constructs an empty occupancy grid. More... | |
virtual | ~OccupancyGrid () |
Destructor. More... | |
int | get_cell_width () |
Get the cell width (in cm) More... | |
int | get_cell_height () |
Get the cell height (in cm) More... | |
int | get_width () |
Get the width of the grid. More... | |
int | get_height () |
Get the height of the grid. More... | |
void | set_cell_width (int cell_width) |
Resets the cell width (in cm) More... | |
void | set_cell_height (int cell_height) |
Resets the cell height (in cm) More... | |
void | set_width (int width) |
Resets the width of the grid and constructs a new empty grid. More... | |
void | set_height (int height) |
Resets the height of the grid and constructs a new empty grid. More... | |
virtual void | set_prob (int x, int y, Probability prob) |
Reset the occupancy probability of a cell. More... | |
void | fill (Probability prob) |
Resets all occupancy probabilities. More... | |
Probability | get_prob (int x, int y) |
Get the occupancy probability of a cell. More... | |
Probability & | operator() (const int x, const int y) |
Get the occupancy probability of a cell. More... | |
void | init_grid () |
Init a new empty grid with the predefined parameters */. More... | |
Additional Inherited Members | |
![]() | |
std::vector< std::vector< Probability > > | occupancy_probs_ |
The occupancy probability of the cells in a 2D array. More... | |
![]() | |
int | cell_width_ |
Cell width in cm. More... | |
int | cell_height_ |
Cell height in cm. More... | |
int | width_ |
Width of the grid in # cells. More... | |
int | height_ |
Height of the grid in # cells. More... | |
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed purposes.
Definition at line 49 of file og_laser.h.
fawkes::LaserOccupancyGrid::LaserOccupancyGrid | ( | Laser360Interface * | laser, |
Logger * | logger, | ||
Configuration * | config, | ||
tf::Transformer * | listener, | ||
int | width = 150 , |
||
int | height = 150 , |
||
int | cell_width = 5 , |
||
int | cell_height = 5 |
||
) |
Constructor.
laser | The Laser object |
logger | The fawkes logger |
config | The fawkes configuration |
listener | The tf::Transformer |
width | The width of the grid (in m) |
height | The height of the grid (in m) |
cell_width | The width of a cell (in cm) |
cell_height | The height of a cell (in cm) |
Definition at line 61 of file og_laser.cpp.
References fawkes::deg2rad(), fawkes::colli_cell_cost_t::far, fawkes::colli_cell_cost_t::free, fawkes::Configuration::get_bool(), fawkes::RoboShape::get_complete_width_x(), fawkes::RoboShape::get_complete_width_y(), fawkes::Configuration::get_float(), fawkes::Configuration::get_int(), fawkes::RoboShapeColli::get_robot_length_for_deg(), fawkes::Configuration::get_string(), fawkes::OccupancyGrid::init_grid(), fawkes::RoboShape::is_angular_robot(), fawkes::Logger::log_debug(), fawkes::Logger::log_warn(), fawkes::colli_cell_cost_t::mid, fawkes::colli_cell_cost_t::near, fawkes::colli_cell_cost_t::occ, fawkes::Interface::resize_buffers(), fawkes::point_struct::x, fawkes::cart_coord_2d_struct::x, fawkes::point_struct::y, and fawkes::cart_coord_2d_struct::y.
fawkes::LaserOccupancyGrid::~LaserOccupancyGrid | ( | ) |
Descturctor.
Definition at line 145 of file og_laser.cpp.
colli_cell_cost_t fawkes::LaserOccupancyGrid::get_cell_costs | ( | ) | const |
Get cell costs.
Definition at line 442 of file og_laser.cpp.
References fawkes::OccupancyGrid::cell_height_, fawkes::OccupancyGrid::cell_width_, fawkes::distance(), fawkes::RoboShape::get_complete_width_x(), fawkes::RoboShape::get_complete_width_y(), fawkes::ColliObstacleMap::get_obstacle(), fawkes::OccupancyGrid::height_, fawkes::Time::in_sec(), fawkes::Clock::instance(), fawkes::OccupancyGrid::occupancy_probs_, fawkes::sqr(), fawkes::OccupancyGrid::width_, fawkes::point_struct::x, fawkes::cart_coord_2d_struct::x, fawkes::point_struct::y, and fawkes::cart_coord_2d_struct::y.
Referenced by fawkes::AbstractSearch::AbstractSearch(), fawkes::AStar::AStar(), ColliThread::loop(), fawkes::EscapePotentialFieldDriveModule::update(), and fawkes::EscapePotentialFieldOmniDriveModule::update().
point_t fawkes::LaserOccupancyGrid::get_laser_position | ( | ) |
Get the laser's position in the grid.
Definition at line 421 of file og_laser.cpp.
void fawkes::LaserOccupancyGrid::reset_old | ( | ) |
Reset all old readings and forget about the world state!
Definition at line 152 of file og_laser.cpp.
References fawkes::angle_distance_signed(), fawkes::Interface::buffer_timestamp(), fawkes::Interface::copy_shared_to_buffer(), fawkes::Laser360Interface::distances(), fawkes::Laser360Interface::frame(), fawkes::Time::in_sec(), fawkes::Clock::instance(), fawkes::Logger::log_debug(), fawkes::tf::Transformer::lookup_transform(), fawkes::Laser360Interface::maxlenof_distances(), fawkes::normalize_rad(), fawkes::Clock::now(), fawkes::polar_coord_2d_t::phi, fawkes::polar2cart2d(), fawkes::polar_coord_2d_t::r, fawkes::rad2deg(), fawkes::Interface::read(), fawkes::Interface::read_from_buffer(), fawkes::Interface::timestamp(), fawkes::cart_coord_2d_struct::x, and fawkes::cart_coord_2d_struct::y.
Referenced by ColliThread::loop().
void fawkes::LaserOccupancyGrid::set_base_offset | ( | float | x, |
float | y | ||
) |
Set the offset of base_link from laser.
x | offset in x-direction (in meters) |
y | offset in y-direction (in meters) |
Definition at line 431 of file og_laser.cpp.
References fawkes::OccupancyGrid::cell_height_, fawkes::OccupancyGrid::cell_width_, fawkes::point_struct::x, fawkes::cart_coord_2d_struct::x, fawkes::point_struct::y, and fawkes::cart_coord_2d_struct::y.
Referenced by ColliThread::init(), and ColliThread::loop().
float fawkes::LaserOccupancyGrid::update_occ_grid | ( | int | midX, |
int | midY, | ||
float | inc, | ||
float | vx, | ||
float | vy | ||
) |
Put the laser readings in the occupancy grid.
Put the laser readings in the occupancy grid Also, every reading gets a radius according to the relative direction of this reading to the robot.
midX | is the current x position of the robot in the grid. |
midY | is the current y position of the robot in the grid. |
inc | is the current constant to increase the obstacles. |
vx | Translation x velocity of the motor |
vy | Translation y velocity of the motor |
Definition at line 357 of file og_laser.cpp.
References fawkes::colli_cell_cost_t::free, fawkes::OccupancyGrid::height_, fawkes::Logger::log_error(), fawkes::tf::Transformer::lookup_transform(), fawkes::OccupancyGrid::occupancy_probs_, fawkes::OccupancyGrid::width_, fawkes::point_struct::x, and fawkes::point_struct::y.
Referenced by ColliThread::loop().