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 
Definition: player_interfaces.h:3034
uint32_t data_count
The number of cells.
Definition: player_interfaces.h:3066
struct player_map_data player_map_data_t
Request/reply: get grid map tile.
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:771
Data AND Request/reply: geometry.
Definition: player_interfaces.h:785
int8_t * data
Cell occupancy value.
Definition: player_interfaces.h:3070
Generic message header.
Definition: player.h:161
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:74
double px
X [m].
Definition: player.h:220
Request/reply: get grid map tile.
Definition: player_interfaces.h:3055
int ReadInt(int section, const char *name, int value)
Read an integer value.
void * GetPayload()
Get pointer to payload.
Definition: message.h:188
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
double py
Y [m].
Definition: player.h:222
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
#define PLAYER_WARN2(msg, a, b)
Error message macros.
Definition: error.h:91
uint32_t row
The tile origin [pixels].
Definition: player_interfaces.h:3060
int width
the map is defined as x,y -> pose (px,py,pa,P)
Definition: gridmap.h:117
uint32_t width
The size of the tile [pixels].
Definition: player_interfaces.h:3062
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
uint32_t height
The size of the tile [pixels].
Definition: player_interfaces.h:3064
#define PLAYER_SONAR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:761
Definition: gridmap.h:90
float * ranges
The range readings [m].
Definition: player_interfaces.h:776
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
#define PLAYER_SONAR_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:755
Class for loading configuration file information.
Definition: configfile.h:196
#define PLAYER_MAP_REQ_GET_DATA
Request/reply subtype: get grid map tile
Definition: player_interfaces.h:3024
uint32_t col
The tile origin [pixels].
Definition: player_interfaces.h:3058
#define PLAYER_CAPABILITIES_REQ
Capability request message type.
Definition: player.h:397
A device address.
Definition: player.h:145
An autopointer for the message queue.
Definition: message.h:73
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:611
Definition: gridmap.h:67
position2d data
Definition: player_interfaces.h:606
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:552
uint32_t ranges_count
The number of valid range readings.
Definition: player_interfaces.h:774
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
Definition: gridmap.h:111
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
Reference-counted message objects.
Definition: message.h:132
double pa
yaw [rad]
Definition: player.h:224
Base class for all drivers.
Definition: driver.h:108
Definition: gridmap.h:52
#define PLAYER_MAP_DATA_INFO
Data subtype: grid map metadata.
Definition: player_interfaces.h:3033
#define PLAYER_MAP_REQ_GET_INFO
Request/reply subtype: get grid map metadata
Definition: player_interfaces.h:3021