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
32namespace 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 */
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 */
67void
68Search::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 */
106bool
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 */
115std::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 */
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
137Search::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
160Search::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
168Search::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.
204bool
205Search::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
Class AStar.
Definition: astar.h:46
This is the abstract search interpretation class for an arbitrary search algorithm to find its way th...
colli_cell_cost_t cell_costs_
The costs for cells in occupancy grid.
point_t local_target_
the calculated target where to drive to
point_t local_trajec_
the calculated trajectory where to drive to
LaserOccupancyGrid * occ_grid_
The occupancy grid.
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
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.
void update(int robo_x, int robo_y, int target_x, int target_y)
update complete plan things
std::vector< point_t > * get_plan()
Get the current plan.
point_t get_robot_position()
Get the robot's position in the grid, used for the plan.
bool updated_successful()
returns, if the update was successful or not.
virtual ~Search()
Destructor.
Search(LaserOccupancyGrid *occ_grid, Logger *logger, Configuration *config)
Constructor.
Fawkes library namespace.
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
Definition: astar.h:39
unsigned int far
The cost for a cell near an obstacle (distance="near")
Definition: types.h:54
unsigned int occ
The cost for an occupied cell.
Definition: types.h:51
unsigned int mid
The cost for a cell near an obstacle (distance="near")
Definition: types.h:53
unsigned int near
The cost for a cell near an obstacle (distance="near")
Definition: types.h:52
unsigned int free
The cost for a free cell.
Definition: types.h:55
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