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

Last updated 12 September 2005 21:38:45