plan.h

00001 
00002 /**************************************************************************
00003  * Desc: Path planning
00004  * Author: Andrew Howard
00005  * Date: 10 Oct 2002
00006  * CVS: $Id: plan.h 6290 2008-04-09 01:37:38Z gerkey $
00007 **************************************************************************/
00008 
00009 #ifndef PLAN_H
00010 #define PLAN_H
00011 
00012 #include "heap.h"
00013 
00014 #ifdef __cplusplus
00015 extern "C" {
00016 #endif
00017 
00018 #define PLAN_DEFAULT_HEAP_SIZE 1000
00019 #define PLAN_MAX_COST 1e9
00020 
00021 // Description for a grid single cell
00022 typedef struct _plan_cell_t
00023 {
00024   // Cell index in grid map
00025   unsigned short ci, cj;
00026   
00027   // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
00028   char occ_state;
00029   char occ_state_dyn;
00030 
00031   // Distance to the nearest occupied cell
00032   float occ_dist;
00033   float occ_dist_dyn;
00034 
00035   // Distance (cost) to the goal
00036   float plan_cost;
00037 
00038   // Mark used in dynamic programming
00039   char mark;
00040 
00041   // The next cell in the plan
00042   struct _plan_cell_t *plan_next;
00043   
00044 } plan_cell_t;
00045 
00046 
00047 // Planner info
00048 typedef struct
00049 {
00050   // Grid dimensions (number of cells)
00051   int size_x, size_y;
00052 
00053   // Grid bounds (for limiting the search).
00054   int min_x, min_y, max_x, max_y;
00055 
00056   // Grid origin (real-world coords, in meters, of the lower-left grid
00057   // cell)
00058   double origin_x, origin_y;
00059 
00060   // Grid scale (m/cell)
00061   double scale;
00062 
00063   // Effective robot radius
00064   double des_min_radius, abs_min_radius;
00065 
00066   // Max radius we will consider
00067   double max_radius;
00068 
00069   // Penalty factor for cells inside the max radius
00070   double dist_penalty;
00071 
00072   // The grid data
00073   plan_cell_t *cells;
00074 
00075   // Dynamic obstacle hitpoints
00076   unsigned short* obs_pts;
00077   size_t obs_pts_size;
00078   size_t obs_pts_num;
00079 
00080   // Distance penalty kernel, pre-computed in plan_compute_dist_kernel();
00081   float* dist_kernel;
00082   int dist_kernel_width;
00083   float dist_kernel_3x3[9];
00084   
00085   // Priority queue of cells to update
00086   heap_t* heap;
00087 
00088   // The global path
00089   int path_count, path_size;
00090   plan_cell_t **path;
00091   
00092   // The local path (mainly for debugging)
00093   int lpath_count, lpath_size;
00094   plan_cell_t **lpath;
00095 
00096   // Waypoints extracted from global path
00097   int waypoint_count, waypoint_size;
00098   plan_cell_t **waypoints;
00099 } plan_t;
00100 
00101 
00102 // Create a planner
00103 plan_t *plan_alloc(double abs_min_radius, double des_min_radius,
00104                    double max_radius, double dist_penalty);
00105 
00106 void plan_compute_dist_kernel(plan_t* plan);
00107 
00108 // Destroy a planner
00109 void plan_free(plan_t *plan);
00110 
00111 // Initialize the plan
00112 void plan_init(plan_t *plan);
00113 
00114 // Reset the plan
00115 void plan_reset(plan_t *plan);
00116 
00117 #if 0
00118 // Load the occupancy values from an image file
00119 int plan_load_occ(plan_t *plan, const char *filename, double scale);
00120 #endif
00121 
00122 void plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y);
00123 
00124 void plan_set_bbox(plan_t* plan, double padding, double min_size,
00125                    double x0, double y0, double x1, double y1);
00126 
00127 int plan_check_inbounds(plan_t* plan, double x, double y);
00128 
00129 // Construct the configuration space from the occupancy grid.
00130 //void plan_update_cspace(plan_t *plan, const char* cachefile);
00131 void plan_compute_cspace(plan_t *plan);
00132 
00133 int plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy);
00134 
00135 int plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth);
00136 
00137 // Generate a path to the goal
00138 void plan_update_waypoints(plan_t *plan, double px, double py);
00139 
00140 // Get the ith waypoint; returns zero if there are no more waypoints
00141 int plan_get_waypoint(plan_t *plan, int i, double *px, double *py);
00142 
00143 // Convert given waypoint cell to global x,y
00144 void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint, 
00145                            double *px, double *py);
00146 
00147 double plan_get_carrot(plan_t* plan, double* px, double* py, 
00148                        double lx, double ly, 
00149                        double maxdist, double distweight);
00150 
00151 #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
00152 // Write the cspace occupancy distance values to a file, one per line.
00153 // Read them back in with plan_read_cspace().
00154 // Returns non-zero on error.
00155 int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash);
00156 
00157 // Read the cspace occupancy distance values from a file, one per line.
00158 // Write them in first with plan_read_cspace().
00159 // Returns non-zero on error.
00160 int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash);
00161 
00162 // Compute and return the 16-bit MD5 hash of the map data in the given plan
00163 // object.
00164 void plan_md5(unsigned int* digest, plan_t* plan);
00165 #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
00166 
00167 /**************************************************************************
00168  * Plan manipulation macros
00169  **************************************************************************/
00170 
00171 // Convert from plan index to world coords
00172 //#define PLAN_WXGX(plan, i) (((i) - plan->size_x / 2) * plan->scale)
00173 //#define PLAN_WYGY(plan, j) (((j) - plan->size_y / 2) * plan->scale)
00174 #define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale)
00175 #define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale)
00176 
00177 // Convert from world coords to plan coords
00178 //#define PLAN_GXWX(plan, x) (floor((x) / plan->scale + 0.5) + plan->size_x / 2)
00179 //#define PLAN_GYWY(plan, y) (floor((y) / plan->scale + 0.5) + plan->size_y / 2)
00180 #define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5))
00181 #define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5))
00182 
00183 // Test to see if the given plan coords lie within the absolute plan bounds.
00184 #define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
00185 // Test to see if the given plan coords lie within the user-specified plan bounds
00186 #define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y))
00187 
00188 // Compute the cell index for the given plan coords.
00189 #define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)
00190 
00191 #ifdef __cplusplus
00192 }
00193 #endif
00194 
00195 #endif

Last updated 12 September 2005 21:38:45