00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <fvutils/fileformat/fvfile.h>
00025
00026 #include <core/exceptions/system.h>
00027
00028 #include <cstring>
00029 #include <cstdio>
00030 #include <cerrno>
00031 #include <netinet/in.h>
00032 #include <sys/time.h>
00033
00034 using namespace fawkes;
00035
00036 namespace firevision {
00037 #if 0
00038 }
00039 #endif
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094 FireVisionDataFile::FireVisionDataFile(unsigned short int magic_token,
00095 unsigned short int version)
00096 {
00097 __header = (fvff_header_t *)calloc(1, sizeof(fvff_header_t));
00098
00099 __magic_token = magic_token;
00100 __version = version;
00101 __comment = strdup("");
00102
00103 _spec_header = NULL;
00104 _spec_header_size = 0;
00105
00106 __owns_blocks = true;
00107
00108 clear();
00109 }
00110
00111
00112
00113 FireVisionDataFile::~FireVisionDataFile()
00114 {
00115 clear();
00116
00117 free(__header);
00118 free(__comment);
00119 if ( _spec_header ) {
00120 free(_spec_header);
00121 }
00122 }
00123
00124
00125
00126
00127
00128 void
00129 FireVisionDataFile::clear()
00130 {
00131 if (__owns_blocks) {
00132 for (__bi = __blocks.begin(); __bi != __blocks.end(); ++__bi) {
00133 delete *__bi;
00134 }
00135 }
00136
00137 __blocks.clear();
00138 memset(__header, 0, sizeof(fvff_header_t));
00139
00140 __header->magic_token = htons(__magic_token);
00141 __header->version = __version;
00142 __header->num_blocks = 0;
00143 #if __BYTE_ORDER == __BIG_ENDIAN
00144 __header->endianess = 1;
00145 #else
00146 __header->endianess = 0;
00147 #endif
00148 free(__comment);
00149 __comment = strdup("");
00150 }
00151
00152
00153
00154
00155
00156 unsigned int
00157 FireVisionDataFile::magic_token()
00158 {
00159 return __header->magic_token;
00160 }
00161
00162
00163
00164
00165
00166 unsigned int
00167 FireVisionDataFile::version()
00168 {
00169 return __header->version;
00170 }
00171
00172
00173
00174
00175
00176 bool
00177 FireVisionDataFile::is_big_endian()
00178 {
00179 return (__header->endianess == 1);
00180 }
00181
00182
00183
00184
00185
00186 bool
00187 FireVisionDataFile::is_little_endian()
00188 {
00189 return (__header->endianess == 0);
00190 }
00191
00192
00193
00194
00195
00196 const char *
00197 FireVisionDataFile::get_comment() const
00198 {
00199 return __comment;
00200 }
00201
00202
00203
00204
00205
00206 void
00207 FireVisionDataFile::set_comment(const char *comment)
00208 {
00209 free(__comment);
00210 __comment = strndup(comment, FVFF_COMMENT_SIZE);
00211 strncpy(__header->comment, comment, FVFF_COMMENT_SIZE);
00212 }
00213
00214
00215
00216
00217
00218
00219
00220 void
00221 FireVisionDataFile::set_owns_blocks(bool owns_blocks)
00222 {
00223 __owns_blocks = owns_blocks;
00224 }
00225
00226
00227
00228
00229
00230 size_t
00231 FireVisionDataFile::num_blocks()
00232 {
00233 return __blocks.size();
00234 }
00235
00236
00237
00238
00239
00240 void
00241 FireVisionDataFile::add_block(FireVisionDataFileBlock *block)
00242 {
00243 __blocks.push_back(block);
00244 }
00245
00246
00247
00248
00249
00250 FireVisionDataFile::BlockList &
00251 FireVisionDataFile::blocks()
00252 {
00253 return __blocks;
00254 }
00255
00256
00257
00258
00259
00260 void
00261 FireVisionDataFile::write(const char *file_name)
00262 {
00263 FILE *f = fopen(file_name, "w");
00264 if ( f == NULL ) {
00265 throw CouldNotOpenFileException(file_name, errno, "Could not open rectlut file "
00266 "for writing");
00267 }
00268
00269 __header->num_blocks = (unsigned int)__blocks.size();
00270 timeval t;
00271 gettimeofday(&t, NULL);
00272 __header->created_sec = t.tv_sec;
00273 __header->created_usec = t.tv_usec;
00274 __header->spec_head_size = _spec_header_size;
00275
00276
00277 if ( fwrite(__header, sizeof(fvff_header_t), 1, f) != 1 ) {
00278 fclose(f);
00279 throw FileWriteException(file_name, errno, "Writing fvff header failed");
00280 }
00281
00282 if ( _spec_header_size > 0 ) {
00283
00284 if ( fwrite(_spec_header, _spec_header_size, 1, f) != 1 ) {
00285 fclose(f);
00286 throw FileWriteException(file_name, errno, "Writing content specific header failed");
00287 }
00288 }
00289
00290 for (__bi = __blocks.begin(); __bi != __blocks.end(); ++__bi) {
00291
00292
00293 if ( fwrite((*__bi)->block_memptr(), (*__bi)->block_size(), 1, f) != 1 ) {
00294 fclose(f);
00295 throw FileWriteException(file_name, errno, "Failed to write info block");
00296 }
00297 }
00298
00299 fclose(f);
00300 }
00301
00302
00303
00304
00305
00306 void
00307 FireVisionDataFile::read(const char *file_name)
00308 {
00309 FILE *f = fopen(file_name, "r");
00310 if ( f == NULL ) {
00311 throw CouldNotOpenFileException(file_name, errno, "Could not open rectlut file "
00312 "for reading");
00313 }
00314
00315 clear();
00316
00317
00318 if ( fread(__header, sizeof(fvff_header_t), 1, f) != 1) {
00319 fclose(f);
00320 throw FileReadException(file_name, errno, "Reading header failed");
00321 }
00322
00323 if ( __header->magic_token != htons(__magic_token) ) {
00324 fclose(f);
00325 throw Exception("Unknown magic in fvff file (read: 0x%04x req: 0x%04x)",
00326 __header->magic_token, __magic_token);
00327 }
00328
00329 if ( __header->version != __version ) {
00330 fclose(f);
00331 throw Exception("Unsupported version of fvff file (read: %u req: %u)",
00332 __header->version, __version);
00333 }
00334
00335 if ( __header->endianess ==
00336 #if __BYTE_ORDER == __BIG_ENDIAN
00337 0
00338 #else
00339 1
00340 #endif
00341 ) {
00342 fclose(f);
00343 throw Exception("FVFile header cannot be translated for endianess by now");
00344 }
00345
00346 free(__comment);
00347 __comment = strndup(__header->comment, FVFF_COMMENT_SIZE);
00348
00349 if ( _spec_header ) {
00350 free(_spec_header);
00351 }
00352 _spec_header = calloc(1, __header->spec_head_size);
00353 if ( ! _spec_header ) {
00354 throw OutOfMemoryException("Cannot allocate memory for content specific header");
00355 }
00356
00357 if ( __header->spec_head_size > 0 ) {
00358
00359 if ( fread(_spec_header, __header->spec_head_size, 1, f) != 1) {
00360 fclose(f);
00361 throw FileReadException(file_name, errno, "Reading content specific header failed");
00362 }
00363 }
00364
00365
00366 for (uint8_t b = 0; b < __header->num_blocks && !feof(f); ++b) {
00367 fvff_block_header_t bh;
00368
00369 if ( fread(&bh, sizeof(bh), 1, f) != 1 ) {
00370 fclose(f);
00371 throw FileReadException(file_name, errno,
00372 "Could not read block info header while there should be one");
00373 }
00374 void *spec_header = NULL;
00375
00376 if ( bh.spec_head_size > 0 ) {
00377
00378 spec_header = malloc(bh.spec_head_size);
00379 if ( ! spec_header ) {
00380 throw OutOfMemoryException("Could not allocate %u bytes for content specific header",
00381 bh.spec_head_size);
00382 }
00383
00384
00385 if ( fread(spec_header, bh.spec_head_size, 1, f) != 1 ) {
00386 fclose(f);
00387 free(spec_header);
00388 throw FileReadException(file_name, errno,
00389 "Could not read content specific block header");
00390 }
00391 }
00392
00393 FireVisionDataFileBlock *block = new FireVisionDataFileBlock(bh.type, bh.size,
00394 spec_header, bh.spec_head_size);
00395
00396 free(spec_header);
00397
00398
00399 if ( bh.size && fread(block->data_ptr(), bh.size, 1, f) != 1 ) {
00400 fclose(f);
00401 delete block;
00402 throw FileReadException(file_name, errno,
00403 "Could not read block data");
00404 }
00405
00406 __blocks.push_back(block);
00407 }
00408
00409 fclose(f);
00410 }
00411
00412
00413
00414
00415
00416
00417 unsigned short int
00418 FireVisionDataFile::read_magic_token(const char *filename)
00419 {
00420 uint16_t magic_token = 0;
00421
00422 FILE *f;
00423 f = fopen(filename, "r");
00424 if (f != NULL) {
00425 if ( fread((char *)&magic_token, sizeof(magic_token), 1, f) != 1 ) {
00426 fclose(f);
00427 throw FileReadException(filename, errno, "Could not read magic token from file");
00428 }
00429 }
00430 fclose(f);
00431
00432 return magic_token;
00433 }
00434
00435
00436
00437
00438
00439
00440
00441 bool
00442 FireVisionDataFile::has_magic_token(const char *filename, unsigned short int magic_token)
00443 {
00444 uint16_t file_magic_token = read_magic_token(filename);
00445 return (htons(magic_token) == file_magic_token);
00446 }
00447
00448 }