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
35using namespace fawkes;
36using 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 */
50FvRetrieverThread::FvRetrieverThread(const std::string &camera_string,
51 const std::string &cfg_name,
52 const std::string &cfg_prefix)
53: Thread("FvRetrieverThread", Thread::OPMODE_WAITFORWAKEUP),
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
69void
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
174void
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. */
188void
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.