amcl_laser.cc

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00022 //
00023 // Desc: AMCL laser routines
00024 // Author: Andrew Howard
00025 // Date: 6 Feb 2003
00026 // CVS: $Id: amcl_laser.cc 6496 2008-06-09 22:32:57Z thjc $
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> // required by Darwin
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 // Default constructor
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 // Load laser settings
00058 int AMCLLaser::Load(ConfigFile* cf, int section)
00059 {
00060   // Get the map settings.  Don't error check here; we'll do it later, in
00061   // SetupMap().
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 // Unload the model
00077 int AMCLLaser::Unload(void)
00078 {
00079   //laser_free(this->model);
00080   //this->model = NULL;
00081   
00082   return 0;
00083 }
00084 
00085 
00087 // Set up the laser
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   // Subscribe to the Laser device
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   // Ask for the laser's geometry
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     // Set the laser pose relative to the robot
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 // TODO: should Unsubscribe from the map on error returns in the function
00138 int
00139 AMCLLaser::SetupMap(void)
00140 {
00141   Device* mapdev;
00142 
00143   // Subscribe to the map device
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   // Create the map
00156   this->map = map_alloc();
00157   PLAYER_MSG1(2, "AMCL loading map from map:%d...", this->map_addr.index);
00158 
00159   // Fill in the map structure (I'm doing it here instead of in libmap, 
00160   // because libmap is written in C, so it'd be a pain to invoke the internal 
00161   // device API from there)
00162 
00163   // first, get the map info
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   // copy in the map info
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   // allocate space for map cells
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   // now, get the map data
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   // Tile size, limit to sensible maximum of 640x640 tiles
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     // copy the map data
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   // we're done with the map device now
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 // Shut down the laser
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 // Get the current laser reading
00277 //AMCLSensorData *AMCLLaser::GetData(void)
00278 // Process message for this interface
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   // Read the range data
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 // Apply the laser sensor model
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   // Apply the laser sensor model
00332   pf_update_sensor(pf, (pf_sensor_model_fn_t) SensorModel, data);
00333   
00334   return true;
00335 }
00336 
00337 
00339 // Determine the probability for the given pose
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   // Compute the sample weights
00357   for (j = 0; j < set->sample_count; j++)
00358   {
00359     sample = set->samples + j;
00360     pose = sample->pose;
00361 
00362     // Take account of the laser pose relative to the robot
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       // Compute the range according to the map
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         // TODO: proper sensor model (using Kolmagorov?)
00384         // Simple gaussian model
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          if (obs->range >= 8.0 && map_range >= 8.0)
00392          p *= 1.0;
00393          else if (obs->range >= 8.0 && map_range < 8.0)
00394          p *= self->range_bad;
00395          else if (obs->range < 8.0 && map_range >= 8.0)
00396          p *= self->range_bad;
00397          else
00398          p *= laser_sensor_prob(self, obs->range, map_range);
00399        */
00400 
00401       p *= pz;
00402     }
00403 
00404     //printf("%e\n", p);
00405     //assert(p >= 0);
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 // Setup the GUI
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   // Draw the laser map
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 // Shutdown the GUI
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 // Draw the laser values
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   // Draw the complete scan
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   // Draw the significant part of the scan
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     //m = map_calc_range(this->map, pose.v[0], pose.v[1], pose.v[2] + b, 8.0);
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 

Last updated 12 September 2005 21:38:45