Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
retriever_thread.cpp
1 
2 /***************************************************************************
3  * retriever_thread.cpp - FireVision Retriever Thread
4  *
5  * Created: Tue Jun 26 17:39:11 2007
6  * Copyright 2006-2008 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "retriever_thread.h"
24 
25 #include <fvcams/camera.h>
26 #include <fvutils/ipc/shm_image.h>
27 #include <utils/time/tracker.h>
28 #include <fvutils/writers/seq_writer.h>
29 #include <fvutils/writers/jpeg.h>
30 #include <fvmodels/color/lookuptable.h>
31 
32 #include <cstring>
33 #include <cstdlib>
34 
35 using namespace fawkes;
36 using namespace firevision;
37 
38 /** @class FvRetrieverThread "retriever_thread.h"
39  * FireVision retriever thread.
40  * This implements the functionality of the FvRetrieverPlugin.
41  * @author Tim Niemueller
42  */
43 
44 /** Constructor.
45  * @param camera_string camera argument string for camera to open
46  * @param cfg_name ID used to form thread name (FvRetrieverThread_[ID]) and shared
47  * memory image segment ID (retriever_[ID]).
48  * @param cfg_prefix configuration prefix path
49  */
50 FvRetrieverThread::FvRetrieverThread(std::string camera_string,
51  std::string cfg_name, std::string cfg_prefix)
52  : Thread("FvRetrieverThread", Thread::OPMODE_WAITFORWAKEUP),
53  VisionAspect(VisionAspect::CYCLIC)
54 {
55  cfg_name_ = cfg_name;
56  cfg_prefix_ = cfg_prefix;
57  camera_string_ = camera_string;
58  set_name("FvRetrieverThread_%s", cfg_name_.c_str());
59  seq_writer = NULL;
60 }
61 
62 
63 /** Destructor. */
65 {
66 }
67 
68 
69 void
71 {
72  try {
73  logger->log_debug(name(), "Registering for camera '%s'", camera_string_.c_str());
74  cam = vision_master->register_for_camera(camera_string_.c_str(), this);
75  } catch (Exception &e) {
76  e.append("FvRetrieverThread::init() failed");
77  throw;
78  }
79  try {
80  char *imgbufname;
81  if ( asprintf(&imgbufname, "retriever_%s", cfg_name_.c_str()) == -1 ) {
82  throw Exception("Cannot allocate buffer name");
83  }
84  shm = new SharedMemoryImageBuffer(imgbufname, cam->colorspace(),
85  cam->pixel_width(), cam->pixel_height());
86 
87  free(imgbufname);
88  if ( ! shm->is_valid() ) {
89  throw Exception("Shared memory segment not valid");
90  }
91  } catch (Exception &e) {
92  delete cam;
93  cam = NULL;
94  throw;
95  }
96 
97  try {
98  std::string frame_id = config->get_string((cfg_prefix_ + "frame").c_str());
99  shm->set_frame_id(frame_id.c_str());
100  } catch (Exception &e) {/* ignored, not critical */}
101 
102  seq_writer = NULL;
103  try {
104  if ( config->get_bool("/firevision/retriever/save_images") ) {
105  logger->log_info(name(), "Writing images to disk");
106  Writer* writer = new JpegWriter();
107  seq_writer = new SeqWriter(writer);
108  std::string save_path;
109  try {
110  save_path = config->get_string("/firevision/retriever/save_path");
111  } catch (Exception &e) {
112  save_path = ("recorded_images");
113  logger->log_info(name(), "No save path specified. Using './%s'", save_path.c_str());
114  }
115  seq_writer->set_path( save_path.c_str() );
116  seq_writer->set_dimensions( cam->pixel_width(), cam->pixel_height() );
117  seq_writer->set_colorspace( cam->colorspace() );
118  }
119  } catch (Exception &e) {
120  // ignored, not critical
121  }
122 
123  __tt = NULL;
124  try {
125  if ( config->get_bool("/firevision/retriever/use_time_tracker") ) {
126  __tt = new TimeTracker();
127  __ttc_capture = __tt->add_class("Capture");
128  __ttc_memcpy = __tt->add_class("Memcpy");
129  __ttc_dispose = __tt->add_class("Dispose");
130  __loop_count = 0;
131  }
132  } catch (Exception &e) {
133  // ignored, not critical
134  }
135 
136  __cm = new ColorModelLookupTable(1, "retriever-colormap", true);
137  YuvColormap *ycm = __cm->get_colormap();
138  for (unsigned int u = 100; u < 150; ++u) {
139  for (unsigned int v = 100; v < 150; ++v) {
140  ycm->set(128, u, v, C_ORANGE);
141  }
142  }
143 
144  __cam_has_timestamp_support = true;
145  try {
146  fawkes::Time *t = cam->capture_time();
147  if (t->is_zero()) {
148  throw NotImplementedException("");
149  }
150  cap_time_ = NULL;
151  }
152  catch (NotImplementedException &e)
153  {
154  __cam_has_timestamp_support = false;
155  cap_time_ = new Time(clock);
156  }
157 }
158 
159 
160 void
162 {
163  logger->log_debug(name(), "Unregistering from vision master");
165  delete cam;
166  delete shm;
167  delete seq_writer;
168  delete __tt;
169  delete __cm;
170  delete cap_time_;
171 }
172 
173 
174 /** Thread loop. */
175 void
177 {
178  if (__tt) {
179  // use time tracker
180  __tt->ping_start(__ttc_capture);
181  cam->capture();
182  __tt->ping_end(__ttc_capture);
183  __tt->ping_start(__ttc_memcpy);
184  memcpy(shm->buffer(), cam->buffer(), cam->buffer_size()-1);
185  __tt->ping_end(__ttc_memcpy);
186  if (__cam_has_timestamp_support) shm->set_capture_time(cam->capture_time());
187  __tt->ping_start(__ttc_dispose);
188  cam->dispose_buffer();
189  __tt->ping_end(__ttc_dispose);
190  if ( (++__loop_count % 200) == 0 ) {
191  // output results every 200 loops
192  __tt->print_to_stdout();
193  }
194  } else {
195  // no time tracker
196  cam->capture();
197  memcpy(shm->buffer(), cam->buffer(), cam->buffer_size());
198  if (__cam_has_timestamp_support) {
199  shm->set_capture_time(cam->capture_time());
200  } else {
201  cap_time_->stamp();
202  shm->set_capture_time(cap_time_);
203  }
204  cam->dispose_buffer();
205  }
206 
207  if (seq_writer) {
208  seq_writer->write( shm->buffer() );
209  }
210 }