gridmap.h
1 // objects definitions
2 #if !defined(WIN32)
3  #include <unistd.h>
4  #include <netinet/in.h>
5 #endif
6 #include <string.h>
7 #include <math.h>
8 #include <libplayercore/playercore.h>
9 #include <iostream>
10 #include <map> //stl
11 /*
12  * Player - One Hell of a Robot Server
13  * Copyright (C) 2003
14  * Brian Gerkey, Andrew Howard
15  *
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation; either version 2 of the License, or
20  * (at your option) any later version.
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  *
31  */
32 
33 /*
34  * Desc: A Mapping driver which maps from sonar data
35  * from the multidriver example by Andrew Howard
36  * Author: Marco Paladini, (mail: breakthru@inwind.it)
37  * Date: 29 April 2006
38  */
40 #define LOCAL2GLOBAL_X(x,y,px,py,pa) (cos(pa)*(x) - sin(pa)*(y) + px)
41 #define LOCAL2GLOBAL_Y(x,y,px,py,pa) (sin(pa)*(x) + cos(pa)*(y) + py)
42 #define MAP_INDEX(map, i, j) (int)((i) + (j) * map.width)
43 #define MAP_VALID(map, i, j) ((i >= 0) && (i <= (int)map.width) && (j >= 0) && (j <= (int)map.height))
44 #define ROTATE_X(x,y,th) (cos(th)*(x) - sin(th)*(y))
45 #define ROTATE_Y(x,y,th) (sin(th)*(x) + cos(th)*(y))
46 
47 using namespace std;
48 
49 
50 class Sonar
51 {
52 public:
53 double px,py,th;
54 double sonar_treshold; //default to 4.5
55 double sonar_aperture; // 30 degrees
56 double sensor_model(double x,double y,double r);
57  Sonar()
58  {
59  sonar_treshold=4.5;
60  sonar_aperture=0.5235987;
61  }
62 };
63 
64 class MAP_POINT
65 {
66 public:
67 int x;
68 int y; // coordinates on map
69 
70 MAP_POINT(int x1,int y1)
71 {
72  x=x1;
73  y=y1;
74 }
75 
76  bool operator<(const MAP_POINT &b) const {
77  if (x < b.x) return(1);
78  else if (x == b.x) return(y < b.y);
79  else return(0);
80  }
81 
82 };
83 
84 class MAP_POSE
85 {
86 public:
87 double px;
88 double py;
89 double pa; // where the robot was when this point was added
90 double P; // occupancy probability
91 
92  MAP_POSE()
93  {pa=px=py=P=0;}
94 
95 MAP_POSE(double px1,
96  double py1,
97  double pa1,
98  double P1) {
99  pa=pa1;
100  px=px1;
101  py=py1;
102  P=P1;
103  }
104 
105 };
106 
107 
108 class Map : public map<MAP_POINT,MAP_POSE>
109 {
113 public:
114 int width;
115 int height;
116 int startx;
117 int starty;
118 float scale; //default to 0.028
119 float sonar_treshold; //default to 4.5
120 Map();
121 Map(int width,
122  int height,
123  int startx,
124  int starty,
125  int scale,
126  int sonar_treshold);
127 ~Map();
128 player_map_data_t ToPlayer();
129 };
130 
131 Map::~Map() {
132 }
133 
134 Map::Map()
135 {
136 //some default values (not always good)
137 width=800;
138 height=800;
139 startx=0;
140 starty=0;
141 scale=0.028f;
142 sonar_treshold=4.5;
143 }
144 
145 Map::Map(int width,
146  int height,
147  int startx,
148  int starty,
149  int scale,
150  int sonar_treshold)
151 {
152 std::cout<< "not implemented yet" << std::endl;
153 }
154 
155 
156 
157 double Sonar::sensor_model(double x,double y,double r)
158 {
159 return(
160 exp((-pow(x,2)/r)-(pow(y,2)/sonar_aperture))/((double)1.7)
161 );
162 
163 }
Definition: gridmap.h:64
int width
the map is defined as x,y -> pose (px,py,pa,P)
Definition: gridmap.h:114
Definition: gridmap.h:84
Definition: gridmap.h:50
Request/reply: get grid map tile.
Definition: player_interfaces.h:3032
Definition: gridmap.h:108

Last updated 12 September 2005 21:38:45