plan.h
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2003
* Andrew Howard
* Brian Gerkey
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Path planning
* Author: Andrew Howard
* Date: 10 Oct 2002
* CVS: $Id: plan.h 8108 2009-07-23 23:03:37Z thjc $
**************************************************************************/
#ifndef PLAN_H
#define PLAN_H
#include "heap.h"
#ifdef __cplusplus
extern "C" {
#endif
#define PLAN_DEFAULT_HEAP_SIZE 1000
#define PLAN_MAX_COST 1e9
// Description for a grid single cell
typedef struct _plan_cell_t
{
// Cell index in grid map
unsigned short ci, cj;
// Occupancy state (-1 = free, 0 = unknown, +1 = occ)
char occ_state;
char occ_state_dyn;
// Distance to the nearest occupied cell
float occ_dist;
float occ_dist_dyn;
// Distance (cost) to the goal
float plan_cost;
// Mark used in dynamic programming
char mark;
// Mark used in path hysterisis
char lpathmark;
// The next cell in the plan
struct _plan_cell_t *plan_next;
} plan_cell_t;
// Planner info
typedef struct
{
// Grid dimensions (number of cells)
int size_x, size_y;
// Grid bounds (for limiting the search).
int min_x, min_y, max_x, max_y;
// Grid origin (real-world coords, in meters, of the lower-left grid
// cell)
double origin_x, origin_y;
// Grid scale (m/cell)
double scale;
// Effective robot radius
double des_min_radius, abs_min_radius;
// Max radius we will consider
double max_radius;
// Penalty factor for cells inside the max radius
double dist_penalty;
// Cost multiplier for cells on the previous local path
double hysteresis_factor;
// The grid data
plan_cell_t *cells;
// Distance penalty kernel, pre-computed in plan_compute_dist_kernel();
float* dist_kernel;
int dist_kernel_width;
float dist_kernel_3x3[9];
// Priority queue of cells to update
heap_t* heap;
// The global path
int path_count, path_size;
plan_cell_t **path;
// The local path (mainly for debugging)
int lpath_count, lpath_size;
plan_cell_t **lpath;
// Waypoints extracted from global path
int waypoint_count, waypoint_size;
plan_cell_t **waypoints;
} plan_t;
// Create a planner
plan_t *plan_alloc(double abs_min_radius,
double des_min_radius,
double max_radius,
double dist_penalty,
double hysteresis_factor);
void plan_compute_dist_kernel(plan_t* plan);
// Destroy a planner
void plan_free(plan_t *plan);
// Initialize the plan
void plan_init(plan_t *plan);
// Reset the plan
void plan_reset(plan_t *plan);
#if 0
// Load the occupancy values from an image file
int plan_load_occ(plan_t *plan, const char *filename, double scale);
#endif
void plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y);
void plan_set_bbox(plan_t* plan, double padding, double min_size,
double x0, double y0, double x1, double y1);
int plan_check_inbounds(plan_t* plan, double x, double y);
// Construct the configuration space from the occupancy grid.
//void plan_update_cspace(plan_t *plan, const char* cachefile);
void plan_compute_cspace(plan_t *plan);
int plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy);
int plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth);
// Generate a path to the goal
void plan_update_waypoints(plan_t *plan, double px, double py);
// Get the ith waypoint; returns zero if there are no more waypoints
int plan_get_waypoint(plan_t *plan, int i, double *px, double *py);
// Convert given waypoint cell to global x,y
void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint,
double *px, double *py);
double plan_get_carrot(plan_t* plan, double* px, double* py,
double lx, double ly,
double maxdist, double distweight);
int plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va,
int* rotate_dir,
double lx, double ly, double la,
double gx, double gy, double ga,
double goal_d, double goal_a,
double maxd, double dweight,
double tvmin, double tvmax,
double avmin, double avmax,
double amin, double amax);
int plan_check_done(plan_t* plan,
double lx, double ly, double la,
double gx, double gy, double ga,
double goal_d, double goal_a);
void plan_set_obstacles(plan_t* plan, double* obs, size_t num);
#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
// Write the cspace occupancy distance values to a file, one per line.
// Read them back in with plan_read_cspace().
// Returns non-zero on error.
int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash);
// Read the cspace occupancy distance values from a file, one per line.
// Write them in first with plan_read_cspace().
// Returns non-zero on error.
int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash);
// Compute and return the 16-bit MD5 hash of the map data in the given plan
// object.
void plan_md5(unsigned int* digest, plan_t* plan);
#endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
/**************************************************************************
* Plan manipulation macros
**************************************************************************/
// Convert from plan index to world coords
//#define PLAN_WXGX(plan, i) (((i) - plan->size_x / 2) * plan->scale)
//#define PLAN_WYGY(plan, j) (((j) - plan->size_y / 2) * plan->scale)
#define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale)
#define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale)
// Convert from world coords to plan coords
//#define PLAN_GXWX(plan, x) (floor((x) / plan->scale + 0.5) + plan->size_x / 2)
//#define PLAN_GYWY(plan, y) (floor((y) / plan->scale + 0.5) + plan->size_y / 2)
#define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5))
#define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5))
// Test to see if the given plan coords lie within the absolute plan bounds.
#define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
// Test to see if the given plan coords lie within the user-specified plan bounds
#define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y))
// Compute the cell index for the given plan coords.
#define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)
#ifdef __cplusplus
}
#endif
#endif
plan_t
Definition: plan.h:72
_plan_cell_t
Definition: plan.h:44
heap
Definition: heap.h:42

Last updated 12 September 2005 21:38:45