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);
137 void plan_init(
plan_t *plan);
140 void plan_reset(
plan_t *plan);
144 int plan_load_occ(
plan_t *plan,
const char *filename,
double scale);
147 void plan_set_bounds(
plan_t* plan,
int min_x,
int min_y,
int max_x,
int max_y);
149 void plan_set_bbox(
plan_t* plan,
double padding,
double min_size,
150 double x0,
double y0,
double x1,
double y1);
152 int plan_check_inbounds(
plan_t* plan,
double x,
double y);
156 void plan_compute_cspace(
plan_t *plan);
158 int plan_do_global(
plan_t *plan,
double lx,
double ly,
double gx,
double gy);
160 int plan_do_local(
plan_t *plan,
double lx,
double ly,
double plan_halfwidth);
163 void plan_update_waypoints(
plan_t *plan,
double px,
double py);
166 int plan_get_waypoint(
plan_t *plan,
int i,
double *px,
double *py);
170 double *px,
double *py);
172 double plan_get_carrot(
plan_t* plan,
double* px,
double* py,
173 double lx,
double ly,
174 double maxdist,
double distweight);
175 int plan_compute_diffdrive_cmds(
plan_t* plan,
double* vx,
double *va,
177 double lx,
double ly,
double la,
178 double gx,
double gy,
double ga,
179 double goal_d,
double goal_a,
180 double maxd,
double dweight,
181 double tvmin,
double tvmax,
182 double avmin,
double avmax,
183 double amin,
double amax);
184 int plan_check_done(
plan_t* plan,
185 double lx,
double ly,
double la,
186 double gx,
double gy,
double ga,
187 double goal_d,
double goal_a);
189 void plan_set_obstacles(
plan_t* plan,
double* obs,
size_t num);
191 #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO 195 int plan_write_cspace(
plan_t *plan,
const char* fname,
unsigned int* hash);
200 int plan_read_cspace(
plan_t *plan,
const char* fname,
unsigned int* hash);
204 void plan_md5(
unsigned int* digest,
plan_t* plan);
205 #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO 214 #define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale) 215 #define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale) 220 #define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5)) 221 #define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5)) 224 #define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y)) 226 #define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y)) 229 #define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)