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