Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * shm_image.cpp - shared memory image buffer 00004 * 00005 * Created: Thu Jan 12 14:10:43 2006 00006 * Copyright 2005-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #include <core/exception.h> 00025 #include <fvutils/ipc/shm_image.h> 00026 #include <fvutils/ipc/shm_exceptions.h> 00027 #include <utils/system/console_colors.h> 00028 #include <utils/ipc/shm_exceptions.h> 00029 00030 #include <iostream> 00031 #include <cstring> 00032 #include <cstdlib> 00033 #include <cstdio> 00034 00035 using namespace std; 00036 using namespace fawkes; 00037 00038 namespace firevision { 00039 #if 0 /* just to make Emacs auto-indent happy */ 00040 } 00041 #endif 00042 00043 /** @class SharedMemoryImageBuffer <fvutils/ipc/shm_image.h> 00044 * Shared memory image buffer. 00045 * Write images to or retrieve images from a shared memory segment. 00046 * @author Tim Niemueller 00047 */ 00048 00049 /** Write Constructor. 00050 * Create a new shared memory segment. Will open a shared memory segment that 00051 * exactly fits the given information. Will throw an error if image with id 00052 * image_id exists. 00053 * I will create a new segment if no matching segment was found. 00054 * The segment is accessed in read-write mode. 00055 * 00056 * Use this constructor to open a shared memory image buffer for writing. 00057 * @param image_id image ID to open 00058 * @param cspace colorspace 00059 * @param width image width 00060 * @param height image height 00061 */ 00062 SharedMemoryImageBuffer::SharedMemoryImageBuffer(const char *image_id, 00063 colorspace_t cspace, 00064 unsigned int width, 00065 unsigned int height) 00066 : SharedMemory(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, 00067 /* read-only */ false, 00068 /* create */ true, 00069 /* destroy on delete */ true) 00070 { 00071 constructor(image_id, cspace, width, height, false); 00072 add_semaphore(); 00073 } 00074 00075 00076 /** Read Constructor. 00077 * This constructor is used to search for an existing shared memory segment. 00078 * It will throw an error if it cannot find a segment with the specified data. 00079 * The segment is opened read-only by default, but this can be overridden with 00080 * the is_read_only argument if needed. 00081 * 00082 * Use this constructor to open an image for reading. 00083 * @param image_id Image ID to open 00084 * @param is_read_only true to open image read-only 00085 */ 00086 SharedMemoryImageBuffer::SharedMemoryImageBuffer(const char *image_id, bool is_read_only) 00087 : SharedMemory(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, is_read_only, /* create */ false, /* destroy */ false) 00088 { 00089 constructor(image_id, CS_UNKNOWN, 0, 0, is_read_only); 00090 } 00091 00092 00093 void 00094 SharedMemoryImageBuffer::constructor(const char *image_id, colorspace_t cspace, 00095 unsigned int width, unsigned int height, 00096 bool is_read_only) 00097 { 00098 _image_id = strdup(image_id); 00099 _is_read_only = is_read_only; 00100 00101 _colorspace = cspace; 00102 _width = width; 00103 _height = height; 00104 00105 priv_header = new SharedMemoryImageBufferHeader(_image_id, _colorspace, width, height); 00106 _header = priv_header; 00107 try { 00108 attach(); 00109 raw_header = priv_header->raw_header(); 00110 } catch (Exception &e) { 00111 e.append("SharedMemoryImageBuffer: could not attach to '%s'\n", image_id); 00112 ::free(_image_id); 00113 _image_id = NULL; 00114 delete priv_header; 00115 throw; 00116 } 00117 } 00118 00119 00120 /** Destructor. */ 00121 SharedMemoryImageBuffer::~SharedMemoryImageBuffer() 00122 { 00123 ::free(_image_id); 00124 delete priv_header; 00125 } 00126 00127 00128 /** Set image number. 00129 * This will close the currently opened image and will try to open the new 00130 * image. This operation should be avoided. 00131 * @param image_id new image ID 00132 * @return true on success 00133 */ 00134 bool 00135 SharedMemoryImageBuffer::set_image_id(const char *image_id) 00136 { 00137 free(); 00138 ::free(_image_id); 00139 _image_id = strdup(image_id); 00140 priv_header->set_image_id(_image_id); 00141 attach(); 00142 raw_header = priv_header->raw_header(); 00143 return (_memptr != NULL); 00144 } 00145 00146 00147 /** Get Image ID. 00148 * @return image id 00149 */ 00150 const char * 00151 SharedMemoryImageBuffer::image_id() const 00152 { 00153 return _image_id; 00154 } 00155 00156 00157 /** Get the time when the image was captured. 00158 * @param sec upon return contains the seconds part of the time 00159 * @param usec upon return contains the micro seconds part of the time 00160 */ 00161 void 00162 SharedMemoryImageBuffer::capture_time(long int *sec, long int *usec) const 00163 { 00164 *sec = raw_header->capture_time_sec; 00165 *usec = raw_header->capture_time_usec; 00166 } 00167 00168 /** Get the time when the image was captured. 00169 * @return capture time 00170 */ 00171 Time 00172 SharedMemoryImageBuffer::capture_time() const 00173 { 00174 return Time(raw_header->capture_time_sec, raw_header->capture_time_usec); 00175 } 00176 00177 00178 /** Set the capture time. 00179 * @param time capture time 00180 */ 00181 void 00182 SharedMemoryImageBuffer::set_capture_time(Time *time) 00183 { 00184 if (_is_read_only) { 00185 throw Exception("Buffer is read-only. Not setting capture time."); 00186 } 00187 00188 const timeval *t = time->get_timeval(); 00189 raw_header->capture_time_sec = t->tv_sec; 00190 raw_header->capture_time_usec = t->tv_usec; 00191 } 00192 00193 /** Set the capture time. 00194 * @param sec seconds part of capture time 00195 * @param usec microseconds part of capture time 00196 */ 00197 void 00198 SharedMemoryImageBuffer::set_capture_time(long int sec, long int usec) 00199 { 00200 if (_is_read_only) { 00201 throw Exception("Buffer is read-only. Not setting capture time."); 00202 } 00203 00204 raw_header->capture_time_sec = sec; 00205 raw_header->capture_time_usec = usec; 00206 } 00207 00208 /** Get image buffer. 00209 * @return image buffer. 00210 */ 00211 unsigned char * 00212 SharedMemoryImageBuffer::buffer() const 00213 { 00214 return (unsigned char *)_memptr; 00215 } 00216 00217 00218 /** Get color space. 00219 * @return colorspace 00220 */ 00221 colorspace_t 00222 SharedMemoryImageBuffer::colorspace() const 00223 { 00224 return (colorspace_t)raw_header->colorspace; 00225 } 00226 00227 00228 /** Get image width. 00229 * @return width 00230 */ 00231 unsigned int 00232 SharedMemoryImageBuffer::width() const 00233 { 00234 return raw_header->width; 00235 } 00236 00237 00238 /** Get image height. 00239 * @return image height 00240 */ 00241 unsigned int 00242 SharedMemoryImageBuffer::height() const 00243 { 00244 return raw_header->height; 00245 } 00246 00247 00248 /** Get ROI X. 00249 * @return ROI X 00250 */ 00251 unsigned int 00252 SharedMemoryImageBuffer::roi_x() const 00253 { 00254 return raw_header->roi_x; 00255 } 00256 00257 00258 /** Get ROI Y. 00259 * @return ROI Y 00260 */ 00261 unsigned int 00262 SharedMemoryImageBuffer::roi_y() const 00263 { 00264 return raw_header->roi_y; 00265 } 00266 00267 00268 /** Get ROI width. 00269 * @return ROI width 00270 */ 00271 unsigned int 00272 SharedMemoryImageBuffer::roi_width() const 00273 { 00274 return raw_header->roi_width; 00275 } 00276 00277 00278 /** Get ROI height. 00279 * @return ROI height 00280 */ 00281 unsigned int 00282 SharedMemoryImageBuffer::roi_height() const 00283 { 00284 return raw_header->roi_height; 00285 } 00286 00287 00288 /** Get circle X. 00289 * @return circle X 00290 */ 00291 int 00292 SharedMemoryImageBuffer::circle_x() const 00293 { 00294 return raw_header->circle_x; 00295 } 00296 00297 00298 /** Get circle Y. 00299 * @return circle Y 00300 */ 00301 int 00302 SharedMemoryImageBuffer::circle_y() const 00303 { 00304 return raw_header->circle_y; 00305 } 00306 00307 00308 /** Get circle radius. 00309 * @return circle radius 00310 */ 00311 unsigned int 00312 SharedMemoryImageBuffer::circle_radius() const 00313 { 00314 return raw_header->circle_radius; 00315 } 00316 00317 00318 /** Set ROI X. 00319 * @param roi_x new ROI X 00320 */ 00321 void 00322 SharedMemoryImageBuffer::set_roi_x(unsigned int roi_x) 00323 { 00324 if (_is_read_only) { 00325 throw Exception("Buffer is read-only. Not setting ROI X."); 00326 } 00327 raw_header->roi_x = roi_x; 00328 } 00329 00330 00331 /** Set ROI Y. 00332 * @param roi_y new ROI Y 00333 */ 00334 void 00335 SharedMemoryImageBuffer::set_roi_y(unsigned int roi_y) 00336 { 00337 if (_is_read_only) { 00338 throw Exception("Buffer is read-only. Not setting ROI Y."); 00339 } 00340 raw_header->roi_y = roi_y; 00341 } 00342 00343 00344 /** Set ROI width. 00345 * @param roi_w new ROI width 00346 */ 00347 void 00348 SharedMemoryImageBuffer::set_roi_width(unsigned int roi_w) 00349 { 00350 if (_is_read_only) { 00351 throw Exception("Buffer is read-only. Not setting ROI width."); 00352 } 00353 raw_header->roi_width = roi_w; 00354 } 00355 00356 00357 /** Set ROI height. 00358 * @param roi_h new ROI height 00359 */ 00360 void 00361 SharedMemoryImageBuffer::set_roi_height(unsigned int roi_h) 00362 { 00363 if (_is_read_only) { 00364 throw Exception("Buffer is read-only. Not setting ROI height."); 00365 } 00366 raw_header->roi_height = roi_h; 00367 } 00368 00369 00370 /** Set ROI data. 00371 * @param roi_x new ROI X 00372 * @param roi_y new ROI Y 00373 * @param roi_w new ROI width 00374 * @param roi_h new ROI height 00375 */ 00376 void 00377 SharedMemoryImageBuffer::set_roi(unsigned int roi_x, unsigned int roi_y, 00378 unsigned int roi_w, unsigned int roi_h) 00379 { 00380 if (_is_read_only) { 00381 throw Exception("Buffer is read-only. Not setting ROI X/Y."); 00382 } 00383 raw_header->roi_x = roi_x; 00384 raw_header->roi_y = roi_y; 00385 raw_header->roi_width = roi_w; 00386 raw_header->roi_height = roi_h; 00387 } 00388 00389 00390 /** Set circle X. 00391 * @param circle_x new circle X 00392 */ 00393 void 00394 SharedMemoryImageBuffer::set_circle_x(int circle_x) 00395 { 00396 if (_is_read_only) { 00397 throw Exception("Buffer is read-only. Not setting circle X."); 00398 } 00399 raw_header->circle_x = circle_x; 00400 } 00401 00402 00403 /** Set circle Y. 00404 * @param circle_y new circle Y 00405 */ 00406 void 00407 SharedMemoryImageBuffer::set_circle_y(int circle_y) 00408 { 00409 if (_is_read_only) { 00410 throw Exception("Buffer is read-only. Not setting circle Y."); 00411 } 00412 raw_header->circle_y = circle_y; 00413 } 00414 00415 00416 /** Set circle radius. 00417 * @param circle_radius new circle radius 00418 */ 00419 void 00420 SharedMemoryImageBuffer::set_circle_radius(unsigned int circle_radius) 00421 { 00422 if (_is_read_only) { 00423 throw Exception("Buffer is read-only. Not setting circle radius."); 00424 } 00425 raw_header->circle_radius = circle_radius; 00426 } 00427 00428 00429 /** Set circle data. 00430 * @param x circle X 00431 * @param y circle Y 00432 * @param r circle radius 00433 */ 00434 void 00435 SharedMemoryImageBuffer::set_circle(int x, int y, unsigned int r) 00436 { 00437 if (_is_read_only) { 00438 throw Exception("Buffer is read-only. Not setting circle X/Y/radius."); 00439 } 00440 raw_header->circle_x = x; 00441 raw_header->circle_y = y; 00442 raw_header->circle_radius = r; 00443 } 00444 00445 00446 /** Set circle found. 00447 * @param found true if circle found 00448 */ 00449 void 00450 SharedMemoryImageBuffer::set_circle_found(bool found) 00451 { 00452 raw_header->flag_circle_found = (found ? 1 : 0); 00453 } 00454 00455 00456 /** Check if circle was found . 00457 * @return true if circle was found, false otherwise 00458 */ 00459 bool 00460 SharedMemoryImageBuffer::circle_found() const 00461 { 00462 return (raw_header->flag_circle_found == 1); 00463 } 00464 00465 00466 /** List all shared memory segments that contain a FireVision image. */ 00467 void 00468 SharedMemoryImageBuffer::list() 00469 { 00470 SharedMemoryImageBufferLister *lister = new SharedMemoryImageBufferLister(); 00471 SharedMemoryImageBufferHeader *h = new SharedMemoryImageBufferHeader(); 00472 00473 SharedMemory::list(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, h, lister); 00474 00475 delete lister; 00476 delete h; 00477 } 00478 00479 00480 /** Erase all shared memory segments that contain FireVision images. 00481 * @param use_lister if true a lister is used to print the shared memory segments 00482 * to stdout while cleaning up. 00483 */ 00484 void 00485 SharedMemoryImageBuffer::cleanup(bool use_lister) 00486 { 00487 SharedMemoryImageBufferLister *lister = NULL; 00488 SharedMemoryImageBufferHeader *h = new SharedMemoryImageBufferHeader(); 00489 00490 if (use_lister) { 00491 lister = new SharedMemoryImageBufferLister(); 00492 } 00493 00494 SharedMemory::erase_orphaned(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, h, lister); 00495 00496 delete lister; 00497 delete h; 00498 } 00499 00500 00501 /** Check image availability. 00502 * @param image_id image ID to check 00503 * @return true if shared memory segment with requested image exists 00504 */ 00505 bool 00506 SharedMemoryImageBuffer::exists(const char *image_id) 00507 { 00508 SharedMemoryImageBufferHeader *h = new SharedMemoryImageBufferHeader(image_id, CS_UNKNOWN, 0, 0); 00509 00510 bool ex = SharedMemory::exists(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, h); 00511 00512 delete h; 00513 return ex; 00514 } 00515 00516 00517 /** Erase a specific shared memory segment that contains an image. 00518 * @param image_id ID of image to wipe 00519 */ 00520 void 00521 SharedMemoryImageBuffer::wipe(const char *image_id) 00522 { 00523 SharedMemoryImageBufferHeader *h = new SharedMemoryImageBufferHeader(image_id, CS_UNKNOWN, 0, 0); 00524 00525 SharedMemory::erase(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, h, NULL); 00526 00527 delete h; 00528 } 00529 00530 00531 /** @class SharedMemoryImageBufferHeader <fvutils/ipc/shm_image.h> 00532 * Shared memory image buffer header. 00533 */ 00534 00535 /** Constructor. */ 00536 SharedMemoryImageBufferHeader::SharedMemoryImageBufferHeader() 00537 { 00538 _colorspace = CS_UNKNOWN; 00539 _image_id = NULL; 00540 _width = 0; 00541 _height = 0; 00542 _header = NULL; 00543 _orig_image_id = NULL; 00544 } 00545 00546 00547 /** Constructor. 00548 * @param image_id image id 00549 * @param colorspace colorspace 00550 * @param width width 00551 * @param height height 00552 */ 00553 SharedMemoryImageBufferHeader::SharedMemoryImageBufferHeader(const char *image_id, 00554 colorspace_t colorspace, 00555 unsigned int width, 00556 unsigned int height) 00557 { 00558 _image_id = strdup(image_id); 00559 _colorspace = colorspace; 00560 _width = width; 00561 _height = height; 00562 _header = NULL; 00563 00564 _orig_image_id = NULL; 00565 _orig_width = 0; 00566 _orig_height = 0; 00567 _orig_colorspace = CS_UNKNOWN; 00568 } 00569 00570 00571 /** Copy constructor. 00572 * @param h shared memory image header to copy 00573 */ 00574 SharedMemoryImageBufferHeader::SharedMemoryImageBufferHeader(const SharedMemoryImageBufferHeader *h) 00575 { 00576 if ( h->_image_id != NULL ) { 00577 _image_id = strdup(h->_image_id); 00578 } else { 00579 _image_id = NULL; 00580 } 00581 _colorspace = h->_colorspace; 00582 _width = h->_width; 00583 _height = h->_height; 00584 _header = h->_header; 00585 00586 _orig_image_id = NULL; 00587 _orig_width = 0; 00588 _orig_height = 0; 00589 _orig_colorspace = CS_UNKNOWN; 00590 } 00591 00592 00593 /** Destructor. */ 00594 SharedMemoryImageBufferHeader::~SharedMemoryImageBufferHeader() 00595 { 00596 if ( _image_id != NULL) free(_image_id); 00597 if ( _orig_image_id != NULL) free(_orig_image_id); 00598 } 00599 00600 00601 size_t 00602 SharedMemoryImageBufferHeader::size() 00603 { 00604 return sizeof(SharedMemoryImageBuffer_header_t); 00605 } 00606 00607 00608 SharedMemoryHeader * 00609 SharedMemoryImageBufferHeader::clone() const 00610 { 00611 return new SharedMemoryImageBufferHeader(this); 00612 } 00613 00614 00615 size_t 00616 SharedMemoryImageBufferHeader::data_size() 00617 { 00618 if (_header == NULL) { 00619 return colorspace_buffer_size(_colorspace, _width, _height); 00620 } else { 00621 return colorspace_buffer_size((colorspace_t)_header->colorspace, _header->width, _header->height); 00622 } 00623 } 00624 00625 00626 bool 00627 SharedMemoryImageBufferHeader::matches(void *memptr) 00628 { 00629 SharedMemoryImageBuffer_header_t *h = (SharedMemoryImageBuffer_header_t *)memptr; 00630 00631 if (_image_id == NULL) { 00632 return true; 00633 00634 } else if (strncmp(h->image_id, _image_id, IMAGE_ID_MAX_LENGTH) == 0) { 00635 00636 if ( (_colorspace == CS_UNKNOWN) || 00637 (((colorspace_t)h->colorspace == _colorspace) && 00638 (h->width == _width) && 00639 (h->height == _height) 00640 ) 00641 ) { 00642 return true; 00643 } else { 00644 throw InconsistentImageException("Inconsistent image found in memory (meta)"); 00645 } 00646 } else { 00647 return false; 00648 } 00649 } 00650 00651 /** Check for equality of headers. 00652 * First checks if passed SharedMemoryHeader is an instance of 00653 * SharedMemoryImageBufferHeader. If not returns false, otherwise it compares 00654 * image ID, colorspace, width, and height. If all match returns true, false 00655 * if any of them differs. 00656 * @param s shared memory header to compare to 00657 * @return true if the two instances identify the very same shared memory segments, 00658 * false otherwise 00659 */ 00660 bool 00661 SharedMemoryImageBufferHeader::operator==(const SharedMemoryHeader &s) const 00662 { 00663 const SharedMemoryImageBufferHeader *h = dynamic_cast<const SharedMemoryImageBufferHeader *>(&s); 00664 if ( ! h ) { 00665 return false; 00666 } else { 00667 return ( (strncmp(_image_id, h->_image_id, IMAGE_ID_MAX_LENGTH) == 0) && 00668 (_colorspace == h->_colorspace) && 00669 (_width == h->_width) && 00670 (_height == h->_height) ); 00671 } 00672 } 00673 00674 /** Print some info. */ 00675 void 00676 SharedMemoryImageBufferHeader::print_info() 00677 { 00678 if (_image_id == NULL) { 00679 cout << "No image set" << endl; 00680 return; 00681 } 00682 cout << "SharedMemory Image Info: " << endl; 00683 printf(" address: %p\n", _header); 00684 cout << " image id: " << _image_id << endl 00685 << " colorspace: " << _colorspace << endl 00686 << " dimensions: " << _width << "x" << _height << endl; 00687 /* 00688 << " ROI: at (" << header->roi_x << "," << header->roi_y 00689 << ") dim " << header->roi_width << "x" << header->roi_height << endl 00690 << " circle: " << (header->flag_circle_found ? "" : "not ") 00691 << "found at (" << header->circle_x << "," << header->circle_y 00692 << ") radius " << header->circle_radius << endl 00693 << " img ready: " << (header->flag_image_ready ? "yes" : "no") << endl; 00694 */ 00695 } 00696 00697 00698 /** Create if colorspace, width and height have been supplied. 00699 * @return true if colorspace has been set, width and height are greater than zero. 00700 */ 00701 bool 00702 SharedMemoryImageBufferHeader::create() 00703 { 00704 return ( (_colorspace != CS_UNKNOWN) && 00705 (_width > 0) && 00706 (_height > 0) ); 00707 } 00708 00709 00710 void 00711 SharedMemoryImageBufferHeader::initialize(void *memptr) 00712 { 00713 SharedMemoryImageBuffer_header_t *header = (SharedMemoryImageBuffer_header_t *)memptr; 00714 memset(memptr, 0, sizeof(SharedMemoryImageBuffer_header_t)); 00715 00716 strncpy(header->image_id, _image_id, IMAGE_ID_MAX_LENGTH); 00717 header->colorspace = _colorspace; 00718 header->width = _width; 00719 header->height = _height; 00720 00721 _header = header; 00722 } 00723 00724 00725 void 00726 SharedMemoryImageBufferHeader::set(void *memptr) 00727 { 00728 SharedMemoryImageBuffer_header_t *header = (SharedMemoryImageBuffer_header_t *)memptr; 00729 if ( NULL != _orig_image_id ) free(_orig_image_id); 00730 if ( NULL != _image_id ) { 00731 _orig_image_id = strdup(_image_id); 00732 free(_image_id); 00733 } else { 00734 _orig_image_id = NULL; 00735 } 00736 _orig_width = _width; 00737 _orig_height = _height; 00738 _orig_colorspace = _colorspace; 00739 _header = header; 00740 00741 _image_id = strndup(header->image_id, IMAGE_ID_MAX_LENGTH); 00742 _width = header->width; 00743 _height = header->height; 00744 _colorspace = (colorspace_t)header->colorspace; 00745 } 00746 00747 00748 void 00749 SharedMemoryImageBufferHeader::reset() 00750 { 00751 if ( NULL != _image_id ) { 00752 free(_image_id); 00753 _image_id = NULL; 00754 } 00755 if ( _orig_image_id != NULL ) { 00756 _image_id = strdup(_orig_image_id); 00757 } 00758 _width =_orig_width; 00759 _height =_orig_height; 00760 _colorspace =_orig_colorspace; 00761 _header = NULL; 00762 } 00763 00764 00765 /** Get colorspace. 00766 * @return colorspace 00767 */ 00768 colorspace_t 00769 SharedMemoryImageBufferHeader::colorspace() const 00770 { 00771 if ( _header) return (colorspace_t)_header->colorspace; 00772 else return _colorspace; 00773 } 00774 00775 00776 /** Get width. 00777 * @return image width 00778 */ 00779 unsigned int 00780 SharedMemoryImageBufferHeader::width() const 00781 { 00782 if ( _header) return _header->width; 00783 else return _width; 00784 } 00785 00786 00787 /** Get height. 00788 * @return image height 00789 */ 00790 unsigned int 00791 SharedMemoryImageBufferHeader::height() const 00792 { 00793 if ( _header) return _header->height; 00794 else return _height; 00795 } 00796 00797 00798 /** Get image number 00799 * @return image number 00800 */ 00801 const char * 00802 SharedMemoryImageBufferHeader::image_id() const 00803 { 00804 return _image_id; 00805 } 00806 00807 00808 /** Set image id 00809 * @param image_id image ID 00810 */ 00811 void 00812 SharedMemoryImageBufferHeader::set_image_id(const char *image_id) 00813 { 00814 if ( _image_id != NULL) ::free(_image_id); 00815 _image_id = strdup(image_id); 00816 } 00817 00818 00819 /** Get raw header. 00820 * @return raw header. 00821 */ 00822 SharedMemoryImageBuffer_header_t * 00823 SharedMemoryImageBufferHeader::raw_header() 00824 { 00825 return _header; 00826 } 00827 00828 00829 /** @class SharedMemoryImageBufferLister <fvutils/ipc/shm_image.h> 00830 * Shared memory image buffer lister. 00831 */ 00832 00833 /** Constructor. */ 00834 SharedMemoryImageBufferLister::SharedMemoryImageBufferLister() 00835 { 00836 } 00837 00838 00839 /** Destructor. */ 00840 SharedMemoryImageBufferLister::~SharedMemoryImageBufferLister() 00841 { 00842 } 00843 00844 00845 void 00846 SharedMemoryImageBufferLister::print_header() 00847 { 00848 cout << endl << cgreen << "FireVision Shared Memory Segments - Images" << cnormal << endl 00849 << "========================================================================================" << endl 00850 << cdarkgray; 00851 printf ("%-20s %-10s %-10s %-9s %-16s %-5s %-5s %s\n", 00852 "Image ID", "ShmID", "Semaphore", "Bytes", "Color Space", "Width", "Height", 00853 "State"); 00854 cout << cnormal 00855 << "----------------------------------------------------------------------------------------" << endl; 00856 } 00857 00858 00859 void 00860 SharedMemoryImageBufferLister::print_footer() 00861 { 00862 } 00863 00864 00865 void 00866 SharedMemoryImageBufferLister::print_no_segments() 00867 { 00868 cout << "No FireVision shared memory segments found" << endl; 00869 } 00870 00871 00872 void 00873 SharedMemoryImageBufferLister::print_no_orphaned_segments() 00874 { 00875 cout << "No orphaned FireVision shared memory segments found" << endl; 00876 } 00877 00878 00879 void 00880 SharedMemoryImageBufferLister::print_info(const SharedMemoryHeader *header, 00881 int shm_id, int semaphore, 00882 unsigned int mem_size, 00883 const void *memptr) 00884 { 00885 00886 SharedMemoryImageBufferHeader *h = (SharedMemoryImageBufferHeader *)header; 00887 00888 const char *colorspace = colorspace_to_string(h->colorspace()); 00889 00890 printf("%-20s %-10d %-10d %-9u %-16s %-5u %-5u %s%s\n", 00891 h->image_id(), shm_id, semaphore, mem_size, colorspace, 00892 h->width(), h->height(), 00893 (SharedMemory::is_swapable(shm_id) ? "S" : ""), 00894 (SharedMemory::is_destroyed(shm_id) ? "D" : "") 00895 ); 00896 } 00897 00898 } // end namespace firevision