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 #if 0 /* just to make Emacs auto-indent happy */
33 }
34 #endif
35 
36 /** Occupancy threshold. */
37 const float OCCUPANCY_THRESHOLD = 0.45f;
38 
40 {
41  public:
42  OccupancyGrid(int width, int height, int cell_width=5, int cell_height=5);
43  virtual ~OccupancyGrid();
44 
45  ///\brief Get the cell width (in cm)
46  int get_cell_width();
47 
48  ///\brief Get the cell height (in cm)
49  int get_cell_height();
50 
51  ///\brief Get the width of the grid
52  int get_width();
53 
54  ///\brief Get the height of the grid
55  int get_height();
56 
57  ///\brief Resets the cell width (in cm)
58  void set_cell_width(int cell_width);
59 
60  ///\brief Resets the cell height (in cm)
61  void set_cell_height(int cell_height);
62 
63  ///\brief Resets the width of the grid and constructs a new empty grid
64  void set_width(int width);
65 
66  ///\brief Resets the height of the grid and constructs a new empty grid
67  void set_height(int height);
68 
69  ///\brief Reset the occupancy probability of a cell
70  virtual void set_prob(int x, int y, Probability prob);
71 
72  ///\brief Resets all occupancy probabilities
73  void fill(Probability prob);
74 
75  ///\brief Get the occupancy probability of a cell
76  Probability get_prob(int x, int y);
77 
78  ///\brief Get the occupancy probability of a cell
79  Probability& operator () (const int x, const int y);
80 
81  ///\brief Init a new empty grid with the predefined parameters */
82  void init_grid();
83 
84  /// The occupancy probability of the cells in a 2D array
85  std::vector<std::vector<Probability> > occupancy_probs_;
86 
87  protected:
88  int cell_width_; /**< Cell width in cm */
89  int cell_height_; /**< Cell height in cm */
90  int width_; /**< Width of the grid in # cells */
91  int height_; /**< Height of the grid in # cells */
92 
93 };
94 
95 } // namespace fawkes
96 
97 #endif
void fill(Probability prob)
Resets all occupancy probabilities.
float Probability
A probability type.
Definition: probability.h:33
int get_width()
Get the width of the grid.
int get_height()
Get the height of the grid.
Probability & operator()(const int x, const int y)
Get the occupancy probability of a cell.
int get_cell_width()
Get the cell width (in cm)
Fawkes library namespace.
void set_cell_height(int cell_height)
Resets the cell height (in cm)
int get_cell_height()
Get the cell height (in cm)
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
const float OCCUPANCY_THRESHOLD
Occupancy threshold.
Definition: occupancygrid.h:37
int cell_height_
Cell height in cm.
Definition: occupancygrid.h:89
int width_
Width of the grid in # cells.
Definition: occupancygrid.h:90
virtual ~OccupancyGrid()
Destructor.
int cell_width_
Cell width in cm.
Definition: occupancygrid.h:88
int height_
Height of the grid in # cells.
Definition: occupancygrid.h:91
void set_width(int width)
Resets the width of the grid and constructs a new empty grid.
Occupancy Grid class for general use.
Definition: occupancygrid.h:39
virtual void set_prob(int x, int y, Probability prob)
Reset the occupancy probability of a cell.
void set_height(int height)
Resets the height of the grid and constructs a new empty grid.
void init_grid()
Init a new empty grid with the predefined parameters */.
std::vector< std::vector< Probability > > occupancy_probs_
The occupancy probability of the cells in a 2D array.
Definition: occupancygrid.h:85
OccupancyGrid(int width, int height, int cell_width=5, int cell_height=5)
Constructs an empty occupancy grid.
void set_cell_width(int cell_width)
Resets the cell width (in cm)