Fawkes API  Fawkes Development Version
obstacle_map.h
1 
2 /***************************************************************************
3  * obstacle_map.h - Collection of fast obstacles
4  *
5  * Created: Wed Apr 30 16:03:23 2014
6  * Copyright 2002 Stefan Jacobs
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_SEARCH_OBSTACLE_MAP_H_
24 #define __PLUGINS_COLLI_SEARCH_OBSTACLE_MAP_H_
25 
26 #include "obstacle.h"
27 #include "../common/types.h"
28 
29 #include <vector>
30 #include <map>
31 
32 namespace fawkes
33 {
34 #if 0 /* just to make Emacs auto-indent happy */
35 }
36 #endif
37 
38 /** @class ColliObstacleMap <plugins/colli/search/obstacle_map.h>
39  * This is an implementation of a collection of fast obstacles.
40  */
41 
43 {
44  public:
45  ColliObstacleMap(colli_cell_cost_t cell_costs, bool is_rectangle = false);
46  ~ColliObstacleMap() { obstacles_.clear(); }
47 
48  const std::vector< int > get_obstacle( int width, int height, bool obstacle_increasement = true );
49 
50  private:
51  std::map< unsigned int, ColliFastObstacle * > obstacles_;
52  bool is_rectangle_;
53  colli_cell_cost_t cell_costs_;
54 };
55 
56 /** Constructor.
57  * @param cell_costs struct containing the occ-grid cell costs
58  * @param is_rectangle Defines if obstacles are rectangles or ellipses(=default).
59  */
60 inline
62 {
63  cell_costs_ = cell_costs;
64  is_rectangle_ = is_rectangle;
65 }
66 
67 /** Get the occupied cells that match a given obstacle.
68  * @param width The width of the obstacle
69  * @param height The height of the obstacle
70  * @param obstacle_increasement Enable obstacle increasement?
71  * @return vector with pairwise cell coordinates (x,y), that are occupied by such an obstacle
72  */
73 inline const std::vector< int >
74 ColliObstacleMap::get_obstacle( int width, int height, bool obstacle_increasement )
75 {
76  unsigned int key = ((unsigned int)width << 16) | (unsigned int)height;
77 
78  std::map< unsigned int, ColliFastObstacle * >::iterator p = obstacles_.find( key );
79  if ( p == obstacles_.end() ) {
80  // obstacle not found
81  ColliFastObstacle* obstacle;
82  if( is_rectangle_ )
83  obstacle = new ColliFastRectangle( width, height, cell_costs_ );
84  else
85  obstacle = new ColliFastEllipse( width, height, cell_costs_, obstacle_increasement );
86  obstacle->set_key( key );
87  obstacles_[ key ] = obstacle;
88  return obstacle->get_obstacle();
89 
90  } else {
91  // obstacle found in p (previously created obstacles)
92  return obstacles_[ key ]->get_obstacle();
93  }
94 }
95 
96 } // namespace fawkes
97 
98 #endif
ColliObstacleMap(colli_cell_cost_t cell_costs, bool is_rectangle=false)
Constructor.
Definition: obstacle_map.h:61
const std::vector< int > get_obstacle()
Return the occupied cells with their values.
Definition: obstacle.h:52
Fawkes library namespace.
This is an implementation of a a fast obstacle.
Definition: obstacle.h:41
This is an implementation of a a fast ellipse.
Definition: obstacle.h:96
Costs of occupancy-grid cells.
Definition: types.h:51
void set_key(int key)
Set key.
Definition: obstacle.h:67
This is an implementation of a a fast rectangle.
Definition: obstacle.h:87
This is an implementation of a collection of fast obstacles.
Definition: obstacle_map.h:42
const std::vector< int > get_obstacle(int width, int height, bool obstacle_increasement=true)
Get the occupied cells that match a given obstacle.
Definition: obstacle_map.h:74