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