pmap.h

Go to the documentation of this file.

/*
  pmap: simple mapping utilities
  Copyright (C) 2004 Andrew Howard  ahoward@usc.edu

  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.
*/

#ifndef PMAP_H
#define PMAP_H

#ifdef __cplusplus
extern "C"
{
#endif

#include "gsl/gsl_rng.h"
#include "slap.h"


#define PMAP_MAX_RANGES 401
  
  
typedef struct
{
  int dx, dy;

  double dist;
  
} pmap_nbor_t;


typedef struct
{
  double time;
  
  double ranges[PMAP_MAX_RANGES];
  
} pmap_scan_t;


typedef struct
{
  double w;
  
  double err;

  pose2_t pose;

  vector2_t *global_points;

  pose2_t *poses;
  
  signed char *cells;

} pmap_sample_t;


  
typedef struct
{
  int num_ranges;

  double range_max;

  double range_start;

  double range_step;

  double grid_res;
  int grid_sx, grid_sy;

  int traj_size, grid_size;

  matrix44_t action_model;

  double max_err;

  int resample_interval;
  
  double resample_s;

  int num_nbors;
  pmap_nbor_t *nbors;

  int scan_count;

  int step_count;

  int step_max_count;

  pmap_scan_t *scans;

  int samples_len, sample_set;

  pmap_sample_t *samples;

  int best_sample;
  
  pose2_t initial_pose;

  pose2_t odom_pose;
  double odom_dist, odom_turn;

  double update_dist, update_turn;

  int resample_count;
  
  gsl_rng *rng;

} pmap_t;


pmap_t *pmap_alloc(int num_ranges, double range_max,
                   double range_start, double range_step, int samples_len,
                   double grid_width, double grid_height, double grid_scale);

void pmap_free(pmap_t *self);

void pmap_init_nbors(pmap_t *self);

void pmap_set_pose(pmap_t *self, pose2_t pose);

void pmap_update(pmap_t *self, double dtime, pose2_t odom_pose, int num_ranges, double *ranges);

void pmap_apply_action(pmap_t *self, pose2_t delta);

void pmap_add_scan(pmap_t *self, double dtime, double *ranges);

void pmap_add_scan_sample(pmap_t *self, int sample_index, double *ranges);

void pmap_apply_sensor(pmap_t *self, double *ranges);

void pmap_apply_sensor_sample(pmap_t *self, int sample_index, double *ranges);

double pmap_entropy(pmap_t *self, int scan_count);

void pmap_resample(pmap_t *self, int scan_count);

void pmap_draw_scan(pmap_t *self, double *ranges);

void pmap_draw_samples(pmap_t *self);

void pmap_draw_sample(pmap_t *self, int sample_index);

void pmap_draw_map(pmap_t *self, double scale);

void pmap_draw_sample_map(pmap_t *self, double scale, int sample_index);

#define PMAP_GET_SAMPLE(self, i) (self->samples + self->sample_set * self->samples_len + (i))

#define PMAP_GRIDX(self, x) ((int) floor((x) / self->grid_res) + self->grid_sx / 2)
#define PMAP_GRIDY(self, y) ((int) floor((y) / self->grid_res) + self->grid_sy / 2)
#define PMAP_GRID_VALID(self, x, y) ((x) >= 0 && (x) < self->grid_sx && (y) >= 0 && (y) < self->grid_sy)
#define PMAP_GRID_INDEX(self, x, y) ((x) + (y) * self->grid_sx)

  
#ifdef __cplusplus
}
#endif

#endif

---