00001
00002
00003
00004
00005
00006
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
00022 typedef struct _plan_cell_t
00023 {
00024
00025 unsigned short ci, cj;
00026
00027
00028 char occ_state;
00029 char occ_state_dyn;
00030
00031
00032 float occ_dist;
00033 float occ_dist_dyn;
00034
00035
00036 float plan_cost;
00037
00038
00039 char mark;
00040
00041
00042 struct _plan_cell_t *plan_next;
00043
00044 } plan_cell_t;
00045
00046
00047
00048 typedef struct
00049 {
00050
00051 int size_x, size_y;
00052
00053
00054 int min_x, min_y, max_x, max_y;
00055
00056
00057
00058 double origin_x, origin_y;
00059
00060
00061 double scale;
00062
00063
00064 double des_min_radius, abs_min_radius;
00065
00066
00067 double max_radius;
00068
00069
00070 double dist_penalty;
00071
00072
00073 plan_cell_t *cells;
00074
00075
00076 unsigned short* obs_pts;
00077 size_t obs_pts_size;
00078 size_t obs_pts_num;
00079
00080
00081 float* dist_kernel;
00082 int dist_kernel_width;
00083 float dist_kernel_3x3[9];
00084
00085
00086 heap_t* heap;
00087
00088
00089 int path_count, path_size;
00090 plan_cell_t **path;
00091
00092
00093 int lpath_count, lpath_size;
00094 plan_cell_t **lpath;
00095
00096
00097 int waypoint_count, waypoint_size;
00098 plan_cell_t **waypoints;
00099 } plan_t;
00100
00101
00102
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
00109 void plan_free(plan_t *plan);
00110
00111
00112 void plan_init(plan_t *plan);
00113
00114
00115 void plan_reset(plan_t *plan);
00116
00117 #if 0
00118
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
00130
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
00138 void plan_update_waypoints(plan_t *plan, double px, double py);
00139
00140
00141 int plan_get_waypoint(plan_t *plan, int i, double *px, double *py);
00142
00143
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
00153
00154
00155 int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash);
00156
00157
00158
00159
00160 int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash);
00161
00162
00163
00164 void plan_md5(unsigned int* digest, plan_t* plan);
00165 #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
00166
00167
00168
00169
00170
00171
00172
00173
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
00178
00179
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
00184 #define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
00185
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
00189 #define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)
00190
00191 #ifdef __cplusplus
00192 }
00193 #endif
00194
00195 #endif