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
#define PLAYER_DIO_CMD_VALUES
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1987
char * string
The string to say.
Definition: player_interfaces.h:1802
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:89
char * guid
The Globally Unique IDentifier (GUID) of the tag.
Definition: player_interfaces.h:4323
player_rfid_tag_t * tags
The list of RFID tags.
Definition: player_interfaces.h:4334
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Generic message header.
Definition: player.h:160
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:657
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:663
float * voltages
the samples [V]
Definition: player_interfaces.h:2058
virtual void Main(void)=0
Main method for driver thread.
int ReadInt(int section, const char *name, int value)
Read an integer value.
Definition: plan.h:72
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
uint32_t digout
output bitfield
Definition: player_interfaces.h:2011
#define PLAYER_WARN2(msg, a, b)
Error message macros.
Definition: error.h:90
#define PLAYER_RFID_DATA_TAGS
Data subtype.
Definition: player_interfaces.h:4298
#define PLAYER_SPEECH_CMD_SAY
Command subtype: say a string.
Definition: player_interfaces.h:1789
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Command: output values (PLAYER_DIO_CMD_VALUES)
Definition: player_interfaces.h:2006
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1994
Definition: plan.h:44
uint32_t voltages_count
number of valid samples
Definition: player_interfaces.h:2056
Class for loading configuration file information.
Definition: configfile.h:195
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2053
A device address.
Definition: player.h:144
An autopointer for the message queue.
Definition: message.h:72
uint32_t bits
bitfield of samples
Definition: player_interfaces.h:1999
uint32_t tags_count
The number of RFID tags found.
Definition: player_interfaces.h:4332
#define PLAYER_WSN_DATA_STATE
Data subtypes
Definition: player_interfaces.h:4369
uint32_t count
the command
Definition: player_interfaces.h:2009
uint32_t type
Tag type.
Definition: player_interfaces.h:4319
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:80
Base class for drivers which oeprate with a thread.
Definition: driver.h:551
Command: say a string (PLAYER_SPEECH_CMD_SAY)
Definition: player_interfaces.h:1797
Data (PLAYER_WSN_DATA_STATE)
Definition: player_interfaces.h:4413
#define PLAYER_AIO_DATA_STATE
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2046
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:88
Structure describing a single RFID tag.
Definition: player_interfaces.h:4316
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
uint32_t count
number of samples
Definition: player_interfaces.h:1997
#define PLAYER_DIO_DATA_VALUES
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1984
Base class for all drivers.
Definition: driver.h:107
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:104
uint32_t string_count
Length of string.
Definition: player_interfaces.h:1800
uint32_t guid_count
GUID count.
Definition: player_interfaces.h:4321
Data.
Definition: player_interfaces.h:4329
Definition: heap.h:42