gridmap.h
00001 // objects definitions
00002 #if !defined(WIN32)
00003         #include <unistd.h>
00004         #include <netinet/in.h>
00005 #endif
00006 #include <string.h>
00007 #include <math.h>
00008 #include <libplayercore/playercore.h>
00009 #include <iostream>
00010 #include <map>  //stl
00011 /*
00012  *  Player - One Hell of a Robot Server
00013  *  Copyright (C) 2003  
00014  *     Brian Gerkey, Andrew Howard
00015  *                      
00016  * 
00017  *  This program is free software; you can redistribute it and/or modify
00018  *  it under the terms of the GNU General Public License as published by
00019  *  the Free Software Foundation; either version 2 of the License, or
00020  *  (at your option) any later version.
00021  *
00022  *  This program is distributed in the hope that it will be useful,
00023  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00024  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00025  *  GNU General Public License for more details.
00026  *
00027  *  You should have received a copy of the GNU General Public License
00028  *  along with this program; if not, write to the Free Software
00029  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00030  *
00031  */
00032 
00033 /*
00034  * Desc: A Mapping driver which maps from sonar data
00035  * from the multidriver example by Andrew Howard
00036  * Author: Marco Paladini, (mail: breakthru@inwind.it)
00037  * Date: 29 April 2006
00038  */
00040 #define LOCAL2GLOBAL_X(x,y,px,py,pa) (cos(pa)*(x) - sin(pa)*(y) + px)
00041 #define LOCAL2GLOBAL_Y(x,y,px,py,pa) (sin(pa)*(x) + cos(pa)*(y) + py)
00042 #define MAP_INDEX(map, i, j) (int)((i) + (j) * map.width)
00043 #define MAP_VALID(map, i, j) ((i >= 0) && (i <= (int)map.width) && (j >= 0) && (j <= (int)map.height))
00044 #define ROTATE_X(x,y,th) (cos(th)*(x) - sin(th)*(y))
00045 #define ROTATE_Y(x,y,th) (sin(th)*(x) + cos(th)*(y))
00046 
00047 using namespace std;
00048 
00049 
00050 class Sonar
00051 {
00052 public:
00053 double px,py,th;
00054 double sonar_treshold; //default to 4.5
00055 double sonar_aperture; // 30 degrees
00056 double sensor_model(double x,double y,double r);
00057         Sonar() 
00058         {
00059         sonar_treshold=4.5;
00060         sonar_aperture=0.5235987;
00061         }
00062 };
00063 
00064 class MAP_POINT
00065 {
00066 public:
00067 int x;
00068 int y; // coordinates on map
00069 
00070 MAP_POINT(int x1,int y1)
00071 {
00072   x=x1;
00073   y=y1;
00074 }
00075 
00076  bool operator<(const MAP_POINT &b) const {
00077    if (x < b.x) return(1);
00078    else if (x == b.x) return(y < b.y);
00079    else return(0);
00080  }
00081 
00082 };
00083 
00084 class MAP_POSE
00085 {
00086 public:
00087 double px; 
00088 double py;
00089 double pa; // where the robot was when this point was added
00090 double P;  // occupancy probability
00091 
00092  MAP_POSE()
00093    {pa=px=py=P=0;}
00094 
00095 MAP_POSE(double px1,
00096          double py1,
00097          double pa1,
00098          double P1)   {
00099  pa=pa1;
00100  px=px1;
00101  py=py1;
00102  P=P1;
00103  }
00104 
00105 };
00106 
00107 
00108 class Map : public map<MAP_POINT,MAP_POSE>
00109 {
00113 public:
00114 int width;
00115 int height;
00116 int startx;
00117 int starty;
00118 float scale; //default to 0.028
00119 float sonar_treshold; //default to 4.5
00120 Map();
00121 Map(int width,
00122     int height,
00123     int startx,
00124     int starty,
00125     int scale,
00126     int sonar_treshold);
00127 ~Map();
00128 player_map_data_t ToPlayer();
00129 };
00130 
00131 Map::~Map() {
00132 }
00133 
00134 Map::Map()
00135 {
00136 //some default values (not always good)
00137 width=800;
00138 height=800;
00139 startx=0;
00140 starty=0;
00141 scale=0.028f;
00142 sonar_treshold=4.5;
00143 }
00144 
00145 Map::Map(int width,
00146     int height,
00147     int startx,
00148     int starty,
00149     int scale,
00150     int sonar_treshold)
00151 {
00152 std::cout<< "not implemented yet" << std::endl;
00153 }
00154 
00155 
00156 
00157 double Sonar::sensor_model(double x,double y,double r)
00158 {
00159 return(
00160 exp((-pow(x,2)/r)-(pow(y,2)/sonar_aperture))/((double)1.7)
00161 );
00162 
00163 }