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
115 m_intrinsic.put("physFocalLength",m_physFocalLength);
116 m_intrinsic.put("focalLengthX",m_focalLengthX);
117 m_intrinsic.put("focalLengthY",m_focalLengthY);
118 m_intrinsic.put("principalPointX",m_principalPointX);
119 m_intrinsic.put("principalPointY",m_principalPointY);
120
121 Value* val = Value::makeList();
122 auto* bb = val->asList();
123 for (double num : m_rectificationMatrix) { bb->addFloat64(num); }
124 m_intrinsic.put("rectificationMatrix",val);
125
126 m_intrinsic.put("distortionModel", m_distortionModel);
127 m_intrinsic.put("k1",m_k1);
128 m_intrinsic.put("k2",m_k2);
129 m_intrinsic.put("k3",m_k3);
130 m_intrinsic.put("t1",m_t1);
131 m_intrinsic.put("t2",m_t2);
132 //Only for debug
134 conf1.height=128;
135 conf1.width=128;
136 conf1.framerate=60.0;
137 conf1.pixelCoding=VOCAB_PIXEL_RGB;
138 configurations.push_back(conf1);
139
141 conf2.height=256;
142 conf2.width=256;
143 conf2.framerate=30.0;
144 conf2.pixelCoding=VOCAB_PIXEL_BGR;
145 configurations.push_back(conf2);
146
148 conf3.height=512;
149 conf3.width=512;
150 conf3.framerate=15.0;
151 conf3.pixelCoding=VOCAB_PIXEL_MONO;
152 configurations.push_back(conf3);
153
154 // The parameter `freq` is used to compute the period if the latter is not provided,
155 // but it can be used as an alternative to set the frequency of the grabber.
156 // The parameter `freq` has precedence over `period`, so if both are provided,
157 // `freq` will be used and `period` will be ignored.
158 if (m_freq>0)
159 {
160 m_period = 1/ m_freq;
161 }
162 else if (m_period>0)
163 {
164 //ok
165 }
166 else
167 {
168 yCError(FAKEFRAMEGRABBER, "Either `period` or `freq` parameters must be a valid >0 value");
169 return false;
170 }
171
172 if (!m_src.empty())
173 {
174 if (!yarp::sig::file::read(background, m_src))
175 {
176 yCWarning(FAKEFRAMEGRABBER, "Unable to open file");
177 return false;
178 }
179 }
180 if (background.width()>0)
181 {
182 if (m_width>0 || m_height>0)
183 {
184 yCWarning(FAKEFRAMEGRABBER, "width and height options are ignored when passing a background image");
185 }
186 m_width = background.width();
187 m_height = background.height();
188 m_have_bg = true;
189 }
190
191
192 m_snr = yarp::conf::clamp(m_snr, 0.0, 1.0);
193 m_mono = m_mono || m_bayer;
194
196 "Test grabber period %g / freq %g , mode %s",
197 m_period,
198 (1.0/m_period),
199 m_mode.c_str());
200
201 m_bx = m_width/2;
202 m_by = m_height/2;
203
204 for (auto& buff : buffs) {
205 buff.resize(m_width, m_height);
206 buff.zero();
207 }
208
209 if (!m_rpcPort.open(m_fakeFrameGrabber_rpc_port.c_str()))
210 {
211 yCError(FAKEFRAMEGRABBER, "Failed to open port %s", m_fakeFrameGrabber_rpc_port.c_str());
212 yCError(FAKEFRAMEGRABBER, "Do you have multiple FakeFrameGrabber devices running?");
213 yCError(FAKEFRAMEGRABBER, "If yes, use the `fakeFrameGrabber_rpc_port` parameter to set a different name for each of them");
214 return false;
215 }
216 m_rpcPort.setReader(*this);
217
218 start();
219
220 return true;
221}
222
224 double now = yarp::os::Time::now();
225
226 if (now-prev>1000) {
227 prev = now;
228 }
229 double dt = m_period-(now-prev);
230
231 if (dt>0) {
233 }
234
235 // this is the controlled instant when we consider the
236 // image as going out
237 prev += m_period;
238}
239
241{
242 std::lock_guard<std::mutex> lock(rpc_methods_mutex);
243 return m_height;
244}
245
247{
248 std::lock_guard<std::mutex> lock(rpc_methods_mutex);
249 return m_width;
250}
251
253{
254 while (!isStopping()) {
255 for (size_t i = 0; i < 2 && !isStopping(); ++i) {
256 if (!m_syncro) {
257 std::unique_lock<std::mutex> lk(mutex[i]);
258 createTestImage(buffs[i], buff_ts[i]);
259 timing();
260 lk.unlock();
261
262 curr_buff_mutex.lock();
263 curr_buff = i;
264 curr_buff_mutex.unlock();
265 std::this_thread::yield();
266 } else {
267 std::unique_lock<std::mutex> lk(mutex[i]);
268 img_consumed_cv[i].wait(lk, [&]{ if (img_ready[i]) {img_ready_cv[i].notify_one();} return (isStopping() || img_consumed[i]);});
269 if (isStopping()) {
270 break;
271 }
272 img_ready[i] = false;
273 img_consumed[i] = false;
274 createTestImage(buffs[i], buff_ts[i]);
275 img_ready[i] = true;
276 img_ready_cv[i].notify_all();
277 }
278 }
279 }
280}
281
283{
284 // Unlock any blocked thread.
285 if (m_syncro) {
286 for (size_t i = 0; i < 2; ++i) {
287 std::unique_lock<std::mutex> lk(mutex[i]);
288 img_consumed[i] = true;
289 img_consumed_cv[i].notify_all();
290 img_ready[i] = true;
291 img_ready_cv[i].notify_all();
292 }
293 }
294}
295
296
298{
299 if (!isRunning()) {
300 return ReturnValue::return_code::return_value_error_not_ready;
301 }
302
303 if (!m_syncro) {
304 curr_buff_mutex.lock();
305 size_t cb = curr_buff;
306 std::unique_lock<std::mutex> lk(mutex[cb]);
307 // Release the mutex after we get the lock on current image to avoid
308 // that the image is swapped while we are waiting to read it
309 curr_buff_mutex.unlock();
310 image.copy(buffs[cb]);
311 stamp.update(buff_ts[cb]);
312 } else {
313 curr_buff_mutex.lock();
314 timing();
315 size_t cb = curr_buff;
316 std::unique_lock<std::mutex> lk(mutex[cb]);
317 img_ready_cv[cb].wait(lk, [&]{return (!isRunning() || img_ready[cb]);});
318 if (!isRunning()) {
319 return ReturnValue::return_code::return_value_error_not_ready;
320 }
321
322 image.copy(buffs[cb]);
323 stamp.update(buff_ts[cb]);
324 img_consumed[cb] = true;
325 img_consumed_cv[cb].notify_one();
326
327 curr_buff = (cb + 1) % 2;
328 curr_buff_mutex.unlock();
329 }
330
331 return ReturnValue_ok;
332}
333
335{
336 if (!isRunning()) {
337 return ReturnValue::return_code::return_value_error_not_ready;
338 }
339
340 if (!m_syncro) {
341 curr_buff_mutex.lock();
342 size_t cb = curr_buff;
343 std::unique_lock<std::mutex> lk(mutex[cb]);
344 curr_buff_mutex.unlock();
345 if (m_bayer) {
346 makeSimpleBayer(buffs[cb], image);
347 } else {
348 image.copy(buffs[cb]);
349 }
350 stamp.update(buff_ts[cb]);
351 } else {
352 curr_buff_mutex.lock();
353 timing();
354 size_t cb = curr_buff;
355 std::unique_lock<std::mutex> lk(mutex[cb]);
356 img_ready_cv[cb].wait(lk, [&]{return (!isRunning() || img_ready[cb]);});
357 if (!isRunning()) {
358 return ReturnValue::return_code::return_value_error_not_ready;
359 }
360 if (m_bayer) {
361 makeSimpleBayer(buffs[cb], image);
362 } else {
363 image.copy(buffs[cb]);
364 }
365 stamp.update(buff_ts[cb]);
366 img_consumed[cb] = true;
367 img_consumed_cv[cb].notify_one();
368
369 curr_buff = (cb + 1) % 2;
370 curr_buff_mutex.unlock();
371 }
372
373 return ReturnValue_ok;
374}
375
377 std::vector<yarp::dev::vertex_t> vertices,
379{
380 yCDebugThrottle(FAKEFRAMEGRABBER, 5.0) << "Hardware crop requested!";
382}
383
385 std::vector<yarp::dev::vertex_t> vertices,
387{
388 yCDebugThrottle(FAKEFRAMEGRABBER, 5.0) << "Hardware crop requested!";
390}
391
393{
394 std::lock_guard<std::mutex> lock(rpc_methods_mutex);
395 return stamp;
396}
@ 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::vertex_t > 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.
Struct describing a possible camera configuration.
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:65
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:246
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:252
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:236
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
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition Property.cpp:987
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:21
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
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
A single value (typically within a Bottle).
Definition Value.h:44
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:228
static Value * makeList()
Create a list Value.
Definition Value.cpp:452
virtual Bottle * asList() const
Get list value.
Definition Value.cpp:252
virtual std::string asString() const
Get string value.
Definition Value.cpp:246
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
#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.
Definition OSTestMsg.cpp:13
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)