23 #include "astar_search.h"
28 #include <config/config.h>
29 #include <logging/logger.h>
30 #include <utils/math/types.h>
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");
70 updated_successful_ =
false;
73 robo_position_ =
point_t(robo_x, robo_y);
80 if (robo_x < target_x)
83 if (robo_y < target_y)
86 target_position_ = astar_->remove_target_from_obstacle(target_x, target_y, step_x, step_y);
89 target_position_ =
point_t(target_x, target_y);
92 astar_->solve(robo_position_, target_position_, plan_);
94 if (plan_.size() > 0) {
95 updated_successful_ =
true;
109 return updated_successful_;
115 std::vector<point_t> *
127 return robo_position_;
137 Search::calculate_local_target()
139 point_t target = robo_position_;
142 if (plan_.size() >= 2) {
143 for (std::vector<point_t>::iterator it = plan_.begin() + 1; it != plan_.end(); ++it) {
147 if (is_obstacle_between(robo_position_, target, cfg_search_line_allowed_cost_max_)) {
155 return robo_position_;
160 Search::adjust_waypoint(
const point_t &local_target)
168 Search::calculate_local_trajec_point()
170 int x = robo_position_.
x;
171 int y = robo_position_.
y;
179 && (!is_obstacle_between(robo_position_,
point_t(x, y), max_occ))) {
192 && (!is_obstacle_between(robo_position_,
point_t(x, y), max_occ))) {
205 Search::is_obstacle_between(
const point_t &a,
const point_t &b,
const int maxcount)
207 if (a.x == b.x && a.y == b.y)
213 int _xDirInt, _yDirInt;
216 int dX = abs(endXGrid - _actXGrid);
217 (endXGrid > _actXGrid ? _xDirInt = 1 : _xDirInt = -1);
220 (endYGrid > _actYGrid ? _yDirInt = 1 : _yDirInt = -1);
221 int dY = abs(endYGrid - _actYGrid);
227 _dPru = _dPr - (dX << 1);
230 for (; (_actXGrid != endXGrid) && (_actYGrid != endYGrid); _actXGrid += _xDirInt) {
249 logger_->
log_warn(
"AStar_search",
"(line 261) ERROR IN RAYTRACER!");
251 if (count > maxcount)
254 ((_P > 0) ? _actYGrid += _yDirInt, _P += _dPru : _P += _dPr);
260 _dPru = _dPr - (dY << 1);
263 for (; (_actXGrid != endXGrid) && (_actYGrid != endYGrid); _actYGrid += _yDirInt) {
282 logger_->
log_warn(
"AStar_search",
"(line 295) ERROR IN RAYTRACER!");
284 if (count > maxcount)
287 ((_P > 0) ? _actXGrid += _xDirInt, _P += _dPru : _P += _dPr);