00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00022
00023
00024
00025
00026
00027
00029
00030 #ifdef HAVE_CONFIG_H
00031 #include "config.h"
00032 #endif
00033
00034 #define PLAYER_ENABLE_MSG 1
00035
00036 #include <sys/types.h>
00037 #include <math.h>
00038 #include <stdlib.h>
00039 #include <unistd.h>
00040
00041 #include <libplayercore/playercore.h>
00042 #include <libplayercore/error.h>
00043 #include "amcl_laser.h"
00044
00046
00047 AMCLLaser::AMCLLaser(AdaptiveMCL & aAMCL, player_devaddr_t addr) : AMCLSensor(aAMCL)
00048 {
00049 this->laser_dev = NULL;
00050 this->laser_addr = addr;
00051
00052 return;
00053 }
00054
00055
00057
00058 int AMCLLaser::Load(ConfigFile* cf, int section)
00059 {
00060
00061
00062 cf->ReadDeviceAddr(&(this->map_addr), section, "requires",
00063 PLAYER_MAP_CODE, -1, "laser");
00064
00065 this->max_beams = cf->ReadInt(section, "laser_max_beams", 6);
00066 this->range_var = cf->ReadLength(section, "laser_range_var", 0.10);
00067 this->range_bad = cf->ReadFloat(section, "laser_range_bad", 0.10);
00068
00069 this->time = 0.0;
00070
00071 return 0;
00072 }
00073
00074
00076
00077 int AMCLLaser::Unload(void)
00078 {
00079
00080
00081
00082 return 0;
00083 }
00084
00085
00087
00088 int AMCLLaser::Setup(void)
00089 {
00090 if(this->SetupMap() < 0)
00091 {
00092 PLAYER_ERROR("failed to get laser map");
00093 return(-1);
00094 }
00095
00096
00097 this->laser_dev = deviceTable->GetDevice(this->laser_addr);
00098 if (!this->laser_dev)
00099 {
00100 PLAYER_ERROR("unable to locate suitable laser device");
00101 return -1;
00102 }
00103 if (this->laser_dev->Subscribe(AMCL.InQueue) != 0)
00104 {
00105 PLAYER_ERROR("unable to subscribe to laser device");
00106 return -1;
00107 }
00108
00109
00110 Message* msg;
00111 if(!(msg = laser_dev->Request(AMCL.InQueue,
00112 PLAYER_MSGTYPE_REQ,
00113 PLAYER_LASER_REQ_GET_GEOM,
00114 NULL, 0, NULL,false)))
00115 {
00116 PLAYER_WARN("failed to get laser geometry");
00117 this->laser_pose.v[0] = 0.0;
00118 this->laser_pose.v[1] = 0.0;
00119 this->laser_pose.v[2] = 0.0;
00120 }
00121 else
00122 {
00123 player_laser_geom_t* geom = (player_laser_geom_t*)msg->GetPayload();
00124
00125 this->laser_pose.v[0] = geom->pose.px;
00126 this->laser_pose.v[1] = geom->pose.py;
00127 this->laser_pose.v[2] = geom->pose.pyaw;
00128 PLAYER_MSG3(2, "laser geometry: %f,%f,%f",
00129 this->laser_pose.v[0],
00130 this->laser_pose.v[1],
00131 RTOD(this->laser_pose.v[2]));
00132 delete msg;
00133 }
00134 return 0;
00135 }
00136
00137
00138 int
00139 AMCLLaser::SetupMap(void)
00140 {
00141 Device* mapdev;
00142
00143
00144 if(!(mapdev = deviceTable->GetDevice(this->map_addr)))
00145 {
00146 PLAYER_ERROR("unable to locate suitable map device");
00147 return -1;
00148 }
00149 if(mapdev->Subscribe(AMCL.InQueue) != 0)
00150 {
00151 PLAYER_ERROR("unable to subscribe to map device");
00152 return -1;
00153 }
00154
00155
00156 this->map = map_alloc();
00157 PLAYER_MSG1(2, "AMCL loading map from map:%d...", this->map_addr.index);
00158
00159
00160
00161
00162
00163
00164 Message* msg;
00165 if(!(msg = mapdev->Request(AMCL.InQueue,
00166 PLAYER_MSGTYPE_REQ,
00167 PLAYER_MAP_REQ_GET_INFO,
00168 NULL, 0, NULL, false)))
00169 {
00170 PLAYER_ERROR("failed to get map info");
00171 return(-1);
00172 }
00173
00174 PLAYER_MSG1(2, "AMCL loading map from map:%d...Done", this->map_addr.index);
00175
00176 player_map_info_t* info = (player_map_info_t*)msg->GetPayload();
00177
00178
00179 this->map->origin_x = info->origin.px + (info->scale * info->width) / 2.0;
00180 this->map->origin_y = info->origin.py + (info->scale * info->height) / 2.0;
00181 this->map->scale = info->scale;
00182 this->map->size_x = info->width;
00183 this->map->size_y = info->height;
00184
00185 delete msg;
00186
00187
00188 this->map->cells = (map_cell_t*)malloc(sizeof(map_cell_t) *
00189 this->map->size_x *
00190 this->map->size_y);
00191 assert(this->map->cells);
00192
00193
00194 player_map_data_t* data_req;
00195 int i,j;
00196 int oi,oj;
00197 int sx,sy;
00198 int si,sj;
00199
00200 data_req = (player_map_data_t*) malloc(sizeof(player_map_data_t));
00201 assert(data_req);
00202
00203
00204 sy = sx = 640;
00205 oi=oj=0;
00206 while((oi < this->map->size_x) && (oj < this->map->size_y))
00207 {
00208 si = MIN(sx, this->map->size_x - oi);
00209 sj = MIN(sy, this->map->size_y - oj);
00210
00211 data_req->col = oi;
00212 data_req->row = oj;
00213 data_req->width = si;
00214 data_req->height = sj;
00215 data_req->data_count = 0;
00216
00217 if(!(msg = mapdev->Request(AMCL.InQueue,
00218 PLAYER_MSGTYPE_REQ,
00219 PLAYER_MAP_REQ_GET_DATA,
00220 (void*)data_req,0,NULL,false)))
00221 {
00222 PLAYER_ERROR("failed to get map info");
00223 free(data_req);
00224 free(this->map->cells);
00225 return(-1);
00226 }
00227
00228 player_map_data_t* mapdata = (player_map_data_t*)msg->GetPayload();
00229
00230
00231 for(j=0;j<sj;j++)
00232 {
00233 for(i=0;i<si;i++)
00234 {
00235 this->map->cells[MAP_INDEX(this->map,oi+i,oj+j)].occ_state =
00236 mapdata->data[j*si + i];
00237 this->map->cells[MAP_INDEX(this->map,oi+i,oj+j)].occ_dist = 0;
00238 }
00239 }
00240
00241 delete msg;
00242
00243 oi += si;
00244 if(oi >= this->map->size_x)
00245 {
00246 oi = 0;
00247 oj += sj;
00248 }
00249 }
00250
00251 free(data_req);
00252
00253
00254 if(mapdev->Unsubscribe(AMCL.InQueue) != 0)
00255 PLAYER_WARN("unable to unsubscribe from map device");
00256
00257 PLAYER_MSG0(2, "Done");
00258
00259 return(0);
00260 }
00261
00262
00264
00265 int AMCLLaser::Shutdown(void)
00266 {
00267 this->laser_dev->Unsubscribe(AMCL.InQueue);
00268 this->laser_dev = NULL;
00269 map_free(this->map);
00270
00271 return 0;
00272 }
00273
00274
00276
00277
00278
00279 int AMCLLaser::ProcessMessage(QueuePointer &resp_queue,
00280 player_msghdr * hdr,
00281 void * idata)
00282 {
00283 int i;
00284 double r, b, db;
00285 AMCLLaserData *ndata;
00286
00287 if(!Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
00288 PLAYER_LASER_DATA_SCAN, this->laser_addr))
00289 return -1;
00290
00291 this->time = hdr->timestamp;
00292 player_laser_data_t* data = reinterpret_cast<player_laser_data_t*> (idata);
00293
00294 b = data->min_angle;
00295 db = data->resolution;
00296
00297 ndata = new AMCLLaserData;
00298
00299 ndata->sensor = this;
00300 ndata->time = hdr->timestamp;
00301
00302 ndata->range_count = data->ranges_count;
00303 ndata->range_max = data->max_range;
00304 ndata->ranges = new double [ndata->range_count][2];
00305
00306
00307 for (i = 0; i < ndata->range_count; i++)
00308 {
00309 r = data->ranges[i];
00310 ndata->ranges[i][0] = r;
00311 ndata->ranges[i][1] = b;
00312 b += db;
00313 }
00314
00315 AMCL.Push(ndata);
00316
00317 return 0;
00318 }
00319
00320
00322
00323 bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
00324 {
00325 AMCLLaserData *ndata;
00326
00327 ndata = (AMCLLaserData*) data;
00328 if (this->max_beams < 2)
00329 return false;
00330
00331
00332 pf_update_sensor(pf, (pf_sensor_model_fn_t) SensorModel, data);
00333
00334 return true;
00335 }
00336
00337
00339
00340 double AMCLLaser::SensorModel(AMCLLaserData *data, pf_sample_set_t* set)
00341 {
00342 AMCLLaser *self;
00343 int i, j, step;
00344 double z, c, pz;
00345 double p;
00346 double map_range;
00347 double obs_range, obs_bearing;
00348 double total_weight;
00349 pf_sample_t *sample;
00350 pf_vector_t pose;
00351
00352 self = (AMCLLaser*) data->sensor;
00353
00354 total_weight = 0.0;
00355
00356
00357 for (j = 0; j < set->sample_count; j++)
00358 {
00359 sample = set->samples + j;
00360 pose = sample->pose;
00361
00362
00363 pose = pf_vector_coord_add(self->laser_pose, pose);
00364
00365 p = 1.0;
00366
00367 step = (data->range_count - 1) / (self->max_beams - 1);
00368 for (i = 0; i < data->range_count; i += step)
00369 {
00370 obs_range = data->ranges[i][0];
00371 obs_bearing = data->ranges[i][1];
00372
00373
00374 map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
00375 pose.v[2] + obs_bearing, data->range_max + 1.0);
00376
00377 if (obs_range >= data->range_max && map_range >= data->range_max)
00378 {
00379 pz = 1.0;
00380 }
00381 else
00382 {
00383
00384
00385 c = self->range_var;
00386 z = obs_range - map_range;
00387 pz = self->range_bad + (1 - self->range_bad) * exp(-(z * z) / (2 * c * c));
00388 }
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401 p *= pz;
00402 }
00403
00404
00405
00406
00407 sample->weight *= p;
00408 total_weight += sample->weight;
00409 }
00410
00411 return(total_weight);
00412 }
00413
00414
00415
00416 #ifdef INCLUDE_RTKGUI
00417
00419
00420 void AMCLLaser::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig)
00421 {
00422 this->fig = rtk_fig_create(canvas, robot_fig, 0);
00423
00424
00425 this->map_fig = rtk_fig_create(canvas, NULL, -50);
00426 map_draw_occ(this->map, this->map_fig);
00427
00428 return;
00429 }
00430
00431
00433
00434 void AMCLLaser::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig)
00435 {
00436 rtk_fig_destroy(this->map_fig);
00437 rtk_fig_destroy(this->fig);
00438 this->map_fig = NULL;
00439 this->fig = NULL;
00440
00441 return;
00442 }
00443
00444
00446
00447 void AMCLLaser::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data)
00448 {
00449 int i, step;
00450 double r, b, ax, ay, bx, by;
00451 AMCLLaserData *ndata;
00452
00453 ndata = (AMCLLaserData*) data;
00454
00455 rtk_fig_clear(this->fig);
00456
00457
00458 rtk_fig_color_rgb32(this->fig, 0x8080FF);
00459 for (i = 0; i < ndata->range_count; i++)
00460 {
00461 r = ndata->ranges[i][0];
00462 b = ndata->ranges[i][1];
00463
00464 ax = 0;
00465 ay = 0;
00466 bx = ax + r * cos(b);
00467 by = ay + r * sin(b);
00468
00469 rtk_fig_line(this->fig, ax, ay, bx, by);
00470 }
00471
00472 if (this->max_beams < 2)
00473 return;
00474
00475
00476 step = (ndata->range_count - 1) / (this->max_beams - 1);
00477 for (i = 0; i < ndata->range_count; i += step)
00478 {
00479 r = ndata->ranges[i][0];
00480 b = ndata->ranges[i][1];
00481
00482
00483 ax = 0;
00484 ay = 0;
00485
00486 bx = ax + r * cos(b);
00487 by = ay + r * sin(b);
00488 rtk_fig_color_rgb32(this->fig, 0xFF0000);
00489 rtk_fig_line(this->fig, ax, ay, bx, by);
00490 }
00491
00492 return;
00493 }
00494
00495 #endif
00496
00497
00498