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_;
115std::vector<point_t> *
127 return robo_position_;
137Search::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_;
160Search::adjust_waypoint(
const point_t &local_target)
168Search::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))) {
205Search::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);
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.
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 ...
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.
unsigned int far
The cost for a cell near an obstacle (distance="near")
unsigned int occ
The cost for an occupied cell.
unsigned int mid
The cost for a cell near an obstacle (distance="near")
unsigned int near
The cost for a cell near an obstacle (distance="near")
unsigned int free
The cost for a free cell.
Point with cartesian coordinates as signed integers.