maptransform.cc

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2004  Brian Gerkey gerkey@stanford.edu    
00004  *
00005  *  This program is free software; you can redistribute it and/or modify
00006  *  it under the terms of the GNU General Public License as published by
00007  *  the Free Software Foundation; either version 2 of the License, or
00008  *  (at your option) any later version.
00009  *
00010  *  This program is distributed in the hope that it will be useful,
00011  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  *  GNU General Public License for more details.
00014  *
00015  *  You should have received a copy of the GNU General Public License
00016  *  along with this program; if not, write to the Free Software
00017  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018  *
00019  */
00020 
00021 /*
00022  * $Id: maptransform.cc 4340 2008-02-04 19:40:27Z thjc $
00023  *
00024  * Base class for map transform drivers, simply reimplement the transform method
00025  * with your trasformation function. See MapScale for example
00026  */
00027 
00028 #include "maptransform.h"
00029 
00030 // this one has no data or commands, just configs
00031 MapTransform::MapTransform(ConfigFile* cf, int section)
00032   : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_MAP_CODE)
00033 {
00034   PLAYER_MSG0(9,"Initialising the MapTransform Driver");
00035   memset(&source_map,0,sizeof(source_map));
00036   memset(&new_map,0,sizeof(new_map));
00037 
00038   // Find our source map
00039   if(cf->ReadDeviceAddr(&source_map_addr, section, "requires",
00040                         PLAYER_MAP_CODE, -1,NULL) != 0)
00041   {
00042     PLAYER_ERROR("must specify source map");
00043     return;     
00044   }
00045         
00046   this->source_data = this->new_data = NULL;
00047 }
00048 
00049 MapTransform::~MapTransform()
00050 {
00051 }
00052 
00053 int
00054 MapTransform::Setup()
00055 {
00056   if(this->GetMap() < 0)
00057     return(-1);
00058   if(this->Transform() < 0)
00059     return(-1);
00060 
00061   delete [] source_data;
00062   source_data = NULL;
00063 
00064   return(0);
00065 }
00066 
00067 // get the map from the underlying map device
00068 // TODO: should Unsubscribe from the map on error returns in the function
00069 int
00070 MapTransform::GetMap()
00071 {
00072   Device* mapdev;
00073 
00074   // Subscribe to the map device
00075   if(!(mapdev = deviceTable->GetDevice(this->source_map_addr)))
00076   {
00077     PLAYER_ERROR("unable to locate suitable map device");
00078     return -1;
00079   }
00080   if(mapdev->Subscribe(this->InQueue) != 0)
00081   {
00082     PLAYER_ERROR("unable to subscribe to map device");
00083     return -1;
00084   }
00085 
00086   // first, get the map info
00087   Message* msg;
00088   if(!(msg = mapdev->Request(this->InQueue,
00089                              PLAYER_MSGTYPE_REQ,
00090                              PLAYER_MAP_REQ_GET_INFO,
00091                              NULL, 0, NULL, false)))
00092   {
00093     PLAYER_ERROR("failed to get map info");
00094     return(-1);
00095   }
00096 
00097   // copy in the map info
00098   source_map = *(player_map_info_t*)msg->GetPayload();
00099   
00100   delete msg;
00101 
00102   // allocate space for map cells
00103   this->source_data = new char[this->source_map.width * this->source_map.height];
00104   assert(this->source_data);
00105 
00106   if(!(msg = mapdev->Request(this->InQueue,
00107                              PLAYER_MSGTYPE_REQ,
00108                              PLAYER_MAP_REQ_GET_INFO,
00109                              NULL, 0, NULL, false)))
00110   {
00111     PLAYER_ERROR("failed to get map info");
00112     return(-1);
00113   }
00114 
00115   // now, get the map data
00116   player_map_data_t* data_req;
00117   unsigned int i,j;
00118   unsigned int oi,oj;
00119   unsigned int sx,sy;
00120   unsigned int si,sj;
00121 
00122   data_req = (player_map_data_t*)malloc(sizeof(player_map_data_t));
00123   assert(data_req);
00124 
00125   // Tile size, limit to sensible default of about 640x640
00126   sy = sx = 640;
00127   oi=oj=0;
00128   while((oi < this->source_map.width) && (oj < this->source_map.height))
00129   {
00130     si = MIN(sx, this->source_map.width - oi);
00131     sj = MIN(sy, this->source_map.height - oj);
00132 
00133     data_req->col = oi;
00134     data_req->row = oj;
00135     data_req->width = si;
00136     data_req->height = sj;
00137     data_req->data_count = 0;
00138 
00139     if(!(msg = mapdev->Request(this->InQueue,
00140                                PLAYER_MSGTYPE_REQ,
00141                                PLAYER_MAP_REQ_GET_DATA,
00142                                (void*)data_req,0,NULL,false)))
00143     {
00144       PLAYER_ERROR("failed to get map info");
00145       free(data_req);
00146       delete [] source_data;
00147       source_data=NULL;
00148       return(-1);
00149     }
00150 
00151     player_map_data_t* mapcells = (player_map_data_t*)msg->GetPayload();
00152 
00153     // copy the map data
00154     for(j=0;j<sj;j++)
00155     {
00156       for(i=0;i<si;i++)
00157       {
00158         source_data[MAP_IDX(source_map,oi+i,oj+j)] = 
00159                 mapcells->data[j*si + i];
00160 //        mapdata[MAP_IDX(source_map,oi+i,oj+j)].occ_dist = 0;
00161       }
00162     }
00163 
00164     delete msg;
00165 
00166     oi += si;
00167     if(oi >= this->source_map.width)
00168     {
00169       oi = 0;
00170       oj += sj;
00171     }
00172   }
00173 
00174   free(data_req);
00175 
00176   // we're done with the map device now
00177   if(mapdev->Unsubscribe(this->InQueue) != 0)
00178     PLAYER_WARN("unable to unsubscribe from map device");
00179 
00180 
00181   puts("Done.");
00182   PLAYER_MSG3(4,"MapScale read a %d X %d map, at %.3f m/pix\n",
00183          this->source_map.width, this->source_map.height, this->source_map.scale);
00184   return(0);
00185 }
00186 
00187 int
00188 MapTransform::Shutdown()
00189 {
00190   delete [] this->new_data;
00191   new_data = NULL;
00192   return(0);
00193 }
00194 
00196 // Process an incoming message
00197 int MapTransform::ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data)
00198 {
00199   PLAYER_MSG0(9,"ProcessMessage called for MapTransform Driver");
00200 
00201   assert(hdr);
00202   assert(data);
00203  
00204   if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_MAP_REQ_GET_INFO, device_addr))
00205   {
00206     PLAYER_MSG0(9,"ProcessMessage called for MapTransform Driver: PLAYER_MAP_REQ_GET_INFO");
00207         Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_MAP_REQ_GET_INFO, &new_map, sizeof(new_map), NULL);
00208         return 0;
00209   }
00210   
00211   if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_MAP_REQ_GET_DATA, device_addr))
00212   {
00213     PLAYER_MSG0(9,"ProcessMessage called for MapTransform Driver: PLAYER_MAP_REQ_GET_DATA");
00214     assert(new_data);
00215     player_map_data_t & map_data = *reinterpret_cast<player_map_data_t *> (data);
00216     player_map_data_t resp_data;
00217     memcpy(&resp_data, &map_data, sizeof(map_data));
00218 
00219     unsigned int i, j;
00220     unsigned int oi, oj, si, sj;
00221 
00222     // Construct reply
00223     oi = map_data.col;
00224     oj = map_data.row;
00225     si = map_data.width;
00226     sj = map_data.height;
00227     PLAYER_MSG4(9,"Block Requested is: %d,%d + %d,%d",oi,oj,si,sj);
00228     resp_data.data_count = map_data.width * map_data.height;
00229     resp_data.data = new int8_t [resp_data.data_count];
00230 
00231     // Grab the pixels from the map
00232     for(j = 0; j < sj; j++)
00233     {
00234       for(i = 0; i < si; i++)
00235       {
00236         if(MAP_VALID(new_map, i + oi, j + oj))
00237           resp_data.data[i + j * si] = this->new_data[MAP_IDX(new_map, i+oi, j+oj)];
00238         else
00239         {
00240           PLAYER_WARN2("requested cell (%d,%d) is offmap", i+oi, j+oj);
00241           resp_data.data[i + j * si] = 0;
00242         }
00243       }
00244     }
00245     Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_MAP_REQ_GET_DATA, &resp_data);
00246     delete [] resp_data.data;
00247     return 0;
00248   }
00249 
00250   return -1;
00251 }

Last updated 12 September 2005 21:38:45