Fawkes API  Fawkes Development Version
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 <fvmodels/color/lookuptable.h>
27 #include <fvutils/ipc/shm_image.h>
28 #include <fvutils/writers/jpeg.h>
29 #include <fvutils/writers/seq_writer.h>
30 #include <utils/time/tracker.h>
31 
32 #include <cstdlib>
33 #include <cstring>
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(const std::string &camera_string,
51  const std::string &cfg_name,
52  const std::string &cfg_prefix)
53 : Thread("FvRetrieverThread", Thread::OPMODE_WAITFORWAKEUP),
54  VisionAspect(VisionAspect::CYCLIC),
55  cfg_name_(cfg_name),
56  cfg_prefix_(cfg_prefix),
57  camera_string_(camera_string),
58  cap_time_(NULL)
59 {
60  set_name("FvRetrieverThread_%s", cfg_name_.c_str());
61  seq_writer = NULL;
62 }
63 
64 /** Destructor. */
66 {
67 }
68 
69 void
71 {
72  colorspace_t cspace = YUV422_PLANAR;
73  std::string cspace_str = colorspace_to_string(cspace);
74  try {
75  cspace_str = config->get_string((cfg_prefix_ + "colorspace").c_str());
76  cspace = colorspace_by_name(cspace_str.c_str());
77  } catch (Exception &e) {
78  } // ignored, use default
79  if (cspace == CS_UNKNOWN) {
80  throw Exception("Unknown colorspace '%s' configured", cspace_str.c_str());
81  }
82 
83  try {
85  "Registering for camera '%s' (colorspace %s)",
86  camera_string_.c_str(),
87  colorspace_to_string(cspace));
88  cam = vision_master->register_for_camera(camera_string_.c_str(), this, cspace);
89  } catch (Exception &e) {
90  e.append("FvRetrieverThread::init() failed");
91  throw;
92  }
93  try {
94  char *imgbufname;
95  if (asprintf(&imgbufname, "retriever_%s", cfg_name_.c_str()) == -1) {
96  throw Exception("Cannot allocate buffer name");
97  }
98  shm = new SharedMemoryImageBuffer(imgbufname,
99  cam->colorspace(),
100  cam->pixel_width(),
101  cam->pixel_height());
102 
103  free(imgbufname);
104  if (!shm->is_valid()) {
105  throw Exception("Shared memory segment not valid");
106  }
107  } catch (Exception &e) {
108  delete cam;
109  cam = NULL;
110  throw;
111  }
112 
113  try {
114  std::string frame_id = config->get_string((cfg_prefix_ + "frame").c_str());
115  shm->set_frame_id(frame_id.c_str());
116  } catch (Exception &e) { /* ignored, not critical */
117  }
118 
119  seq_writer = NULL;
120  try {
121  if (config->get_bool("/firevision/retriever/save_images")) {
122  logger->log_info(name(), "Writing images to disk");
123  Writer *writer = new JpegWriter();
124  seq_writer = new SeqWriter(writer);
125  std::string save_path;
126  try {
127  save_path = config->get_string("/firevision/retriever/save_path");
128  } catch (Exception &e) {
129  save_path = ("recorded_images");
130  logger->log_info(name(), "No save path specified. Using './%s'", save_path.c_str());
131  }
132  seq_writer->set_path(save_path.c_str());
133  seq_writer->set_dimensions(cam->pixel_width(), cam->pixel_height());
134  seq_writer->set_colorspace(cam->colorspace());
135  }
136  } catch (Exception &e) {
137  // ignored, not critical
138  }
139 
140  tt_ = NULL;
141  try {
142  if (config->get_bool("/firevision/retriever/use_time_tracker")) {
143  tt_ = new TimeTracker();
144  ttc_capture_ = tt_->add_class("Capture");
145  ttc_memcpy_ = tt_->add_class("Memcpy");
146  ttc_dispose_ = tt_->add_class("Dispose");
147  loop_count_ = 0;
148  }
149  } catch (Exception &e) {
150  // ignored, not critical
151  }
152 
153  cm_ = new ColorModelLookupTable(1, "retriever-colormap", true);
154  YuvColormap *ycm = cm_->get_colormap();
155  for (unsigned int u = 100; u < 150; ++u) {
156  for (unsigned int v = 100; v < 150; ++v) {
157  ycm->set(128, u, v, C_ORANGE);
158  }
159  }
160 
161  cam_has_timestamp_support_ = true;
162  try {
163  fawkes::Time *t = cam->capture_time();
164  if (t->is_zero()) {
165  throw NotImplementedException("");
166  }
167  cap_time_ = NULL;
168  } catch (NotImplementedException &e) {
169  cam_has_timestamp_support_ = false;
170  cap_time_ = new Time(clock);
171  }
172 }
173 
174 void
176 {
177  logger->log_debug(name(), "Unregistering from vision master");
179  delete cam;
180  delete shm;
181  delete seq_writer;
182  delete tt_;
183  delete cm_;
184  delete cap_time_;
185 }
186 
187 /** Thread loop. */
188 void
190 {
191  if (tt_) {
192  // use time tracker
193  tt_->ping_start(ttc_capture_);
194  cam->capture();
195  tt_->ping_end(ttc_capture_);
196  tt_->ping_start(ttc_memcpy_);
197  shm->lock_for_write();
198  memcpy(shm->buffer(), cam->buffer(), cam->buffer_size() - 1);
199  shm->unlock();
200  tt_->ping_end(ttc_memcpy_);
201  if (cam_has_timestamp_support_)
202  shm->set_capture_time(cam->capture_time());
203  tt_->ping_start(ttc_dispose_);
204  cam->dispose_buffer();
205  tt_->ping_end(ttc_dispose_);
206  if ((++loop_count_ % 200) == 0) {
207  // output results every 200 loops
208  tt_->print_to_stdout();
209  }
210  } else {
211  // no time tracker
212  cam->capture();
213  shm->lock_for_write();
214  memcpy(shm->buffer(), cam->buffer(), cam->buffer_size());
215  shm->unlock();
216  if (cam_has_timestamp_support_) {
217  shm->set_capture_time(cam->capture_time());
218  } else {
219  cap_time_->stamp();
220  shm->set_capture_time(cap_time_);
221  }
222  cam->dispose_buffer();
223  }
224 
225  if (seq_writer) {
226  seq_writer->write(shm->buffer());
227  }
228 }
virtual ~FvRetrieverThread()
Destructor.
virtual void init()
Initialize the thread.
FvRetrieverThread(const std::string &camera_string, const std::string &cfg_name, const std::string &cfg_prefix)
Constructor.
virtual void loop()
Thread loop.
virtual void finalize()
Finalize the thread.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Called method has not been implemented.
Definition: software.h:105
bool is_valid() const
Check validity of shared memory segment.
Definition: shm.cpp:811
void lock_for_write()
Lock shared memory segment for writing.
Definition: shm.cpp:959
void unlock()
Unlock memory.
Definition: shm.cpp:1025
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Time tracking utility.
Definition: tracker.h:37
void print_to_stdout()
Print results to stdout.
Definition: tracker.cpp:307
unsigned int add_class(std::string name)
Add a new class.
Definition: tracker.cpp:149
void ping_end(unsigned int cls)
End of given class task.
Definition: tracker.cpp:243
void ping_start(unsigned int cls)
Start of given class task.
Definition: tracker.cpp:218
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
bool is_zero() const
Check if time is zero.
Definition: time.h:143
Thread aspect to use in FireVision apps.
Definition: vision.h:33
firevision::VisionMaster * vision_master
Vision master.
Definition: vision.h:51
virtual fawkes::Time * capture_time()
Get the Time of the last successfully captured image.
Definition: camera.cpp:137
virtual unsigned int pixel_height()=0
Height of image in pixels.
virtual unsigned int buffer_size()=0
Size of buffer.
virtual void dispose_buffer()=0
Dispose current buffer.
virtual unsigned int pixel_width()=0
Width of image in pixels.
virtual void capture()=0
Capture an image.
virtual colorspace_t colorspace()=0
Colorspace of returned image.
virtual unsigned char * buffer()=0
Get access to current image buffer.
Color model based on a lookup table.
Definition: lookuptable.h:35
YuvColormap * get_colormap() const
Get colormap.
JPEG file writer.
Definition: jpeg.h:34
Writes a sequence of images to disk.
Definition: seq_writer.h:34
void set_dimensions(unsigned int width, unsigned int height)
Set the image dimensions.
Definition: seq_writer.cpp:97
void set_colorspace(colorspace_t cspace)
Set the colorspace of the image.
Definition: seq_writer.cpp:106
void write(unsigned char *buffer)
Write a single image to disk.
Definition: seq_writer.cpp:116
void set_path(const char *img_path)
Set the path to where the images are stored.
Definition: seq_writer.cpp:73
Shared memory image buffer.
Definition: shm_image.h:184
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:198
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:228
void set_frame_id(const char *frame_id)
Set frame ID.
Definition: shm_image.cpp:150
virtual Camera * register_for_camera(const char *camera_string, fawkes::Thread *thread, colorspace_t cspace=YUV422_PLANAR)=0
Register thread for camera.
virtual void unregister_thread(fawkes::Thread *thread)=0
Unregister a thread.
Interface to write images.
Definition: writer.h:32
YUV Colormap.
Definition: yuvcm.h:36
virtual void set(unsigned int y, unsigned int u, unsigned int v, color_t c)
Set color class for given YUV value.
Definition: yuvcm.cpp:194
Fawkes library namespace.