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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 *
21 */
22
23
24/**************************************************************************
25 * Desc: Path planning
26 * Author: Andrew Howard
27 * Date: 10 Oct 2002
28 * CVS: $Id$
29**************************************************************************/
30
31#ifndef PLAN_H
32#define PLAN_H
33
34#include "heap.h"
35
36#ifdef __cplusplus
37extern "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
44typedef 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
69
70
71// Planner info
72typedef 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
125plan_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
131void plan_compute_dist_kernel(plan_t* plan);
132
133// Destroy a planner
134void plan_free(plan_t *plan);
135
136// Copy a planner
137plan_t *plan_copy(plan_t *plan);
138
139// Initialize the plan
140void plan_init(plan_t *plan);
141
142// Reset the plan
143void plan_reset(plan_t *plan);
144
145#if 0
146// Load the occupancy values from an image file
147int plan_load_occ(plan_t *plan, const char *filename, double scale);
148#endif
149
150void plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y);
151
152void plan_set_bbox(plan_t* plan, double padding, double min_size,
153 double x0, double y0, double x1, double y1);
154
155int plan_check_inbounds(plan_t* plan, double x, double y);
156
157// Construct the configuration space from the occupancy grid.
158//void plan_update_cspace(plan_t *plan, const char* cachefile);
159void plan_compute_cspace(plan_t *plan);
160
161int plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy);
162
163int plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth);
164
165// Generate a path to the goal
166void plan_update_waypoints(plan_t *plan, double px, double py);
167
168// Get the ith waypoint; returns zero if there are no more waypoints
169int plan_get_waypoint(plan_t *plan, int i, double *px, double *py);
170
171// Convert given waypoint cell to global x,y
172void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint,
173 double *px, double *py);
174
175double plan_get_carrot(plan_t* plan, double* px, double* py,
176 double lx, double ly,
177 double maxdist, double distweight);
178int plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va,
179 int* rotate_dir,
180 double lx, double ly, double la,
181 double gx, double gy, double ga,
182 double goal_d, double goal_a,
183 double maxd, double dweight,
184 double tvmin, double tvmax,
185 double avmin, double avmax,
186 double amin, double amax);
187int plan_check_done(plan_t* plan,
188 double lx, double ly, double la,
189 double gx, double gy, double ga,
190 double goal_d, double goal_a);
191
192void plan_set_obstacles(plan_t* plan, double* obs, size_t num);
193
194#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
195// Write the cspace occupancy distance values to a file, one per line.
196// Read them back in with plan_read_cspace().
197// Returns non-zero on error.
198int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash);
199
200// Read the cspace occupancy distance values from a file, one per line.
201// Write them in first with plan_read_cspace().
202// Returns non-zero on error.
203int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash);
204
205// Compute and return the 16-bit MD5 hash of the map data in the given plan
206// object.
207void plan_md5(unsigned int* digest, plan_t* plan);
208#endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
209
210/**************************************************************************
211 * Plan manipulation macros
212 **************************************************************************/
213
214// Convert from plan index to world coords
215//#define PLAN_WXGX(plan, i) (((i) - plan->size_x / 2) * plan->scale)
216//#define PLAN_WYGY(plan, j) (((j) - plan->size_y / 2) * plan->scale)
217#define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale)
218#define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale)
219
220// Convert from world coords to plan coords
221//#define PLAN_GXWX(plan, x) (floor((x) / plan->scale + 0.5) + plan->size_x / 2)
222//#define PLAN_GYWY(plan, y) (floor((y) / plan->scale + 0.5) + plan->size_y / 2)
223#define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5))
224#define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5))
225
226// Test to see if the given plan coords lie within the absolute plan bounds.
227#define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
228// Test to see if the given plan coords lie within the user-specified plan bounds
229#define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y))
230
231// Compute the cell index for the given plan coords.
232#define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)
233
234#ifdef __cplusplus
235}
236#endif
237
238#endif
Definition plan.h:45
Definition heap.h:43
Definition plan.h:73