Fawkes API Fawkes Development Version
astar.h
1
2/***************************************************************************
3 * astar.h - A* search implementation
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2002 Stefan Jacobs
7 * 2013 Bahram Maleki-Fard
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#ifndef _PLUGINS_COLLI_SEARCH_ASTAR_H_
24#define _PLUGINS_COLLI_SEARCH_ASTAR_H_
25
26#include "../common/types.h"
27#include "astar_state.h"
28
29#include <map>
30#include <queue>
31#include <vector>
32
33namespace fawkes {
34
35class LaserOccupancyGrid;
36class Logger;
37class Configuration;
38
39typedef struct point_struct point_t;
40
41/** Class AStar.
42 * This is an implementation of the A* search algorithm in a
43 * highly efficient way (I hope ;-).
44 */
46{
47public:
48 AStarColli(LaserOccupancyGrid *occGrid, Logger *logger, Configuration *config);
50
51 /* =========================================== */
52 /* ************* PUBLIC METHODS ************** */
53 /* =========================================== */
54 /** solves the given assignment.
55 * This starts the search for a path through the occupance grid to the
56 * target point.
57 * Performing astar search over the occupancy grid and returning the solution.
58 */
59 void solve(const point_t &robo_pos, const point_t &target_pos, std::vector<point_t> &solution);
60
61 ///\brief Method, returning the nearest point outside of an obstacle.
62 point_t remove_target_from_obstacle(int target_x, int target_y, int step_x, int step_y);
63
64private:
65 /* =========================================== */
66 /* ************ PRIVATE VARIABLES ************ */
67 /* =========================================== */
68 fawkes::Logger *logger_;
69
70 // this is the local reference to the occupancy grid.
71 LaserOccupancyGrid *occ_grid_;
72 unsigned int width_;
73 unsigned int height_;
74
75 // Costs for the cells in grid
76 colli_cell_cost_t cell_costs_;
77
78 // this is the local robot position and target point.
79 AStarState robo_pos_;
80 AStarState target_state_;
81
82 // This is a state vector...
83 // It is for speed purposes. So I do not have to do a new each time
84 // I have to malloc a new one each time.
85 std::vector<AStarState *> astar_states_;
86
87 // maximum number of states available for a* and current index
88 int max_states_;
89 int astar_state_count_;
90
91 // this is AStars openlist
92 struct cmp
93 {
94 bool
95 operator()(AStarState *a1, AStarState *a2) const
96 {
97 return (a1->total_cost_ > a2->total_cost_);
98 }
99 };
100
101 std::priority_queue<AStarState *, std::vector<AStarState *>, cmp> open_list_;
102
103 // this is AStars closedList
104 std::map<int, int> closed_list_;
105
106 /* =========================================== */
107 /* ************ PRIVATE METHODS ************** */
108 /* =========================================== */
109
110 // search with AStar through the OccGrid
112
113 // Calculate a unique key for a given coordinate
114 int calculate_key(int x, int y);
115
116 // Check if the state is a goal
117 bool is_goal(AStarState *state);
118
119 // Calculate heuristic for a given state
120 int heuristic(AStarState *state);
121
122 // Generate all children for a given State
123 void generate_children(AStarState *father);
124
125 // Generates a solution sequence for a given state
126 void get_solution_sequence(AStarState *node, std::vector<point_t> &solution);
127};
128
129} // namespace fawkes
130
131#endif
Class AStar.
Definition: astar.h:46
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.
Definition: astar.cpp:334
~AStarColli()
Destructor.
Definition: astar.cpp:84
AStarColli(LaserOccupancyGrid *occGrid, Logger *logger, Configuration *config)
Constructor.
Definition: astar.cpp:52
void solve(const point_t &robo_pos, const point_t &target_pos, std::vector< point_t > &solution)
solves the given assignment.
Definition: astar.cpp:102
This is the abstract(!) class for an A* State.
Definition: astar_state.h:39
int total_cost_
The total cost.
Definition: astar_state.h:44
Interface for configuration handling.
Definition: config.h:68
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:47
Interface for logging.
Definition: logger.h:42
This class tries to translate the found plan to interpreteable data for the rest of the program.
Fawkes library namespace.
Costs of occupancy-grid cells.
Definition: types.h:50
Point with cartesian coordinates as signed integers.
Definition: types.h:42