retriever_thread.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <apps/retriever/retriever_thread.h>
00024
00025 #include <cams/camera.h>
00026 #include <fvutils/ipc/shm_image.h>
00027 #include <utils/time/tracker.h>
00028 #include <fvutils/writers/seq_writer.h>
00029 #include <fvutils/writers/jpeg.h>
00030 #include <models/color/lookuptable.h>
00031
00032 #include <cstring>
00033 #include <cstdlib>
00034
00035 using namespace fawkes;
00036 using namespace firevision;
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 FvRetrieverThread::FvRetrieverThread(const char *camera_string, const char *id)
00050 : Thread("FvRetrieverThread", Thread::OPMODE_WAITFORWAKEUP),
00051 VisionAspect(VisionAspect::CYCLIC)
00052 {
00053 __id = strdup(id);
00054 __camera_string = strdup(camera_string);
00055 set_name("FvRetrieverThread_%s", id);
00056 seq_writer = NULL;
00057 }
00058
00059
00060
00061 FvRetrieverThread::~FvRetrieverThread()
00062 {
00063 free(__camera_string);
00064 free(__id);
00065 }
00066
00067
00068 void
00069 FvRetrieverThread::init()
00070 {
00071 try {
00072 logger->log_debug(name(), "Registering for camera '%s'", __camera_string);
00073 cam = vision_master->register_for_camera(__camera_string, this);
00074 } catch (Exception &e) {
00075 e.append("FvRetrieverThread::init() failed");
00076 throw;
00077 }
00078 try {
00079 char *imgbufname;
00080 if ( asprintf(&imgbufname, "retriever_%s", __id) == -1 ) {
00081 throw Exception("Cannot allocate buffer name");
00082 }
00083 shm = new SharedMemoryImageBuffer(imgbufname, cam->colorspace(),
00084 cam->pixel_width(), cam->pixel_height());
00085
00086 free(imgbufname);
00087 if ( ! shm->is_valid() ) {
00088 throw Exception("Shared memory segment not valid");
00089 }
00090 } catch (Exception &e) {
00091 delete cam;
00092 cam = NULL;
00093 throw;
00094 }
00095
00096 seq_writer = NULL;
00097 try {
00098 if ( config->get_bool("/firevision/retriever/save_images") ) {
00099 logger->log_info(name(), "Writing images to disk");
00100 Writer* writer = new JpegWriter();
00101 seq_writer = new SeqWriter(writer);
00102 std::string save_path;
00103 try {
00104 save_path = config->get_string("/firevision/retriever/save_path");
00105 } catch (Exception &e) {
00106 save_path = ("recorded_images");
00107 logger->log_info(name(), "No save path specified. Using './%s'", save_path.c_str());
00108 }
00109 seq_writer->set_path( save_path.c_str() );
00110 seq_writer->set_dimensions( cam->pixel_width(), cam->pixel_height() );
00111 seq_writer->set_colorspace( cam->colorspace() );
00112 }
00113 } catch (Exception &e) {
00114
00115 }
00116
00117 __tt = NULL;
00118 try {
00119 if ( config->get_bool("/firevision/retriever/use_time_tracker") ) {
00120 __tt = new TimeTracker();
00121 __ttc_capture = __tt->add_class("Capture");
00122 __ttc_memcpy = __tt->add_class("Memcpy");
00123 __ttc_dispose = __tt->add_class("Dispose");
00124 __loop_count = 0;
00125 }
00126 } catch (Exception &e) {
00127
00128 }
00129
00130 __cm = new ColorModelLookupTable(1, "retriever-colormap", true);
00131 YuvColormap *ycm = __cm->get_colormap();
00132 for (unsigned int u = 100; u < 150; ++u) {
00133 for (unsigned int v = 100; v < 150; ++v) {
00134 ycm->set(128, u, v, C_ORANGE);
00135 }
00136 }
00137
00138 __cam_has_timestamp_support = true;
00139 try {
00140 cam->capture_time();
00141 }
00142 catch (NotImplementedException &e)
00143 {
00144 __cam_has_timestamp_support = false;
00145 }
00146 }
00147
00148
00149 void
00150 FvRetrieverThread::finalize()
00151 {
00152 logger->log_debug(name(), "Unregistering from vision master");
00153 vision_master->unregister_thread(this);
00154 delete cam;
00155 delete shm;
00156 delete seq_writer;
00157 delete __tt;
00158 delete __cm;
00159 }
00160
00161
00162
00163 void
00164 FvRetrieverThread::loop()
00165 {
00166 if (__tt) {
00167
00168 __tt->ping_start(__ttc_capture);
00169 cam->capture();
00170 __tt->ping_end(__ttc_capture);
00171 __tt->ping_start(__ttc_memcpy);
00172 memcpy(shm->buffer(), cam->buffer(), cam->buffer_size()-1);
00173 __tt->ping_end(__ttc_memcpy);
00174 if (__cam_has_timestamp_support) shm->set_capture_time(cam->capture_time());
00175 __tt->ping_start(__ttc_dispose);
00176 cam->dispose_buffer();
00177 __tt->ping_end(__ttc_dispose);
00178 if ( (++__loop_count % 200) == 0 ) {
00179
00180 __tt->print_to_stdout();
00181 }
00182 } else {
00183
00184 cam->capture();
00185 memcpy(shm->buffer(), cam->buffer(), cam->buffer_size());
00186 if (__cam_has_timestamp_support) shm->set_capture_time(cam->capture_time());
00187 cam->dispose_buffer();
00188 }
00189
00190 if (seq_writer) {
00191 seq_writer->write( shm->buffer() );
00192 }
00193 }