00001
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>
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
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;
00055 double sonar_aperture;
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;
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;
00090 double P;
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;
00119 float sonar_treshold;
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
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 }