25 #include "../search/og_laser.h"
27 #include <config/config.h>
28 #include <logging/logger.h>
29 #include <utils/math/types.h>
55 logger_->
log_debug(
"AStar",
"(Constructor): Initializing AStar");
57 max_states_ = config->
get_int(
"/plugins/colli/search/a_star/max_states");
65 astar_state_count_ = 0;
66 astar_states_.reserve(max_states_);
68 for (
int i = 0; i < max_states_; i++) {
70 astar_states_[i] = state;
73 while (open_list_.size() > 0)
78 logger_->
log_debug(
"AStar",
"(Constructor): Initializing AStar done");
86 logger_->
log_debug(
"AStar",
"(Destructor): Destroying AStar");
87 for (
int i = 0; i < max_states_; i++)
88 delete astar_states_[i];
89 logger_->
log_debug(
"AStar",
"(Destructor): Destroying AStar done");
105 astar_state_count_ = 0;
106 while (open_list_.size() > 0)
108 closed_list_.clear();
112 robo_pos_.
x_ = robo_pos.
x;
113 robo_pos_.
y_ = robo_pos.
y;
114 target_state_.
x_ = target_pos.
x;
115 target_state_.
y_ = target_pos.
y;
118 AStarState *initial_state = astar_states_[++astar_state_count_];
119 initial_state->
x_ = robo_pos_.
x_;
120 initial_state->
y_ = robo_pos_.
y_;
123 initial_state->
total_cost_ = heuristic(initial_state);
126 open_list_.push(initial_state);
127 get_solution_sequence(
search(), solution);
144 while (open_list_.size() > 0) {
146 if (open_list_.size() > 0) {
147 best = open_list_.top();
155 else if (astar_state_count_ > max_states_ - 6) {
156 logger_->
log_warn(
"AStar",
"**** Warning: Out of states! Increasing A* MaxStates!");
158 for (
int i = 0; i < max_states_; i++)
159 delete astar_states_[i];
161 max_states_ += (int)(max_states_ / 3.0);
163 astar_states_.clear();
164 astar_states_.resize(max_states_);
165 for (
int i = 0; i < max_states_; i++) {
166 best =
new AStarState();
167 astar_states_[i] = best;
169 logger_->
log_warn(
"AStar",
"**** Increasing done!");
174 generate_children(best);
189 AStarColli::calculate_key(
int x,
int y)
191 return (x << 15) | y;
200 AStarColli::generate_children(AStarState *father)
207 if (father->y_ > 0) {
208 prob = occ_grid_->
get_prob(father->x_, father->y_ - 1);
209 if (prob != cell_costs_.
occ) {
210 child = astar_states_[++astar_state_count_];
211 child->x_ = father->x_;
212 child->y_ = father->y_ - 1;
213 key = calculate_key(child->x_, child->y_);
214 if (closed_list_.find(key) == closed_list_.end()) {
215 child->father_ = father;
216 child->past_cost_ = father->past_cost_ + (int)prob;
217 child->total_cost_ = child->past_cost_ + heuristic(child);
218 open_list_.push(child);
219 closed_list_[key] = key;
222 --astar_state_count_;
226 if (father->y_ < (
signed int)height_) {
227 prob = occ_grid_->
get_prob(father->x_, father->y_ + 1);
228 if (prob != cell_costs_.
occ) {
229 child = astar_states_[++astar_state_count_];
230 child->x_ = father->x_;
231 child->y_ = father->y_ + 1;
232 key = calculate_key(child->x_, child->y_);
233 if (closed_list_.find(key) == closed_list_.end()) {
234 child->father_ = father;
235 child->past_cost_ = father->past_cost_ + (int)prob;
236 child->total_cost_ = child->past_cost_ + heuristic(child);
237 open_list_.push(child);
238 closed_list_[key] = key;
241 --astar_state_count_;
245 if (father->x_ > 0) {
246 prob = occ_grid_->
get_prob(father->x_ - 1, father->y_);
247 if (prob != cell_costs_.
occ) {
248 child = astar_states_[++astar_state_count_];
249 child->x_ = father->x_ - 1;
250 child->y_ = father->y_;
251 key = calculate_key(child->x_, child->y_);
252 if (closed_list_.find(key) == closed_list_.end()) {
253 child->father_ = father;
254 child->past_cost_ = father->past_cost_ + (int)prob;
255 child->total_cost_ = child->past_cost_ + heuristic(child);
256 open_list_.push(child);
257 closed_list_[key] = key;
260 --astar_state_count_;
264 if (father->x_ < (
signed int)width_) {
265 prob = occ_grid_->
get_prob(father->x_ + 1, father->y_);
266 if (prob != cell_costs_.
occ) {
267 child = astar_states_[++astar_state_count_];
268 child->x_ = father->x_ + 1;
269 child->y_ = father->y_;
270 key = calculate_key(child->x_, child->y_);
271 if (closed_list_.find(key) == closed_list_.end()) {
272 child->father_ = father;
273 child->past_cost_ = father->past_cost_ + (int)prob;
274 child->total_cost_ = child->past_cost_ + heuristic(child);
275 open_list_.push(child);
276 closed_list_[key] = key;
279 --astar_state_count_;
290 AStarColli::heuristic(AStarState *state)
293 return (
int)(abs(state->x_ - target_state_.
x_) + abs(state->y_ - target_state_.
y_));
300 AStarColli::is_goal(AStarState *state)
302 return ((target_state_.
x_ == state->x_) && (target_state_.
y_ == state->y_));
310 AStarColli::get_solution_sequence(AStarState *node, vector<point_t> &solution)
312 AStarState *state = node;
314 solution.insert(solution.begin(),
point_t(state->x_, state->y_));
315 state = state->father_;
337 while (open_list_.size() > 0)
340 closed_list_.clear();
341 astar_state_count_ = 0;
343 AStarState *initial_state = astar_states_[++astar_state_count_];
344 initial_state->
x_ = target_x;
345 initial_state->
y_ = target_y;
347 open_list_.push(initial_state);
352 while (!(open_list_.empty()) && (astar_state_count_ < max_states_ - 6)) {
353 father = open_list_.top();
355 int key = calculate_key(father->
x_, father->
y_);
357 if (closed_list_.find(key) == closed_list_.end()) {
358 closed_list_[key] = key;
361 if ((father->
x_ > 1) && (father->
x_ < (signed)width_ - 2)) {
362 child = astar_states_[++astar_state_count_];
363 child->
x_ = father->
x_ + step_x;
364 child->
y_ = father->
y_;
366 key = calculate_key(child->
x_, child->
y_);
369 else if (closed_list_.find(key) == closed_list_.end())
370 open_list_.push(child);
373 if ((father->
y_ > 1) && (father->
y_ < (
signed)height_ - 2)) {
374 child = astar_states_[++astar_state_count_];
375 child->
x_ = father->
x_;
376 child->
y_ = father->
y_ + step_y;
378 key = calculate_key(child->
x_, child->
y_);
381 else if (closed_list_.find(key) == closed_list_.end())
382 open_list_.push(child);
387 logger_->
log_debug(
"AStar",
"Failed to get a modified targetpoint");
388 return point_t(target_x, target_y);