23 #include "astar_search.h" 27 #include <utils/math/types.h> 28 #include <logging/logger.h> 29 #include <config/config.h> 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");
75 updated_successful_ =
false;
78 robo_position_ =
point_t( robo_x, robo_y );
85 if ( robo_x < target_x )
88 if ( robo_y < target_y )
94 target_position_ =
point_t( target_x, target_y );
97 astar_->
solve( robo_position_, target_position_, plan_ );
99 if (plan_.size() > 0) {
100 updated_successful_ =
true;
115 return updated_successful_;
121 std::vector<point_t>*
133 return robo_position_;
145 Search::calculate_local_target()
147 point_t target = robo_position_;
150 if( plan_.size() >= 2 ) {
151 for ( std::vector<point_t>::iterator it = plan_.begin()+1; it != plan_.end(); ++it ) {
155 if( is_obstacle_between( robo_position_, target, cfg_search_line_allowed_cost_max_ ) ) {
159 return point_t( plan_.back() );
163 return robo_position_;
169 Search::adjust_waypoint(
const point_t &local_target )
179 Search::calculate_local_trajec_point( )
181 int x = robo_position_.
x;
182 int y = robo_position_.
y;
191 && (!is_obstacle_between( robo_position_,
point_t(x, y), max_occ ) ) )
206 && (!is_obstacle_between( robo_position_,
point_t(x, y), max_occ ) ) )
221 Search::is_obstacle_between(
const point_t &a,
const point_t &b,
const int maxcount )
223 if (a.
x == b.
x && a.
y == b.
y)
229 int _xDirInt, _yDirInt;
232 int dX = abs(endXGrid - _actXGrid);
233 ( endXGrid > _actXGrid ? _xDirInt = 1 : _xDirInt = -1 );
236 ( endYGrid > _actYGrid ? _yDirInt = 1 : _yDirInt = -1 );
237 int dY = abs(endYGrid - _actYGrid);
243 _dPru = _dPr - (dX<<1);
246 for ( ; (_actXGrid != endXGrid) && (_actYGrid != endYGrid); _actXGrid += _xDirInt ) {
266 logger_->
log_warn(
"AStar_search",
"(line 261) ERROR IN RAYTRACER!");
268 if ( count > maxcount )
271 ( ( _P > 0 ) ? _actYGrid += _yDirInt, _P += _dPru : _P += _dPr );
277 _dPru = _dPr - (dY<<1);
280 for ( ; (_actXGrid != endXGrid) && (_actYGrid != endYGrid); _actYGrid += _yDirInt ) {
300 logger_->
log_warn(
"AStar_search",
"(line 295) ERROR IN RAYTRACER!");
302 if ( count > maxcount )
305 ( ( _P > 0 ) ? _actXGrid += _xDirInt, _P += _dPru : _P += _dPr );
point_t local_trajec_
the calculated trajectory where to drive to
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
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...
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")
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
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.
bool updated_successful()
returns, if the update was successful or not.
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's position in the grid, used for the plan.
unsigned int free
The cost for a free cell.
unsigned int occ
The cost for an occupied cell.
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.
unsigned int mid
The cost for a cell near an obstacle (distance="near")
point_t local_target_
the calculated target where to drive to
Interface for configuration handling.
unsigned int near
The cost for a cell near an obstacle (distance="near")