Player
Frontpage
Contents
User
Installation
Quick start
Supported devices
Tutorials
Utilities
Client libraries
FAQ
Help
Developer
Architecture
libplayercore
interfaces
libplayerdrivers
drivers
libplayercommon
libplayerutils
libplayersd
libplayertcp
libplayerxdr
TODO
Online
Homepage
Download
Project
Bugs
Help
server
drivers
map
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
}
Last updated 12 September 2005 21:38:45