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