Fawkes API  Fawkes Development Version
astar_search.cpp
1 
2 /***************************************************************************
3  * astar_search.cpp - A colli-specific 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 #include "astar_search.h"
24 
25 #include "astar.h"
26 #include "og_laser.h"
27 
28 #include <config/config.h>
29 #include <logging/logger.h>
30 #include <utils/math/types.h>
31 
32 namespace fawkes {
33 
34 /** @class search <plugins/colli/search/astar_search.h>
35  * This class tries to translate the found plan to interpreteable
36  * data for the rest of the program.
37  */
38 
39 /** Constructor. Constructs the plan, initializes an A* Object and
40  * makes a reference to the OccupancyGrid.
41  * @param occ_grid The laser occupancy-grid
42  * @param logger The fawkes logger
43  * @param config The fawkes configuration.
44  */
45 Search::Search(LaserOccupancyGrid *occ_grid, Logger *logger, Configuration *config)
46 : AbstractSearch(occ_grid, logger), logger_(logger)
47 {
48  logger_->log_debug("search", "(Constructor): Entering");
49  std::string cfg_prefix = "/plugins/colli/search/";
50  cfg_search_line_allowed_cost_max_ = config->get_int((cfg_prefix + "line/cost_max").c_str());
51  astar_.reset(new AStarColli(occ_grid, logger, config));
52  logger_->log_debug("search", "(Constructor): Exiting");
53 }
54 
55 /** Destructor */
57 {
58 }
59 
60 /** Perform an update by searching in the occgrid for a plan from robopos to targetpos.
61  * precondition: the occupancy grid has to be updated previously!
62  * @param robo_x Robot x position in grid
63  * @param robo_y Robot y position in grid
64  * @param target_x Target x position in grid
65  * @param target_y Target y position in grid
66  */
67 void
68 Search::update(int robo_x, int robo_y, int target_x, int target_y)
69 {
70  updated_successful_ = false;
71 
72  // check, if a position is in an obstacle
73  robo_position_ = point_t(robo_x, robo_y);
74  local_target_ = point_t(robo_x, robo_y);
75  local_trajec_ = point_t(robo_x, robo_y);
76 
77  if (occ_grid_->get_prob(target_x, target_y) == cell_costs_.occ) {
78  int step_x = 1; // initializing to 1
79  int step_y = 1;
80  if (robo_x < target_x) // if we search in the other direction, inverse it!
81  step_x = -1;
82 
83  if (robo_y < target_y)
84  step_y = -1;
85 
86  target_position_ = astar_->remove_target_from_obstacle(target_x, target_y, step_x, step_y);
87 
88  } else {
89  target_position_ = point_t(target_x, target_y);
90  }
91 
92  astar_->solve(robo_position_, target_position_, plan_);
93 
94  if (plan_.size() > 0) {
95  updated_successful_ = true;
96  local_target_ = calculate_local_target();
97  local_target_ = adjust_waypoint(local_target_);
98  local_trajec_ = calculate_local_trajec_point();
99  }
100 }
101 
102 /** Check, if the update was successful or not.
103  * precondition: update had to be called.
104  * @return true, if update was successfule.
105  */
106 bool
108 {
109  return updated_successful_;
110 }
111 
112 /** Get the current plan
113  * @return vector containing all the points in the grid along the plan
114  */
115 std::vector<point_t> *
117 {
118  return &plan_;
119 }
120 
121 /** Get the robot's position in the grid, used for the plan
122  * @return Robot's position in the grid
123  */
124 point_t
126 {
127  return robo_position_;
128 }
129 
130 /* **************************************************************************** */
131 /* **************************************************************************** */
132 /* *********** P R I V A T E - S T U F F *********************************** */
133 /* **************************************************************************** */
134 /* **************************************************************************** */
135 
136 point_t
137 Search::calculate_local_target()
138 {
139  point_t target = robo_position_;
140  point_t prev = robo_position_;
141 
142  if (plan_.size() >= 2) {
143  for (std::vector<point_t>::iterator it = plan_.begin() + 1; it != plan_.end(); ++it) {
144  prev = target;
145  target = *it;
146 
147  if (is_obstacle_between(robo_position_, target, cfg_search_line_allowed_cost_max_)) {
148  return prev;
149  }
150  }
151  return point_t(plan_.back());
152 
153  } else {
154  // return the current position if there is no plan.
155  return robo_position_;
156  }
157 }
158 
159 point_t
160 Search::adjust_waypoint(const point_t &local_target)
161 {
162  return local_target;
163 }
164 
165 // forward and backward plans should no longer make a difference in
166 // trajectory searching
167 point_t
168 Search::calculate_local_trajec_point()
169 {
170  int x = robo_position_.x;
171  int y = robo_position_.y;
172 
173  int max_occ = 10;
174 
175  if (x < local_target_.x) {
176  ++x;
177  while ((x < (int)occ_grid_->get_width()) && (x <= local_target_.x)
178  && (!is_obstacle_between(point_t(x, y), local_target_, max_occ))
179  && (!is_obstacle_between(robo_position_, point_t(x, y), max_occ))) {
180  ++x;
181  }
182 
183  if (x == local_target_.x && y == local_target_.y)
184  return point_t(x, y);
185  else
186  return point_t(x - 1, y);
187 
188  } else {
189  --x;
190  while ((x > 0) && (x >= (int)local_target_.x)
191  && (!is_obstacle_between(point_t(x, y), local_target_, max_occ))
192  && (!is_obstacle_between(robo_position_, point_t(x, y), max_occ))) {
193  --x;
194  }
195 
196  if ((x == local_target_.x) && (y == local_target_.y))
197  return point_t(x, y);
198  else
199  return point_t(x + 1, y);
200  }
201 }
202 
203 // checks per raytracing, if an obstacle is between two points.
204 bool
205 Search::is_obstacle_between(const point_t &a, const point_t &b, const int maxcount)
206 {
207  if (a.x == b.x && a.y == b.y)
208  return false;
209 
210  int count = 0;
211  float prob = 0.0;
212 
213  int _xDirInt, _yDirInt;
214  int _actXGrid = a.x;
215  int endXGrid = b.x;
216  int dX = abs(endXGrid - _actXGrid);
217  (endXGrid > _actXGrid ? _xDirInt = 1 : _xDirInt = -1);
218  int _actYGrid = a.y;
219  int endYGrid = b.y;
220  (endYGrid > _actYGrid ? _yDirInt = 1 : _yDirInt = -1);
221  int dY = abs(endYGrid - _actYGrid);
222 
223  // decide whether direction is more x or more y, and run the algorithm
224  if (dX > dY) {
225  int _P, _dPr, _dPru;
226  _dPr = dY << 1; // amount to increment decision if right is chosen (always)
227  _dPru = _dPr - (dX << 1); // amount to increment decision if up is chosen
228  _P = _dPr - dX; // decision variable start value
229 
230  for (; (_actXGrid != endXGrid) && (_actYGrid != endYGrid); _actXGrid += _xDirInt) {
231  if (_actXGrid < 0 || _actXGrid > occ_grid_->get_width() || _actYGrid < 0
232  || _actXGrid > occ_grid_->get_height()) {
233  return false;
234  }
235 
236  prob = occ_grid_->get_prob(_actXGrid, _actYGrid);
237 
238  if (prob == cell_costs_.free)
239  ;
240  else if (prob == cell_costs_.occ)
241  return true;
242  else if (prob == cell_costs_.far)
243  ++count;
244  else if (prob == cell_costs_.mid)
245  count += 2;
246  else if (prob == cell_costs_.near)
247  count += 4;
248  else
249  logger_->log_warn("AStar_search", "(line 261) ERROR IN RAYTRACER!");
250 
251  if (count > maxcount)
252  return true;
253 
254  ((_P > 0) ? _actYGrid += _yDirInt, _P += _dPru : _P += _dPr);
255  }
256 
257  } else {
258  int _P, _dPr, _dPru;
259  _dPr = dX << 1; // amount to increment decision if right is chosen (always)
260  _dPru = _dPr - (dY << 1); // amount to increment decision if up is chosen
261  _P = _dPr - dY; // decision variable start value
262 
263  for (; (_actXGrid != endXGrid) && (_actYGrid != endYGrid); _actYGrid += _yDirInt) {
264  if (_actXGrid < 0 || _actXGrid > occ_grid_->get_width() || _actYGrid < 0
265  || _actXGrid > occ_grid_->get_height()) {
266  return false;
267  }
268 
269  prob = occ_grid_->get_prob(_actXGrid, _actYGrid);
270 
271  if (prob == cell_costs_.free)
272  ;
273  else if (prob == cell_costs_.occ)
274  return true;
275  else if (prob == cell_costs_.far)
276  ++count;
277  else if (prob == cell_costs_.mid)
278  count += 2;
279  else if (prob == cell_costs_.near)
280  count += 4;
281  else
282  logger_->log_warn("AStar_search", "(line 295) ERROR IN RAYTRACER!");
283 
284  if (count > maxcount)
285  return true;
286 
287  ((_P > 0) ? _actXGrid += _xDirInt, _P += _dPru : _P += _dPr);
288  }
289  }
290 
291  return false; // there is no obstacle between those two points.
292 }
293 
294 } // namespace fawkes
fawkes::point_struct
Point with cartesian coordinates as signed integers.
Definition: types.h:40
fawkes::colli_cell_cost_t::near
unsigned int near
The cost for a cell near an obstacle (distance="near")
Definition: types.h:57
fawkes::colli_cell_cost_t::mid
unsigned int mid
The cost for a cell near an obstacle (distance="near")
Definition: types.h:58
fawkes::OccupancyGrid::get_width
int get_width()
Get the width of the grid.
Definition: occupancygrid.cpp:83
fawkes::colli_cell_cost_t::occ
unsigned int occ
The cost for an occupied cell.
Definition: types.h:56
fawkes::Configuration::get_int
virtual int get_int(const char *path)=0
fawkes::AbstractSearch::local_target_
point_t local_target_
the calculated target where to drive to
Definition: abstract_search.h:77
fawkes::point_t
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
Definition: astar.h:43
fawkes::AbstractSearch::local_trajec_
point_t local_trajec_
the calculated trajectory where to drive to
Definition: abstract_search.h:78
fawkes::AbstractSearch::cell_costs_
colli_cell_cost_t cell_costs_
The costs for cells in occupancy grid.
Definition: abstract_search.h:80
fawkes::OccupancyGrid::get_prob
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
Definition: occupancygrid.cpp:168
fawkes
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::Search::get_plan
std::vector< point_t > * get_plan()
Get the current plan.
Definition: astar_search.cpp:120
fawkes::colli_cell_cost_t::free
unsigned int free
The cost for a free cell.
Definition: types.h:60
fawkes::point_struct::y
int y
y coordinate
Definition: types.h:43
fawkes::Search::Search
Search(LaserOccupancyGrid *occ_grid, Logger *logger, Configuration *config)
Constructor.
Definition: astar_search.cpp:49
fawkes::AbstractSearch::occ_grid_
LaserOccupancyGrid * occ_grid_
The occupancy grid.
Definition: abstract_search.h:75
fawkes::point_struct::x
int x
x coordinate
Definition: types.h:42
fawkes::AStarColli
Class AStar.
Definition: astar.h:49
fawkes::Search::~Search
virtual ~Search()
Destructor.
Definition: astar_search.cpp:60
fawkes::Search::get_robot_position
point_t get_robot_position()
Get the robot's position in the grid, used for the plan.
Definition: astar_search.cpp:129
fawkes::OccupancyGrid::get_height
int get_height()
Get the height of the grid.
Definition: occupancygrid.cpp:92
fawkes::Search::updated_successful
bool updated_successful()
returns, if the update was successful or not.
Definition: astar_search.cpp:111
fawkes::colli_cell_cost_t::far
unsigned int far
The cost for a cell near an obstacle (distance="near")
Definition: types.h:59
fawkes::Search::update
void update(int robo_x, int robo_y, int target_x, int target_y)
update complete plan things
Definition: astar_search.cpp:72