Player
Frontpage
Contents
User
Installation
Quick start
Supported devices
Tutorials
Utilities
Client libraries
FAQ
Help
Developer
Architecture
libplayercore
libplayerinterface
interfaces
libplayerdrivers
drivers
libplayercommon
libplayersd
libplayertcp
libplayerxdr
TODO
Online
Homepage
Download
Project
Bugs
Help
server
drivers
map
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
49
using namespace
std;
50
51
52
class
Sonar
53
{
54
public
:
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
67
class
MAP_POINT
68
{
69
public
:
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
90
class
MAP_POSE
91
{
92
public
:
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
111
class
Map
:
public
map<MAP_POINT,MAP_POSE>
112
{
116
public
:
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
136
Map::~Map() {
137
}
138
139
Map::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
150
Map::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
160
double
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
MAP_POINT
Definition
gridmap.h:68
MAP_POSE
Definition
gridmap.h:91
Map
Definition
gridmap.h:112
Map::width
int width
the map is defined as x,y -> pose (px,py,pa,P)
Definition
gridmap.h:117
Sonar
Definition
gridmap.h:53
Generated on Thu Jul 25 2024 00:00:00 for Player by
1.12.0