24 #include "../search/og_laser.h" 26 #include <utils/math/types.h> 27 #include <logging/logger.h> 28 #include <config/config.h> 58 logger_->
log_debug(
"AStar",
"(Constructor): Initializing AStar");
60 max_states_ = config->
get_int(
"/plugins/colli/search/a_star/max_states" );
68 astar_state_count_ = 0;
69 astar_states_.reserve( max_states_ );
71 for(
int i = 0; i < max_states_; i++) {
73 astar_states_[i] = state;
76 while ( open_list_.size() > 0 )
81 logger_->
log_debug(
"AStar",
"(Constructor): Initializing AStar done");
91 logger_->
log_debug(
"AStar",
"(Destructor): Destroying AStar");
92 for(
int i = 0; i < max_states_; i++ )
93 delete astar_states_[i];
94 logger_->
log_debug(
"AStar",
"(Destructor): Destroying AStar done");
112 astar_state_count_ = 0;
113 while ( open_list_.size() > 0 )
115 closed_list_.clear();
119 robo_pos_.
x_ = robo_pos.
x;
120 robo_pos_.
y_ = robo_pos.
y;
121 target_state_.
x_ = target_pos.
x;
122 target_state_.
y_ = target_pos.
y;
125 AStarState * initial_state = astar_states_[++astar_state_count_];
126 initial_state->
x_ = robo_pos_.
x_;
127 initial_state->
y_ = robo_pos_.
y_;
130 initial_state->
total_cost_ = heuristic( initial_state );
133 open_list_.push( initial_state );
134 get_solution_sequence(
search(), solution );
154 while ( open_list_.size() > 0 ) {
156 if ( open_list_.size() > 0 ) {
157 best = open_list_.top();
163 if ( is_goal( best ) )
165 else if ( astar_state_count_ > max_states_ - 6 ) {
166 logger_->
log_warn(
"AStar",
"**** Warning: Out of states! Increasing A* MaxStates!");
168 for(
int i = 0; i < max_states_; i++ )
169 delete astar_states_[i];
171 max_states_ += (int)(max_states_/3.0);
173 astar_states_.clear();
174 astar_states_.resize( max_states_ );
175 for(
int i = 0; i < max_states_; i++) {
177 astar_states_[i] = best;
179 logger_->
log_warn(
"AStar",
"**** Increasing done!");
184 generate_children( best );
200 AStar::calculate_key(
int x,
int y )
202 return (x << 15) | y;
212 AStar::generate_children(
AStarState * father )
219 if ( father->
y_ > 0 ) {
221 if ( prob != cell_costs_.
occ ) {
222 child = astar_states_[++astar_state_count_];
223 child->
x_ = father->
x_;
224 child->
y_ = father->
y_-1;
225 key = calculate_key( child->
x_, child->
y_ );
226 if ( closed_list_.find( key ) == closed_list_.end() ) {
230 open_list_.push( child );
231 closed_list_[key] = key;
234 --astar_state_count_;
238 if ( father->
y_ < (
signed int)height_ ) {
240 if ( prob != cell_costs_.
occ ) {
241 child = astar_states_[++astar_state_count_];
242 child->
x_ = father->
x_;
243 child->
y_ = father->
y_+1;
244 key = calculate_key( child->
x_, child->
y_ );
245 if ( closed_list_.find( key ) == closed_list_.end() ) {
249 open_list_.push( child );
250 closed_list_[key] = key;
253 --astar_state_count_;
257 if ( father->
x_ > 0 ) {
259 if ( prob != cell_costs_.
occ ) {
260 child = astar_states_[++astar_state_count_];
261 child->
x_ = father->
x_-1;
262 child->
y_ = father->
y_;
263 key = calculate_key( child->
x_, child->
y_ );
264 if ( closed_list_.find( key ) == closed_list_.end() ) {
268 open_list_.push( child );
269 closed_list_[key] = key;
272 --astar_state_count_;
276 if ( father->
x_ < (
signed int)width_ ) {
278 if ( prob != cell_costs_.
occ ) {
279 child = astar_states_[++astar_state_count_];
280 child->
x_ = father->
x_+1;
281 child->
y_ = father->
y_;
282 key = calculate_key( child->
x_, child->
y_ );
283 if ( closed_list_.find( key ) == closed_list_.end() ) {
287 open_list_.push( child );
288 closed_list_[key] = key;
291 --astar_state_count_;
307 return (
int)( abs( state->
x_ - target_state_.
x_ ) +
308 abs( state->
y_ - target_state_.
y_ ) );
318 return ( (target_state_.
x_ == state->
x_) &&
319 (target_state_.
y_ == state->
y_) );
328 AStar::get_solution_sequence(
AStarState * node, vector<point_t> &solution )
331 while ( state != 0 ) {
332 solution.insert( solution.begin(),
point_t( state->
x_, state->
y_ ) );
356 while ( open_list_.size() > 0 )
359 closed_list_.clear();
360 astar_state_count_ = 0;
362 AStarState * initial_state = astar_states_[++astar_state_count_];
363 initial_state->
x_ = target_x;
364 initial_state->
y_ = target_y;
366 open_list_.push( initial_state );
372 while ( !(open_list_.empty()) && (astar_state_count_ < max_states_ - 6) ) {
373 father = open_list_.top();
375 key = calculate_key( father->
x_, father->
y_ );
377 if ( closed_list_.find( key ) == closed_list_.end() ) {
378 closed_list_[key] = key;
381 if ( (father->
x_ > 1) && ( father->
x_ < (signed)width_-2 ) ) {
382 child = astar_states_[++astar_state_count_];
383 child->
x_ = father->
x_ + step_x;
384 child->
y_ = father->
y_;
386 key = calculate_key( child->
x_, child->
y_ );
389 else if ( closed_list_.find( key ) == closed_list_.end() )
390 open_list_.push( child );
393 if ( (father->
y_ > 1) && (father->
y_ < (
signed)height_-2) ) {
394 child = astar_states_[++astar_state_count_];
395 child->
x_ = father->
x_;
396 child->
y_ = father->
y_ + step_y;
398 key = calculate_key( child->
x_, child->
y_ );
401 else if ( closed_list_.find( key ) == closed_list_.end() )
402 open_list_.push( child );
408 logger_->
log_debug(
"AStar",
"Failed to get a modified targetpoint");
409 return point_t( target_x, target_y );
This is the abstract(!) class for an A* State.
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
int get_width()
Get the width of the grid.
int total_cost_
The total cost.
int get_height()
Get the height of the grid.
This class tries to translate the found plan to interpreteable data for the rest of the program...
Fawkes library namespace.
int past_cost_
The past cost.
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
std::vector< AStarState * > solve(AStarState *initialState)
Solves a situation given by the initial state with AStar, and returns a vector of AStarStates that so...
colli_cell_cost_t get_cell_costs() const
Get cell costs.
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 ...
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.
unsigned int occ
The cost for an occupied cell.
int x_
x coordinate of the state
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.
int y_
y coordinate of the state
Point with cartesian coordinates as signed integers.
AStarState * father_
The predecessor state.
Interface for configuration handling.
unsigned int near
The cost for a cell near an obstacle (distance="near")