Player
Frontpage
Contents
User
Installation
Quick start
Supported devices
Tutorials
Utilities
Client libraries
FAQ
Help
Developer
Architecture
libplayercore
libplayerinterface
interfaces
libplayerdrivers
drivers
libplayercommon
libplayersd
libplayertcp
libplayerxdr
TODO
Online
Homepage
Download
Project
Bugs
Help
server
drivers
planner
wavefront
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
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
// Copy a planner
137
plan_t
*plan_copy(
plan_t
*plan);
138
139
// Initialize the plan
140
void
plan_init(
plan_t
*plan);
141
142
// Reset the plan
143
void
plan_reset(
plan_t
*plan);
144
145
#if 0
146
// Load the occupancy values from an image file
147
int
plan_load_occ(
plan_t
*plan,
const
char
*filename,
double
scale);
148
#endif
149
150
void
plan_set_bounds(
plan_t
* plan,
int
min_x,
int
min_y,
int
max_x,
int
max_y);
151
152
void
plan_set_bbox(
plan_t
* plan,
double
padding,
double
min_size,
153
double
x0,
double
y0,
double
x1,
double
y1);
154
155
int
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);
159
void
plan_compute_cspace(
plan_t
*plan);
160
161
int
plan_do_global(
plan_t
*plan,
double
lx,
double
ly,
double
gx,
double
gy);
162
163
int
plan_do_local(
plan_t
*plan,
double
lx,
double
ly,
double
plan_halfwidth);
164
165
// Generate a path to the goal
166
void
plan_update_waypoints(
plan_t
*plan,
double
px,
double
py);
167
168
// Get the ith waypoint; returns zero if there are no more waypoints
169
int
plan_get_waypoint(
plan_t
*plan,
int
i,
double
*px,
double
*py);
170
171
// Convert given waypoint cell to global x,y
172
void
plan_convert_waypoint(
plan_t
* plan,
plan_cell_t
*waypoint,
173
double
*px,
double
*py);
174
175
double
plan_get_carrot(
plan_t
* plan,
double
* px,
double
* py,
176
double
lx,
double
ly,
177
double
maxdist,
double
distweight);
178
int
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);
187
int
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
192
void
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.
198
int
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.
203
int
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.
207
void
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
_plan_cell_t
Definition
plan.h:45
heap
Definition
heap.h:43
plan_t
Definition
plan.h:73
Generated on Thu Jul 25 2024 00:00:00 for Player by
1.12.0