40 #define PLAN_DEFAULT_HEAP_SIZE 1000
41 #define PLAN_MAX_COST 1e9
47 unsigned short ci, cj;
78 int min_x, min_y, max_x, max_y;
82 double origin_x, origin_y;
88 double des_min_radius, abs_min_radius;
97 double hysteresis_factor;
104 int dist_kernel_width;
105 float dist_kernel_3x3[9];
111 int path_count, path_size;
115 int lpath_count, lpath_size;
119 int waypoint_count, waypoint_size;
125 plan_t *plan_alloc(
double abs_min_radius,
126 double des_min_radius,
129 double hysteresis_factor);
131 void plan_compute_dist_kernel(
plan_t* plan);
134 void plan_free(
plan_t *plan);
140 void plan_init(
plan_t *plan);
143 void plan_reset(
plan_t *plan);
147 int plan_load_occ(
plan_t *plan,
const char *filename,
double scale);
150 void plan_set_bounds(
plan_t* plan,
int min_x,
int min_y,
int max_x,
int max_y);
152 void plan_set_bbox(
plan_t* plan,
double padding,
double min_size,
153 double x0,
double y0,
double x1,
double y1);
155 int plan_check_inbounds(
plan_t* plan,
double x,
double y);
159 void plan_compute_cspace(
plan_t *plan);
161 int plan_do_global(
plan_t *plan,
double lx,
double ly,
double gx,
double gy);
163 int plan_do_local(
plan_t *plan,
double lx,
double ly,
double plan_halfwidth);
166 void plan_update_waypoints(
plan_t *plan,
double px,
double py);
169 int plan_get_waypoint(
plan_t *plan,
int i,
double *px,
double *py);
173 double *px,
double *py);
175 double plan_get_carrot(
plan_t* plan,
double* px,
double* py,
176 double lx,
double ly,
177 double maxdist,
double distweight);
178 int plan_compute_diffdrive_cmds(
plan_t* plan,
double* vx,
double *va,
180 double lx,
double ly,
double la,
181 double gx,
double gy,
double ga,
182 double goal_d,
double goal_a,
183 double maxd,
double dweight,
184 double tvmin,
double tvmax,
185 double avmin,
double avmax,
186 double amin,
double amax);
187 int plan_check_done(
plan_t* plan,
188 double lx,
double ly,
double la,
189 double gx,
double gy,
double ga,
190 double goal_d,
double goal_a);
192 void plan_set_obstacles(
plan_t* plan,
double* obs,
size_t num);
194 #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
198 int plan_write_cspace(
plan_t *plan,
const char* fname,
unsigned int* hash);
203 int plan_read_cspace(
plan_t *plan,
const char* fname,
unsigned int* hash);
207 void plan_md5(
unsigned int* digest,
plan_t* plan);
217 #define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale)
218 #define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale)
223 #define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5))
224 #define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5))
227 #define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
229 #define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y))
232 #define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)