plan.h

00001 
00002 /**************************************************************************
00003  * Desc: Path planning
00004  * Author: Andrew Howard
00005  * Date: 10 Oct 2002
00006  * CVS: $Id: plan.h,v 1.11 2005/10/07 00:54:33 gerkey Exp $
00007 **************************************************************************/
00008 
00009 #ifndef PLAN_H
00010 #define PLAN_H
00011 
00012 #ifdef __cplusplus
00013 extern "C" {
00014 #endif
00015 
00016 // Description for a grid single cell
00017 typedef struct _plan_cell_t
00018 {
00019   // Cell index in grid map
00020   unsigned short ci, cj;
00021   
00022   // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
00023   char occ_state;
00024 
00025   // Distance to the nearest occupied cell
00026   float occ_dist;
00027 
00028   // Distance (cost) to the goal
00029   float plan_cost;
00030 
00031   // The next cell in the plan
00032   struct _plan_cell_t *plan_next;
00033   
00034 } plan_cell_t;
00035 
00036 
00037 // Planner info
00038 typedef struct
00039 {
00040   // Grid dimensions (number of cells)
00041   int size_x, size_y;
00042 
00043   // Grid bounds (for limiting the search).
00044   int min_x, min_y, max_x, max_y;
00045 
00046   // Grid origin (real-world coords, in meters, of the lower-left grid
00047   // cell)
00048   double origin_x, origin_y;
00049 
00050   // Grid scale (m/cell)
00051   double scale;
00052 
00053   // Effective robot radius
00054   double des_min_radius, abs_min_radius;
00055 
00056   // Max radius we will consider
00057   double max_radius;
00058 
00059   // Penalty factor for cells inside the max radius
00060   double dist_penalty;
00061 
00062   // The grid data
00063   plan_cell_t *cells;
00064   unsigned char* marks;
00065   size_t marks_size;
00066   
00067   // Queue of cells to update
00068   int queue_start, queue_len, queue_size;
00069   plan_cell_t **queue;
00070 
00071   // Waypoints
00072   int waypoint_count, waypoint_size;
00073   plan_cell_t **waypoints;
00074 } plan_t;
00075 
00076 
00077 // Create a planner
00078 plan_t *plan_alloc(double abs_min_radius, double des_min_radius,
00079                    double max_radius, double dist_penalty);
00080 
00081 // Destroy a planner
00082 void plan_free(plan_t *plan);
00083 
00084 // Reset the plan
00085 void plan_reset(plan_t *plan);
00086 
00087 #if 0
00088 // Load the occupancy values from an image file
00089 int plan_load_occ(plan_t *plan, const char *filename, double scale);
00090 #endif
00091 
00092 void plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y);
00093 
00094 void plan_set_bbox(plan_t* plan, double padding, double min_size,
00095                    double x0, double y0, double x1, double y1);
00096 
00097 int plan_check_inbounds(plan_t* plan, double x, double y);
00098 
00099 // Construct the configuration space from the occupancy grid.
00100 void plan_update_cspace(plan_t *plan, const char* cachefile);
00101 
00102 // Generate the plan
00103 void plan_update_plan(plan_t *plan, double gx, double gy);
00104 
00105 // Generate a path to the goal
00106 void plan_update_waypoints(plan_t *plan, double px, double py);
00107 
00108 // Get the ith waypoint; returns zero if there are no more waypoints
00109 int plan_get_waypoint(plan_t *plan, int i, double *px, double *py);
00110 
00111 // Convert given waypoint cell to global x,y
00112 void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint, 
00113                            double *px, double *py);
00114 
00115 #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
00116 // Write the cspace occupancy distance values to a file, one per line.
00117 // Read them back in with plan_read_cspace().
00118 // Returns non-zero on error.
00119 int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash);
00120 
00121 // Read the cspace occupancy distance values from a file, one per line.
00122 // Write them in first with plan_read_cspace().
00123 // Returns non-zero on error.
00124 int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash);
00125 
00126 // Compute and return the 16-bit MD5 hash of the map data in the given plan
00127 // object.
00128 void plan_md5(unsigned int* digest, plan_t* plan);
00129 #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
00130 
00131 /**************************************************************************
00132  * Plan manipulation macros
00133  **************************************************************************/
00134 
00135 // Convert from plan index to world coords
00136 //#define PLAN_WXGX(plan, i) (((i) - plan->size_x / 2) * plan->scale)
00137 //#define PLAN_WYGY(plan, j) (((j) - plan->size_y / 2) * plan->scale)
00138 #define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale)
00139 #define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale)
00140 
00141 // Convert from world coords to plan coords
00142 //#define PLAN_GXWX(plan, x) (floor((x) / plan->scale + 0.5) + plan->size_x / 2)
00143 //#define PLAN_GYWY(plan, y) (floor((y) / plan->scale + 0.5) + plan->size_y / 2)
00144 #define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5))
00145 #define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5))
00146 
00147 // Test to see if the given plan coords lie within the absolute plan bounds.
00148 #define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
00149 // Test to see if the given plan coords lie within the user-specified plan bounds
00150 #define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y))
00151 
00152 // Compute the cell index for the given plan coords.
00153 #define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)
00154 
00155 #ifdef __cplusplus
00156 }
00157 #endif
00158 
00159 #endif

Last updated 12 September 2005 21:38:45