Fawkes API  Fawkes Development Version
occupancygrid.h
1 
2 /***************************************************************************
3  * occupancygrid.h - An occupancy-grid
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 AllemaniACs
7  * 2013-2014 Bahram Maleki-Fard
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef _PLUGINS_COLLI_UTILS_OCCUPANCYGRID_OCCUPANCYGRID_H_
24 #define _PLUGINS_COLLI_UTILS_OCCUPANCYGRID_OCCUPANCYGRID_H_
25 
26 #include "probability.h"
27 
28 #include <vector>
29 
30 namespace fawkes {
31 
32 /** Occupancy threshold. */
33 const float OCCUPANCY_THRESHOLD = 0.45f;
34 
36 {
37 public:
38  OccupancyGrid(int width, int height, int cell_width = 5, int cell_height = 5);
39  virtual ~OccupancyGrid();
40 
41  ///\brief Get the cell width (in cm)
42  int get_cell_width();
43 
44  ///\brief Get the cell height (in cm)
45  int get_cell_height();
46 
47  ///\brief Get the width of the grid
48  int get_width();
49 
50  ///\brief Get the height of the grid
51  int get_height();
52 
53  ///\brief Resets the cell width (in cm)
54  void set_cell_width(int cell_width);
55 
56  ///\brief Resets the cell height (in cm)
57  void set_cell_height(int cell_height);
58 
59  ///\brief Resets the width of the grid and constructs a new empty grid
60  void set_width(int width);
61 
62  ///\brief Resets the height of the grid and constructs a new empty grid
63  void set_height(int height);
64 
65  ///\brief Reset the occupancy probability of a cell
66  virtual void set_prob(int x, int y, Probability prob);
67 
68  ///\brief Resets all occupancy probabilities
69  void fill(Probability prob);
70 
71  ///\brief Get the occupancy probability of a cell
72  Probability get_prob(int x, int y);
73 
74  ///\brief Get the occupancy probability of a cell
75  Probability &operator()(const int x, const int y);
76 
77  ///\brief Init a new empty grid with the predefined parameters */
78  void init_grid();
79 
80  /// The occupancy probability of the cells in a 2D array
81  std::vector<std::vector<Probability>> occupancy_probs_;
82 
83 protected:
84  int cell_width_; /**< Cell width in cm */
85  int cell_height_; /**< Cell height in cm */
86  int width_; /**< Width of the grid in # cells */
87  int height_; /**< Height of the grid in # cells */
88 };
89 
90 } // namespace fawkes
91 
92 #endif
fawkes::OccupancyGrid::set_cell_width
void set_cell_width(int cell_width)
Resets the cell width (in cm)
Definition: occupancygrid.cpp:97
fawkes::OccupancyGrid::~OccupancyGrid
virtual ~OccupancyGrid()
Destructor.
Definition: occupancygrid.cpp:52
fawkes::OccupancyGrid::get_width
int get_width()
Get the width of the grid.
Definition: occupancygrid.cpp:79
fawkes::OccupancyGrid::height_
int height_
Height of the grid in # cells.
Definition: occupancygrid.h:87
fawkes::OccupancyGrid::width_
int width_
Width of the grid in # cells.
Definition: occupancygrid.h:86
fawkes::OccupancyGrid::fill
void fill(Probability prob)
Resets all occupancy probabilities.
Definition: occupancygrid.cpp:147
fawkes::OccupancyGrid::operator()
Probability & operator()(const int x, const int y)
Get the occupancy probability of a cell.
Definition: occupancygrid.cpp:179
fawkes::OccupancyGrid::cell_width_
int cell_width_
Cell width in cm.
Definition: occupancygrid.h:84
fawkes::OccupancyGrid::set_prob
virtual void set_prob(int x, int y, Probability prob)
Reset the occupancy probability of a cell.
Definition: occupancygrid.cpp:137
fawkes::OccupancyGrid::set_cell_height
void set_cell_height(int cell_height)
Resets the cell height (in cm)
Definition: occupancygrid.cpp:106
fawkes::OccupancyGrid
Occupancy Grid class for general use.
Definition: occupancygrid.h:36
fawkes::OccupancyGrid::get_prob
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
Definition: occupancygrid.cpp:164
fawkes::OccupancyGrid::cell_height_
int cell_height_
Cell height in cm.
Definition: occupancygrid.h:85
fawkes
Fawkes library namespace.
fawkes::Probability
float Probability
A probability type.
Definition: probability.h:29
fawkes::OccupancyGrid::init_grid
void init_grid()
Init a new empty grid with the predefined parameters *‍/.
Definition: occupancygrid.cpp:186
fawkes::OccupancyGrid::occupancy_probs_
std::vector< std::vector< Probability > > occupancy_probs_
The occupancy probability of the cells in a 2D array.
Definition: occupancygrid.h:81
fawkes::OccupancyGrid::OccupancyGrid
OccupancyGrid(int width, int height, int cell_width=5, int cell_height=5)
Constructs an empty occupancy grid.
Definition: occupancygrid.cpp:41
fawkes::OccupancyGrid::set_width
void set_width(int width)
Resets the width of the grid and constructs a new empty grid.
Definition: occupancygrid.cpp:115
fawkes::OccupancyGrid::set_height
void set_height(int height)
Resets the height of the grid and constructs a new empty grid.
Definition: occupancygrid.cpp:125
fawkes::OCCUPANCY_THRESHOLD
const float OCCUPANCY_THRESHOLD
Occupancy threshold.
Definition: occupancygrid.h:33
fawkes::OccupancyGrid::get_cell_width
int get_cell_width()
Get the cell width (in cm)
Definition: occupancygrid.cpp:61
fawkes::OccupancyGrid::get_height
int get_height()
Get the height of the grid.
Definition: occupancygrid.cpp:88
fawkes::OccupancyGrid::get_cell_height
int get_cell_height()
Get the cell height (in cm)
Definition: occupancygrid.cpp:70