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