plan.h
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2003
4  * Andrew Howard
5  * Brian Gerkey
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2 of the License, or
10  * (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software
19  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
20  *
21  */
22 
23 
24 /**************************************************************************
25  * Desc: Path planning
26  * Author: Andrew Howard
27  * Date: 10 Oct 2002
28  * CVS: $Id: plan.h 8108 2009-07-23 23:03:37Z thjc $
29 **************************************************************************/
30 
31 #ifndef PLAN_H
32 #define PLAN_H
33 
34 #include "heap.h"
35 
36 #ifdef __cplusplus
37 extern "C" {
38 #endif
39 
40 #define PLAN_DEFAULT_HEAP_SIZE 1000
41 #define PLAN_MAX_COST 1e9
42 
43 // Description for a grid single cell
44 typedef struct _plan_cell_t
45 {
46  // Cell index in grid map
47  unsigned short ci, cj;
48 
49  // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
50  char occ_state;
51  char occ_state_dyn;
52 
53  // Distance to the nearest occupied cell
54  float occ_dist;
55  float occ_dist_dyn;
56 
57  // Distance (cost) to the goal
58  float plan_cost;
59 
60  // Mark used in dynamic programming
61  char mark;
62  // Mark used in path hysterisis
63  char lpathmark;
64 
65  // The next cell in the plan
66  struct _plan_cell_t *plan_next;
67 
68 } plan_cell_t;
69 
70 
71 // Planner info
72 typedef struct
73 {
74  // Grid dimensions (number of cells)
75  int size_x, size_y;
76 
77  // Grid bounds (for limiting the search).
78  int min_x, min_y, max_x, max_y;
79 
80  // Grid origin (real-world coords, in meters, of the lower-left grid
81  // cell)
82  double origin_x, origin_y;
83 
84  // Grid scale (m/cell)
85  double scale;
86 
87  // Effective robot radius
88  double des_min_radius, abs_min_radius;
89 
90  // Max radius we will consider
91  double max_radius;
92 
93  // Penalty factor for cells inside the max radius
94  double dist_penalty;
95 
96  // Cost multiplier for cells on the previous local path
97  double hysteresis_factor;
98 
99  // The grid data
100  plan_cell_t *cells;
101 
102  // Distance penalty kernel, pre-computed in plan_compute_dist_kernel();
103  float* dist_kernel;
104  int dist_kernel_width;
105  float dist_kernel_3x3[9];
106 
107  // Priority queue of cells to update
108  heap_t* heap;
109 
110  // The global path
111  int path_count, path_size;
112  plan_cell_t **path;
113 
114  // The local path (mainly for debugging)
115  int lpath_count, lpath_size;
116  plan_cell_t **lpath;
117 
118  // Waypoints extracted from global path
119  int waypoint_count, waypoint_size;
120  plan_cell_t **waypoints;
121 } plan_t;
122 
123 
124 // Create a planner
125 plan_t *plan_alloc(double abs_min_radius,
126  double des_min_radius,
127  double max_radius,
128  double dist_penalty,
129  double hysteresis_factor);
130 
131 void plan_compute_dist_kernel(plan_t* plan);
132 
133 // Destroy a planner
134 void plan_free(plan_t *plan);
135 
136 // Initialize the plan
137 void plan_init(plan_t *plan);
138 
139 // Reset the plan
140 void plan_reset(plan_t *plan);
141 
142 #if 0
143 // Load the occupancy values from an image file
144 int plan_load_occ(plan_t *plan, const char *filename, double scale);
145 #endif
146 
147 void plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y);
148 
149 void plan_set_bbox(plan_t* plan, double padding, double min_size,
150  double x0, double y0, double x1, double y1);
151 
152 int plan_check_inbounds(plan_t* plan, double x, double y);
153 
154 // Construct the configuration space from the occupancy grid.
155 //void plan_update_cspace(plan_t *plan, const char* cachefile);
156 void plan_compute_cspace(plan_t *plan);
157 
158 int plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy);
159 
160 int plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth);
161 
162 // Generate a path to the goal
163 void plan_update_waypoints(plan_t *plan, double px, double py);
164 
165 // Get the ith waypoint; returns zero if there are no more waypoints
166 int plan_get_waypoint(plan_t *plan, int i, double *px, double *py);
167 
168 // Convert given waypoint cell to global x,y
169 void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint,
170  double *px, double *py);
171 
172 double plan_get_carrot(plan_t* plan, double* px, double* py,
173  double lx, double ly,
174  double maxdist, double distweight);
175 int plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va,
176  int* rotate_dir,
177  double lx, double ly, double la,
178  double gx, double gy, double ga,
179  double goal_d, double goal_a,
180  double maxd, double dweight,
181  double tvmin, double tvmax,
182  double avmin, double avmax,
183  double amin, double amax);
184 int plan_check_done(plan_t* plan,
185  double lx, double ly, double la,
186  double gx, double gy, double ga,
187  double goal_d, double goal_a);
188 
189 void plan_set_obstacles(plan_t* plan, double* obs, size_t num);
190 
191 #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
192 // Write the cspace occupancy distance values to a file, one per line.
193 // Read them back in with plan_read_cspace().
194 // Returns non-zero on error.
195 int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash);
196 
197 // Read the cspace occupancy distance values from a file, one per line.
198 // Write them in first with plan_read_cspace().
199 // Returns non-zero on error.
200 int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash);
201 
202 // Compute and return the 16-bit MD5 hash of the map data in the given plan
203 // object.
204 void plan_md5(unsigned int* digest, plan_t* plan);
205 #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
206 
207 /**************************************************************************
208  * Plan manipulation macros
209  **************************************************************************/
210 
211 // Convert from plan index to world coords
212 //#define PLAN_WXGX(plan, i) (((i) - plan->size_x / 2) * plan->scale)
213 //#define PLAN_WYGY(plan, j) (((j) - plan->size_y / 2) * plan->scale)
214 #define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale)
215 #define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale)
216 
217 // Convert from world coords to plan coords
218 //#define PLAN_GXWX(plan, x) (floor((x) / plan->scale + 0.5) + plan->size_x / 2)
219 //#define PLAN_GYWY(plan, y) (floor((y) / plan->scale + 0.5) + plan->size_y / 2)
220 #define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5))
221 #define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5))
222 
223 // Test to see if the given plan coords lie within the absolute plan bounds.
224 #define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
225 // Test to see if the given plan coords lie within the user-specified plan bounds
226 #define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y))
227 
228 // Compute the cell index for the given plan coords.
229 #define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)
230 
231 #ifdef __cplusplus
232 }
233 #endif
234 
235 #endif
Definition: plan.h:72
Definition: plan.h:44
Definition: heap.h:42

Last updated 12 September 2005 21:38:45