YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakeFrameGrabber.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include "FakeFrameGrabber.h"
8
10#include <yarp/os/LogStream.h>
11#include <yarp/sig/ImageDraw.h>
12
13#include <cstdio>
14#include <thread>
15#include <random>
16
17using namespace yarp::os;
18using namespace yarp::dev;
19using namespace yarp::sig;
20using namespace yarp::sig::draw;
21
22namespace {
23YARP_LOG_COMPONENT(FAKEFRAMEGRABBER, "yarp.device.fakeFrameGrabber")
24} // namespace
25
27{
28 yarp::os::Bottle command;
29 yarp::os::Bottle reply;
30 bool ok = command.read(connection);
31 if (!ok) {
32 return false;
33 }
34 reply.clear();
35
36 if (command.get(0).asString()=="help")
37 {
38 reply.addVocab32("many");
39 reply.addString("set_mode <mode>");
40 reply.addString("set_image <file_name>/off");
41 reply.addString("available modes: ball, line, grid, size, rand, none, time");
42 reply.addString("set_noise on/off");
43 reply.addString("set_snr <snr>");
44 reply.addString("");
45 }
46 else if (command.get(0).asString() == "set_mode")
47 {
48 m_mode= command.get(1).asString();
49 reply.addString("ack");
50 }
51 else if (command.get(0).asString() == "set_image")
52 {
53 if (command.get(1).asString() == "off")
54 {
55 m_have_bg=false;
56 reply.addString("ack");
57 }
58 else
59 {
60 if (yarp::sig::file::read(background, command.get(1).asString()))
61 {
62 m_width = background.width();
63 m_height = background.height();
64 m_have_bg = true;
65 reply.addString("ack");
66 }
67 else
68 {
69 m_have_bg = false;
70 reply.addString("err");
71 }
72 }
73 }
74 else if (command.get(0).asString() == "set_noise")
75 {
76 if (command.get(1).asString() == "off") {
77 m_add_noise = false;
78 reply.addString("ack");
79 } else if (command.get(1).asString() == "on") {
80 m_add_noise = true;
81 reply.addString("ack");
82 } else {
83 reply.addString("err");
84 }
85 }
86 else if (command.get(0).asString() == "set_snr")
87 {
88 m_snr = yarp::conf::clamp(command.get(1).asFloat64(), 0.0, 1.0);
89 reply.addString("ack");
90 }
91 else
92 {
93 reply.addString("Unknown command. Type 'help'.");
94 }
95
97 if (returnToSender != nullptr)
98 {
99 reply.write(*returnToSender);
100 }
101
102 return true;
103}
104
106 stop();
107 m_rpcPort.close();
108 return true;
109}
110
112{
113 if (!this->parseParams(config)) {return false;}
114
116 m_intrinsic.focalLengthX = m_focalLengthX;
117 m_intrinsic.focalLengthY = m_focalLengthY;
121 if (m_rectificationMatrix.size() != 9)
122 {
123 yCError(FAKEFRAMEGRABBER, "The size rectificationMatrix parameter must be 9 (3x3");
124 return false;
125 }
126 for (size_t i = 0; i< m_rectificationMatrix.size(); i++)
127 {
129 }
131 m_intrinsic.distortionModel.k1 = m_k1;
132 m_intrinsic.distortionModel.k2 = m_k2;
133 m_intrinsic.distortionModel.k3 = m_k3;
134 m_intrinsic.distortionModel.t1 = m_t1;
135 m_intrinsic.distortionModel.t2 = m_t2;
136 //Only for debug
138 conf1.height=128;
139 conf1.width=128;
140 conf1.framerate=60.0;
141 conf1.pixelCoding=VOCAB_PIXEL_RGB;
142 configurations.push_back(conf1);
143
145 conf2.height=256;
146 conf2.width=256;
147 conf2.framerate=30.0;
148 conf2.pixelCoding=VOCAB_PIXEL_BGR;
149 configurations.push_back(conf2);
150
152 conf3.height=512;
153 conf3.width=512;
154 conf3.framerate=15.0;
155 conf3.pixelCoding=VOCAB_PIXEL_MONO;
156 configurations.push_back(conf3);
157
158 // The parameter `freq` is used to compute the period if the latter is not provided,
159 // but it can be used as an alternative to set the frequency of the grabber.
160 // The parameter `freq` has precedence over `period`, so if both are provided,
161 // `freq` will be used and `period` will be ignored.
162 if (m_freq>0)
163 {
164 m_period = 1/ m_freq;
165 }
166 else if (m_period>0)
167 {
168 //ok
169 }
170 else
171 {
172 yCError(FAKEFRAMEGRABBER, "Either `period` or `freq` parameters must be a valid >0 value");
173 return false;
174 }
175
176 if (!m_src.empty())
177 {
178 if (!yarp::sig::file::read(background, m_src))
179 {
180 yCWarning(FAKEFRAMEGRABBER, "Unable to open file");
181 return false;
182 }
183 }
184 if (background.width()>0)
185 {
186 if (m_width>0 || m_height>0)
187 {
188 yCWarning(FAKEFRAMEGRABBER, "width and height options are ignored when passing a background image");
189 }
190 m_width = background.width();
191 m_height = background.height();
192 m_have_bg = true;
193 }
194
195
196 m_snr = yarp::conf::clamp(m_snr, 0.0, 1.0);
197 m_mono = m_mono || m_bayer;
198
200 "Test grabber period %g / freq %g , mode %s",
201 m_period,
202 (1.0/m_period),
203 m_mode.c_str());
204
205 m_bx = m_width/2;
206 m_by = m_height/2;
207
208 for (auto& buff : buffs) {
209 buff.resize(m_width, m_height);
210 buff.zero();
211 }
212
213 if (!m_rpcPort.open(m_fakeFrameGrabber_rpc_port.c_str()))
214 {
215 yCError(FAKEFRAMEGRABBER, "Failed to open port %s", m_fakeFrameGrabber_rpc_port.c_str());
216 yCError(FAKEFRAMEGRABBER, "Do you have multiple FakeFrameGrabber devices running?");
217 yCError(FAKEFRAMEGRABBER, "If yes, use the `fakeFrameGrabber_rpc_port` parameter to set a different name for each of them");
218 return false;
219 }
220 m_rpcPort.setReader(*this);
221
222 start();
223
224 return true;
225}
226
228 double now = yarp::os::Time::now();
229
230 if (now-prev>1000) {
231 prev = now;
232 }
233 double dt = m_period-(now-prev);
234
235 if (dt>0) {
237 }
238
239 // this is the controlled instant when we consider the
240 // image as going out
241 prev += m_period;
242}
243
245{
246 std::lock_guard<std::mutex> lock(rpc_methods_mutex);
247 return m_height;
248}
249
251{
252 std::lock_guard<std::mutex> lock(rpc_methods_mutex);
253 return m_width;
254}
255
257{
258 while (!isStopping()) {
259 for (size_t i = 0; i < 2 && !isStopping(); ++i) {
260 if (!m_syncro) {
261 std::unique_lock<std::mutex> lk(mutex[i]);
262 createTestImage(buffs[i], buff_ts[i]);
263 timing();
264 lk.unlock();
265
266 curr_buff_mutex.lock();
267 curr_buff = i;
268 curr_buff_mutex.unlock();
269 std::this_thread::yield();
270 } else {
271 std::unique_lock<std::mutex> lk(mutex[i]);
272 img_consumed_cv[i].wait(lk, [&]{ if (img_ready[i]) {img_ready_cv[i].notify_one();} return (isStopping() || img_consumed[i]);});
273 if (isStopping()) {
274 break;
275 }
276 img_ready[i] = false;
277 img_consumed[i] = false;
278 createTestImage(buffs[i], buff_ts[i]);
279 img_ready[i] = true;
280 img_ready_cv[i].notify_all();
281 }
282 }
283 }
284}
285
287{
288 // Unlock any blocked thread.
289 if (m_syncro) {
290 for (size_t i = 0; i < 2; ++i) {
291 std::unique_lock<std::mutex> lk(mutex[i]);
292 img_consumed[i] = true;
293 img_consumed_cv[i].notify_all();
294 img_ready[i] = true;
295 img_ready_cv[i].notify_all();
296 }
297 }
298}
299
300
302{
303 if (!isRunning()) {
304 return ReturnValue::return_code::return_value_error_not_ready;
305 }
306
307 if (!m_syncro) {
308 curr_buff_mutex.lock();
309 size_t cb = curr_buff;
310 std::unique_lock<std::mutex> lk(mutex[cb]);
311 // Release the mutex after we get the lock on current image to avoid
312 // that the image is swapped while we are waiting to read it
313 curr_buff_mutex.unlock();
314 image.copy(buffs[cb]);
315 stamp.update(buff_ts[cb]);
316 } else {
317 curr_buff_mutex.lock();
318 timing();
319 size_t cb = curr_buff;
320 std::unique_lock<std::mutex> lk(mutex[cb]);
321 img_ready_cv[cb].wait(lk, [&]{return (!isRunning() || img_ready[cb]);});
322 if (!isRunning()) {
323 return ReturnValue::return_code::return_value_error_not_ready;
324 }
325
326 image.copy(buffs[cb]);
327 stamp.update(buff_ts[cb]);
328 img_consumed[cb] = true;
329 img_consumed_cv[cb].notify_one();
330
331 curr_buff = (cb + 1) % 2;
332 curr_buff_mutex.unlock();
333 }
334
335 return ReturnValue_ok;
336}
337
339{
340 if (!isRunning()) {
341 return ReturnValue::return_code::return_value_error_not_ready;
342 }
343
344 if (!m_syncro) {
345 curr_buff_mutex.lock();
346 size_t cb = curr_buff;
347 std::unique_lock<std::mutex> lk(mutex[cb]);
348 curr_buff_mutex.unlock();
349 if (m_bayer) {
350 makeSimpleBayer(buffs[cb], image);
351 } else {
352 image.copy(buffs[cb]);
353 }
354 stamp.update(buff_ts[cb]);
355 } else {
356 curr_buff_mutex.lock();
357 timing();
358 size_t cb = curr_buff;
359 std::unique_lock<std::mutex> lk(mutex[cb]);
360 img_ready_cv[cb].wait(lk, [&]{return (!isRunning() || img_ready[cb]);});
361 if (!isRunning()) {
362 return ReturnValue::return_code::return_value_error_not_ready;
363 }
364 if (m_bayer) {
365 makeSimpleBayer(buffs[cb], image);
366 } else {
367 image.copy(buffs[cb]);
368 }
369 stamp.update(buff_ts[cb]);
370 img_consumed[cb] = true;
371 img_consumed_cv[cb].notify_one();
372
373 curr_buff = (cb + 1) % 2;
374 curr_buff_mutex.unlock();
375 }
376
377 return ReturnValue_ok;
378}
379
381 std::vector<yarp::dev::VertexData> vertices,
383{
384 yCDebugThrottle(FAKEFRAMEGRABBER, 5.0) << "Hardware crop requested!";
386}
387
389 std::vector<yarp::dev::VertexData> vertices,
391{
392 yCDebugThrottle(FAKEFRAMEGRABBER, 5.0) << "Hardware crop requested!";
394}
395
397{
398 std::lock_guard<std::mutex> lock(rpc_methods_mutex);
399 return stamp;
400}
@ VOCAB_PIXEL_BGR
Definition Image.h:49
@ VOCAB_PIXEL_MONO
Definition Image.h:42
@ VOCAB_PIXEL_RGB
Definition Image.h:44
#define ReturnValue_ok
Definition ReturnValue.h:82
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
int width() const override
Return the width of each frame.
yarp::os::Stamp getLastInputStamp() override
Return the time stamp relative to the last acquisition.
int height() const override
Return the height of each frame.
bool open(yarp::os::Searchable &config) override
yarp::dev::ReturnValue getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &image) override
Get an image from the frame grabber.
yarp::dev::ReturnValue getImageCrop(yarp::dev::cropType_id_t cropType, std::vector< yarp::dev::VertexData > vertices, yarp::sig::ImageOf< yarp::sig::PixelRgb > &image) override
Get a crop of the image from the frame grabber.
void run() override
Main body of the new thread.
void onStop() override
Call-back, called while halting the thread (before join).
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool close() override
Close the DeviceDriver.
Read a YARP-format image (of a specific type) from a device.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
void addVocab32(yarp::conf::vocab32_t x)
Places a 32 bit vocabulary item in the bottle, at the end of the list.
Definition Bottle.cpp:164
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition Bottle.cpp:241
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:247
void clear()
Empties the bottle of any objects it contains.
Definition Bottle.cpp:121
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
Definition Bottle.cpp:231
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition Bottle.cpp:176
A mini-server for performing network communication in the background.
An interface for reading from a network connection.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition Port.cpp:470
void close() override
Stop port activity.
Definition Port.cpp:330
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition Port.cpp:79
A base class for nested structures that can be searched.
Definition Searchable.h:31
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:18
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:49
bool stop()
Stop the thread.
Definition Thread.cpp:81
bool isStopping()
Returns true if the thread is stopping (Thread::stop has been called).
Definition Thread.cpp:99
bool isRunning()
Returns true if the thread is running (Thread::start has been called successfully and the thread has ...
Definition Thread.cpp:105
bool start()
Start the new thread running.
Definition Thread.cpp:93
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:222
virtual std::string asString() const
Get string value.
Definition Value.cpp:240
static int32_t fromString(const std::string &input)
Typed image class.
Definition Image.h:603
size_t width() const
Gets width of image in pixels.
Definition Image.h:171
size_t height() const
Gets height of image in pixels.
Definition Image.h:177
yarp::sig::DistortionModelData distortionModel
#define yCInfo(component,...)
#define yCError(component,...)
#define yCDebugThrottle(component, period,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
constexpr const T & clamp(const T &v, const T &lo, const T &hi)
Definition numeric.h:92
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition Time.cpp:111
An interface to the operating system, including Port based communication.
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)