Fawkes API  Fawkes Development Version
astar.h
1 
2 /***************************************************************************
3  * astar.h - A* search implementation
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 Stefan Jacobs
7  * 2013 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_ASTAR_H_
24 #define _PLUGINS_COLLI_SEARCH_ASTAR_H_
25 
26 #include "../common/types.h"
27 #include "astar_state.h"
28 
29 #include <map>
30 #include <queue>
31 #include <vector>
32 
33 namespace fawkes {
34 
35 class LaserOccupancyGrid;
36 class Logger;
37 class Configuration;
38 
39 typedef struct point_struct point_t;
40 
41 /** Class AStar.
42  * This is an implementation of the A* search algorithm in a
43  * highly efficient way (I hope ;-).
44  */
45 class AStarColli
46 {
47 public:
48  AStarColli(LaserOccupancyGrid *occGrid, Logger *logger, Configuration *config);
50 
51  /* =========================================== */
52  /* ************* PUBLIC METHODS ************** */
53  /* =========================================== */
54  /** solves the given assignment.
55  * This starts the search for a path through the occupance grid to the
56  * target point.
57  * Performing astar search over the occupancy grid and returning the solution.
58  */
59  void solve(const point_t &robo_pos, const point_t &target_pos, std::vector<point_t> &solution);
60 
61  ///\brief Method, returning the nearest point outside of an obstacle.
62  point_t remove_target_from_obstacle(int target_x, int target_y, int step_x, int step_y);
63 
64 private:
65  /* =========================================== */
66  /* ************ PRIVATE VARIABLES ************ */
67  /* =========================================== */
68  fawkes::Logger *logger_;
69 
70  // this is the local reference to the occupancy grid.
71  LaserOccupancyGrid *occ_grid_;
72  unsigned int width_;
73  unsigned int height_;
74 
75  // Costs for the cells in grid
76  colli_cell_cost_t cell_costs_;
77 
78  // this is the local robot position and target point.
79  AStarState robo_pos_;
80  AStarState target_state_;
81 
82  // This is a state vector...
83  // It is for speed purposes. So I do not have to do a new each time
84  // I have to malloc a new one each time.
85  std::vector<AStarState *> astar_states_;
86 
87  // maximum number of states available for a* and current index
88  int max_states_;
89  int astar_state_count_;
90 
91  // this is AStars openlist
92  struct cmp
93  {
94  bool
95  operator()(AStarState *a1, AStarState *a2) const
96  {
97  return (a1->total_cost_ > a2->total_cost_);
98  }
99  };
100 
101  std::priority_queue<AStarState *, std::vector<AStarState *>, cmp> open_list_;
102 
103  // this is AStars closedList
104  std::map<int, int> closed_list_;
105 
106  /* =========================================== */
107  /* ************ PRIVATE METHODS ************** */
108  /* =========================================== */
109 
110  // search with AStar through the OccGrid
111  AStarState *search();
112 
113  // Calculate a unique key for a given coordinate
114  int calculate_key(int x, int y);
115 
116  // Check if the state is a goal
117  bool is_goal(AStarState *state);
118 
119  // Calculate heuristic for a given state
120  int heuristic(AStarState *state);
121 
122  // Generate all children for a given State
123  void generate_children(AStarState *father);
124 
125  // Generates a solution sequence for a given state
126  void get_solution_sequence(AStarState *node, std::vector<point_t> &solution);
127 };
128 
129 } // namespace fawkes
130 
131 #endif
search
fawkes::point_struct
Point with cartesian coordinates as signed integers.
Definition: types.h:40
fawkes::point_t
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
Definition: astar.h:43
fawkes::Configuration
Definition: config.h:68
fawkes::AStarColli::solve
void solve(const point_t &robo_pos, const point_t &target_pos, std::vector< point_t > &solution)
solves the given assignment.
Definition: astar.cpp:102
fawkes::Logger
Definition: logger.h:40
fawkes
fawkes::AStarColli::AStarColli
AStarColli(LaserOccupancyGrid *occGrid, Logger *logger, Configuration *config)
Constructor.
Definition: astar.cpp:52
fawkes::AStarState
Definition: astar_state.h:43
fawkes::AStarState::total_cost_
int total_cost_
The total cost.
Definition: astar_state.h:48
fawkes::colli_cell_cost_t
Costs of occupancy-grid cells.
Definition: types.h:54
fawkes::AStarColli::remove_target_from_obstacle
point_t remove_target_from_obstacle(int target_x, int target_y, int step_x, int step_y)
Method, returning the nearest point outside of an obstacle.
Definition: astar.cpp:334
fawkes::LaserOccupancyGrid
Definition: og_laser.h:51
fawkes::AStarColli
Class AStar.
Definition: astar.h:49
fawkes::AStarColli::~AStarColli
~AStarColli()
Destructor.
Definition: astar.cpp:84