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