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);
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.
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.
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.
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 ...
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.
void solve(const point_t &robo_pos, const point_t &target_pos, std::vector< point_t > &solution)
solves the given assignment.
Interface for configuration handling.
unsigned int near
The cost for a cell near an obstacle (distance="near")