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
31using namespace std;
32
33namespace 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 */
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 */
101void
102AStarColli::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 */
139AStarColli::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 */
188int
189AStarColli::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 */
199void
200AStarColli::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 */
289int
290AStarColli::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 */
299bool
300AStarColli::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 */
309void
310AStarColli::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 */
334AStarColli::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
AStarColli(LaserOccupancyGrid *occGrid, Logger *logger, Configuration *config)
Constructor.
Definition: astar.cpp:52
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