plan.h
00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2003 00004 * Andrew Howard 00005 * Brian Gerkey 00006 * 00007 * This program is free software; you can redistribute it and/or modify 00008 * it under the terms of the GNU General Public License as published by 00009 * the Free Software Foundation; either version 2 of the License, or 00010 * (at your option) any later version. 00011 * 00012 * This program is distributed in the hope that it will be useful, 00013 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00014 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00015 * GNU General Public License for more details. 00016 * 00017 * You should have received a copy of the GNU General Public License 00018 * along with this program; if not, write to the Free Software 00019 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00020 * 00021 */ 00022 00023 00024 /************************************************************************** 00025 * Desc: Path planning 00026 * Author: Andrew Howard 00027 * Date: 10 Oct 2002 00028 * CVS: $Id: plan.h 8108 2009-07-23 23:03:37Z thjc $ 00029 **************************************************************************/ 00030 00031 #ifndef PLAN_H 00032 #define PLAN_H 00033 00034 #include "heap.h" 00035 00036 #ifdef __cplusplus 00037 extern "C" { 00038 #endif 00039 00040 #define PLAN_DEFAULT_HEAP_SIZE 1000 00041 #define PLAN_MAX_COST 1e9 00042 00043 // Description for a grid single cell 00044 typedef struct _plan_cell_t 00045 { 00046 // Cell index in grid map 00047 unsigned short ci, cj; 00048 00049 // Occupancy state (-1 = free, 0 = unknown, +1 = occ) 00050 char occ_state; 00051 char occ_state_dyn; 00052 00053 // Distance to the nearest occupied cell 00054 float occ_dist; 00055 float occ_dist_dyn; 00056 00057 // Distance (cost) to the goal 00058 float plan_cost; 00059 00060 // Mark used in dynamic programming 00061 char mark; 00062 // Mark used in path hysterisis 00063 char lpathmark; 00064 00065 // The next cell in the plan 00066 struct _plan_cell_t *plan_next; 00067 00068 } plan_cell_t; 00069 00070 00071 // Planner info 00072 typedef struct 00073 { 00074 // Grid dimensions (number of cells) 00075 int size_x, size_y; 00076 00077 // Grid bounds (for limiting the search). 00078 int min_x, min_y, max_x, max_y; 00079 00080 // Grid origin (real-world coords, in meters, of the lower-left grid 00081 // cell) 00082 double origin_x, origin_y; 00083 00084 // Grid scale (m/cell) 00085 double scale; 00086 00087 // Effective robot radius 00088 double des_min_radius, abs_min_radius; 00089 00090 // Max radius we will consider 00091 double max_radius; 00092 00093 // Penalty factor for cells inside the max radius 00094 double dist_penalty; 00095 00096 // Cost multiplier for cells on the previous local path 00097 double hysteresis_factor; 00098 00099 // The grid data 00100 plan_cell_t *cells; 00101 00102 // Distance penalty kernel, pre-computed in plan_compute_dist_kernel(); 00103 float* dist_kernel; 00104 int dist_kernel_width; 00105 float dist_kernel_3x3[9]; 00106 00107 // Priority queue of cells to update 00108 heap_t* heap; 00109 00110 // The global path 00111 int path_count, path_size; 00112 plan_cell_t **path; 00113 00114 // The local path (mainly for debugging) 00115 int lpath_count, lpath_size; 00116 plan_cell_t **lpath; 00117 00118 // Waypoints extracted from global path 00119 int waypoint_count, waypoint_size; 00120 plan_cell_t **waypoints; 00121 } plan_t; 00122 00123 00124 // Create a planner 00125 plan_t *plan_alloc(double abs_min_radius, 00126 double des_min_radius, 00127 double max_radius, 00128 double dist_penalty, 00129 double hysteresis_factor); 00130 00131 void plan_compute_dist_kernel(plan_t* plan); 00132 00133 // Destroy a planner 00134 void plan_free(plan_t *plan); 00135 00136 // Initialize the plan 00137 void plan_init(plan_t *plan); 00138 00139 // Reset the plan 00140 void plan_reset(plan_t *plan); 00141 00142 #if 0 00143 // Load the occupancy values from an image file 00144 int plan_load_occ(plan_t *plan, const char *filename, double scale); 00145 #endif 00146 00147 void plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y); 00148 00149 void plan_set_bbox(plan_t* plan, double padding, double min_size, 00150 double x0, double y0, double x1, double y1); 00151 00152 int plan_check_inbounds(plan_t* plan, double x, double y); 00153 00154 // Construct the configuration space from the occupancy grid. 00155 //void plan_update_cspace(plan_t *plan, const char* cachefile); 00156 void plan_compute_cspace(plan_t *plan); 00157 00158 int plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy); 00159 00160 int plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth); 00161 00162 // Generate a path to the goal 00163 void plan_update_waypoints(plan_t *plan, double px, double py); 00164 00165 // Get the ith waypoint; returns zero if there are no more waypoints 00166 int plan_get_waypoint(plan_t *plan, int i, double *px, double *py); 00167 00168 // Convert given waypoint cell to global x,y 00169 void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint, 00170 double *px, double *py); 00171 00172 double plan_get_carrot(plan_t* plan, double* px, double* py, 00173 double lx, double ly, 00174 double maxdist, double distweight); 00175 int plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va, 00176 int* rotate_dir, 00177 double lx, double ly, double la, 00178 double gx, double gy, double ga, 00179 double goal_d, double goal_a, 00180 double maxd, double dweight, 00181 double tvmin, double tvmax, 00182 double avmin, double avmax, 00183 double amin, double amax); 00184 int plan_check_done(plan_t* plan, 00185 double lx, double ly, double la, 00186 double gx, double gy, double ga, 00187 double goal_d, double goal_a); 00188 00189 void plan_set_obstacles(plan_t* plan, double* obs, size_t num); 00190 00191 #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO 00192 // Write the cspace occupancy distance values to a file, one per line. 00193 // Read them back in with plan_read_cspace(). 00194 // Returns non-zero on error. 00195 int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash); 00196 00197 // Read the cspace occupancy distance values from a file, one per line. 00198 // Write them in first with plan_read_cspace(). 00199 // Returns non-zero on error. 00200 int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash); 00201 00202 // Compute and return the 16-bit MD5 hash of the map data in the given plan 00203 // object. 00204 void plan_md5(unsigned int* digest, plan_t* plan); 00205 #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO 00206 00207 /************************************************************************** 00208 * Plan manipulation macros 00209 **************************************************************************/ 00210 00211 // Convert from plan index to world coords 00212 //#define PLAN_WXGX(plan, i) (((i) - plan->size_x / 2) * plan->scale) 00213 //#define PLAN_WYGY(plan, j) (((j) - plan->size_y / 2) * plan->scale) 00214 #define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale) 00215 #define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale) 00216 00217 // Convert from world coords to plan coords 00218 //#define PLAN_GXWX(plan, x) (floor((x) / plan->scale + 0.5) + plan->size_x / 2) 00219 //#define PLAN_GYWY(plan, y) (floor((y) / plan->scale + 0.5) + plan->size_y / 2) 00220 #define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5)) 00221 #define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5)) 00222 00223 // Test to see if the given plan coords lie within the absolute plan bounds. 00224 #define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y)) 00225 // Test to see if the given plan coords lie within the user-specified plan bounds 00226 #define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y)) 00227 00228 // Compute the cell index for the given plan coords. 00229 #define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x) 00230 00231 #ifdef __cplusplus 00232 } 00233 #endif 00234 00235 #endif