00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "maptransform.h"
00029
00030
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
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
00068
00069 int
00070 MapTransform::GetMap()
00071 {
00072 Device* mapdev;
00073
00074
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
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
00098 source_map = *(player_map_info_t*)msg->GetPayload();
00099
00100 delete msg;
00101
00102
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
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
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
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
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
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
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
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
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 }