Fawkes API  Fawkes Development Version
astar.cpp
1 
2 /***************************************************************************
3  * astar.cpp - A* search implementation
4  *
5  * Created: Fri Oct 18 15:16:23 2013
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 #include "astar.h"
24 #include "../search/og_laser.h"
25 
26 #include <utils/math/types.h>
27 #include <logging/logger.h>
28 #include <config/config.h>
29 
30 using namespace std;
31 
32 namespace fawkes
33 {
34 #if 0 /* just to make Emacs auto-indent happy */
35 }
36 #endif
37 
38 /** @class AStar <plugins/colli/search/astar.h>
39  * This is a high efficient implementation. Therefore this code
40  * does not always look very nice here. So be patient and try to
41  * understand what I was trying to implement here.
42  */
43 
44 /** Constructor.
45  * This constructor does several things ;-)
46  * It gets an occupancy grid for the local pointer to garant fast access,
47  * and queries the settings for the grid.
48  * After that several states are initialized. This is done for speed purposes
49  * again, cause only here new is called in this code..
50  * Afterwards the Openlist, closedlist and states for A* are initialized.
51  * @param occGrid is a pointer to an LaserOccupancyGrid to search through.
52  * @param logger The fawkes logger
53  * @param config The fawkes configuration
54  */
55 AStar::AStar( LaserOccupancyGrid * occGrid, Logger* logger, Configuration* config )
56  : logger_( logger )
57 {
58  logger_->log_debug("AStar", "(Constructor): Initializing AStar");
59 
60  max_states_ = config->get_int( "/plugins/colli/search/a_star/max_states" );
61 
62  occ_grid_ = occGrid;
63  width_ = occ_grid_->get_width() - 1;
64  height_ = occ_grid_->get_height() - 1;
65 
66  cell_costs_ = occ_grid_->get_cell_costs();
67 
68  astar_state_count_ = 0;
69  astar_states_.reserve( max_states_ );
70 
71  for( int i = 0; i < max_states_; i++) {
72  AStarState * state = new AStarState();
73  astar_states_[i] = state;
74  }
75 
76  while ( open_list_.size() > 0 )
77  open_list_.pop();
78 
79  closed_list_.clear();
80 
81  logger_->log_debug("AStar", "(Constructor): Initializing AStar done");
82 }
83 
84 
85 
86 /** Destructor.
87  * This destructor deletes all the states allocated during construction.
88  */
90 {
91  logger_->log_debug("AStar", "(Destructor): Destroying AStar");
92  for( int i = 0; i < max_states_; i++ )
93  delete astar_states_[i];
94  logger_->log_debug("AStar", "(Destructor): Destroying AStar done");
95 }
96 
97 
98 
99 /** solve.
100  * solve is the externally called method to solve the assignment by A*.
101  * It generates an initial state, and sets its values accordingly.
102  * This state is put on the openlist, and then the search is called, after which
103  * the solution sequence is generated by the pointers in all states.
104  * @param robo_pos The position of the robot in the grid
105  * @param target_pos The position of the target in the grid
106  * @param solution a vector that will be filled with the found path
107  */
108 void
109 AStar::solve( const point_t &robo_pos, const point_t &target_pos, vector<point_t> &solution )
110 {
111  // initialize counter, vectors/lists/queues
112  astar_state_count_ = 0;
113  while ( open_list_.size() > 0 )
114  open_list_.pop();
115  closed_list_.clear();
116  solution.clear();
117 
118  // setting start coordinates
119  robo_pos_.x_ = robo_pos.x;
120  robo_pos_.y_ = robo_pos.y;
121  target_state_.x_ = target_pos.x;
122  target_state_.y_ = target_pos.y;
123 
124  // generating initialstate
125  AStarState * initial_state = astar_states_[++astar_state_count_];
126  initial_state->x_ = robo_pos_.x_;
127  initial_state->y_ = robo_pos_.y_;
128  initial_state->father_ = 0;
129  initial_state->past_cost_ = 0;
130  initial_state->total_cost_ = heuristic( initial_state );
131 
132  // performing search
133  open_list_.push( initial_state );
134  get_solution_sequence( search(), solution );
135 }
136 
137 
138 
139 /* =========================================== */
140 /* *************** PRIVATE PART ************** */
141 /* =========================================== */
142 
143 
144 /** search.
145  * This is the magic A* algorithm.
146  * Its really easy, you can find it like this everywhere.
147  */
148 AStarState*
149 AStar::search( )
150 {
151  AStarState * best = 0;
152 
153  // while the openlist not is empty
154  while ( open_list_.size() > 0 ) {
155  // get best state
156  if ( open_list_.size() > 0 ) {
157  best = open_list_.top();
158  open_list_.pop( );
159  } else
160  return 0;
161 
162  // check if its a goal.
163  if ( is_goal( best ) )
164  return best;
165  else if ( astar_state_count_ > max_states_ - 6 ) {
166  logger_->log_warn("AStar", "**** Warning: Out of states! Increasing A* MaxStates!");
167 
168  for( int i = 0; i < max_states_; i++ )
169  delete astar_states_[i];
170 
171  max_states_ += (int)(max_states_/3.0);
172 
173  astar_states_.clear();
174  astar_states_.resize( max_states_ );
175  for( int i = 0; i < max_states_; i++) {
176  best = new AStarState();
177  astar_states_[i] = best;
178  }
179  logger_->log_warn("AStar", "**** Increasing done!");
180  return 0;
181  }
182 
183  // generate all its children
184  generate_children( best );
185  }
186 
187  return 0;
188 }
189 
190 
191 /** calculate_key.
192  * This method produces one unique key for a state for
193  * putting this on the closed list.
194  * It has to be really fast, so the function is not so readable.
195  * What it does is the following: x * 2^14 + y. This is unique,
196  * because first it does a bit shift for 14 bits, and adds (or)
197  * afterwards a number that is smaller tham 14 bits!
198  */
199 int
200 AStar::calculate_key( int x, int y )
201 {
202  return (x << 15) | y; // This line is a crime! But fast ;-)
203 }
204 
205 
206 /** generate_children.
207  * This method generates all children for a given state.
208  * This is done with a little range checking and rule checking.
209  * Afterwards these children are put on the openlist.
210  */
211 void
212 AStar::generate_children( AStarState * father )
213 {
214  AStarState * child;
215  int key;
216 
217  float prob;
218 
219  if ( father->y_ > 0 ) {
220  prob = occ_grid_->get_prob( father->x_, father->y_-1 );
221  if ( prob != cell_costs_.occ ) {
222  child = astar_states_[++astar_state_count_];
223  child->x_ = father->x_;
224  child->y_ = father->y_-1;
225  key = calculate_key( child->x_, child->y_ );
226  if ( closed_list_.find( key ) == closed_list_.end() ) {
227  child->father_ = father;
228  child->past_cost_ = father->past_cost_ + (int)prob;
229  child->total_cost_ = child->past_cost_ + heuristic( child );
230  open_list_.push( child );
231  closed_list_[key] = key;
232 
233  } else
234  --astar_state_count_;
235  }
236  }
237 
238  if ( father->y_ < (signed int)height_ ) {
239  prob = occ_grid_->get_prob( father->x_, father->y_+1 );
240  if ( prob != cell_costs_.occ ) {
241  child = astar_states_[++astar_state_count_];
242  child->x_ = father->x_;
243  child->y_ = father->y_+1;
244  key = calculate_key( child->x_, child->y_ );
245  if ( closed_list_.find( key ) == closed_list_.end() ) {
246  child->father_ = father;
247  child->past_cost_ = father->past_cost_ + (int)prob;
248  child->total_cost_ = child->past_cost_ + heuristic( child );
249  open_list_.push( child );
250  closed_list_[key] = key;
251 
252  } else
253  --astar_state_count_;
254  }
255  }
256 
257  if ( father->x_ > 0 ) {
258  prob = occ_grid_->get_prob( father->x_-1, father->y_ );
259  if ( prob != cell_costs_.occ ) {
260  child = astar_states_[++astar_state_count_];
261  child->x_ = father->x_-1;
262  child->y_ = father->y_;
263  key = calculate_key( child->x_, child->y_ );
264  if ( closed_list_.find( key ) == closed_list_.end() ) {
265  child->father_ = father;
266  child->past_cost_ = father->past_cost_ + (int)prob;
267  child->total_cost_ = child->past_cost_ + heuristic( child );
268  open_list_.push( child );
269  closed_list_[key] = key;
270 
271  } else
272  --astar_state_count_;
273  }
274  }
275 
276  if ( father->x_ < (signed int)width_ ) {
277  prob = occ_grid_->get_prob( father->x_+1, father->y_ );
278  if ( prob != cell_costs_.occ ) {
279  child = astar_states_[++astar_state_count_];
280  child->x_ = father->x_+1;
281  child->y_ = father->y_;
282  key = calculate_key( child->x_, child->y_ );
283  if ( closed_list_.find( key ) == closed_list_.end() ) {
284  child->father_ = father;
285  child->past_cost_ = father->past_cost_ + (int)prob;
286  child->total_cost_ = child->past_cost_ + heuristic( child );
287  open_list_.push( child );
288  closed_list_[key] = key;
289 
290  } else
291  --astar_state_count_;
292  }
293  }
294 
295 }
296 
297 
298 /** heuristic.
299  * This method calculates the heuristic value for a given
300  * state. This is done by the manhatten distance here,
301  * because we are calculating on a grid...
302  */
303 int
304 AStar::heuristic( AStarState * state )
305 {
306  // return (int)( abs( state->x_ - target_state_.x_ ));
307  return (int)( abs( state->x_ - target_state_.x_ ) +
308  abs( state->y_ - target_state_.y_ ) );
309 }
310 
311 
312 /** is_goal.
313  * This method checks, if a state is a goal state.
314  */
315 bool
316 AStar::is_goal( AStarState * state )
317 {
318  return ( (target_state_.x_ == state->x_) &&
319  (target_state_.y_ == state->y_) );
320 }
321 
322 
323 /** get_solution_sequence.
324  * This one enqueues the way of a node back to its root through the
325  * tree into the solution/plan vector.
326  */
327 void
328 AStar::get_solution_sequence( AStarState * node, vector<point_t> &solution )
329 {
330  AStarState * state = node;
331  while ( state != 0 ) {
332  solution.insert( solution.begin(), point_t( state->x_, state->y_ ) );
333  state = state->father_;
334  }
335  //logger_->log_debug("AStar", "(get_solution_sequence): Solutionsize=%u , Used states=%i",
336  // solution.size(), astar_state_count_);
337 }
338 
339 
340 /* =========================================================================== */
341 /* =========================================================================== */
342 /* ** ** ** ** ** ASTAR STUFF END HERE ** ** ** ** ** */
343 /* =========================================================================== */
344 /* =========================================================================== */
345 /** Method, returning the nearest point outside of an obstacle.
346  * @param target_x target x position
347  * @param target_y target y position
348  * @param step_x step size in x direction
349  * @param step_y step size in y direction
350  * @return a new modified point.
351  */
352 point_t
353 AStar::remove_target_from_obstacle( int target_x, int target_y, int step_x, int step_y )
354 {
355  // initializing lists...
356  while ( open_list_.size() > 0 )
357  open_list_.pop();
358 
359  closed_list_.clear();
360  astar_state_count_ = 0;
361  // starting fill algorithm by putting first state in openlist
362  AStarState * initial_state = astar_states_[++astar_state_count_];
363  initial_state->x_ = target_x;
364  initial_state->y_ = target_y;
365  initial_state->total_cost_ = 0;
366  open_list_.push( initial_state );
367  // search algorithm by gridfilling
368  AStarState * child;
369  AStarState * father;
370  int key;
371 
372  while ( !(open_list_.empty()) && (astar_state_count_ < max_states_ - 6) ) {
373  father = open_list_.top();
374  open_list_.pop();
375  key = calculate_key( father->x_, father->y_ );
376 
377  if ( closed_list_.find( key ) == closed_list_.end() ) {
378  closed_list_[key] = key;
379  // generiere zwei kinder. wenn besetzt, pack sie an das ende
380  // der openlist mit kosten + 1, sonst return den Knoten
381  if ( (father->x_ > 1) && ( father->x_ < (signed)width_-2 ) ) {
382  child = astar_states_[++astar_state_count_];
383  child->x_ = father->x_ + step_x;
384  child->y_ = father->y_;
385  child->total_cost_ = father->total_cost_+1;
386  key = calculate_key( child->x_, child->y_ );
387  if ( occ_grid_->get_prob( child->x_, child->y_ ) == cell_costs_.near )
388  return point_t( child->x_, child->y_ );
389  else if ( closed_list_.find( key ) == closed_list_.end() )
390  open_list_.push( child );
391  }
392 
393  if ( (father->y_ > 1) && (father->y_ < (signed)height_-2) ) {
394  child = astar_states_[++astar_state_count_];
395  child->x_ = father->x_;
396  child->y_ = father->y_ + step_y;
397  child->total_cost_ = father->total_cost_+1;
398  key = calculate_key( child->x_, child->y_ );
399  if ( occ_grid_->get_prob( child->x_, child->y_ ) == cell_costs_.near )
400  return point_t( child->x_, child->y_ );
401  else if ( closed_list_.find( key ) == closed_list_.end() )
402  open_list_.push( child );
403  }
404  }
405 
406  }
407 
408  logger_->log_debug("AStar", "Failed to get a modified targetpoint");
409  return point_t( target_x, target_y );
410 }
411 
412 } // namespace fawkes
This is the abstract(!) class for an A* State.
Definition: astar_state.h:38
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
Definition: astar.h:43
int get_width()
Get the width of the grid.
int total_cost_
The total cost.
Definition: astar_state.h:48
int get_height()
Get the height of the grid.
This class tries to translate the found plan to interpreteable data for the rest of the program...
Fawkes library namespace.
int past_cost_
The past cost.
Definition: astar_state.h:47
~AStar()
Destructor.
Definition: astar.cpp:63
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
STL namespace.
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
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:442
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
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
unsigned int occ
The cost for an occupied cell.
Definition: types.h:52
int y
y coordinate
Definition: types.h:42
int x_
x coordinate of the state
Definition: astar_state.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
int y_
y coordinate of the state
Definition: astar_state.h:43
Point with cartesian coordinates as signed integers.
Definition: types.h:40
AStarState * father_
The predecessor state.
Definition: astar_state.h:45
Interface for configuration handling.
Definition: config.h:67
unsigned int near
The cost for a cell near an obstacle (distance="near")
Definition: types.h:53
int x
x coordinate
Definition: types.h:41
Interface for logging.
Definition: logger.h:34