YARP
Yet Another Robot Platform
FakeBot.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 "FakeBot.h"
8
9#include <yarp/sig/all.h>
10#include <yarp/sig/ImageFile.h>
11#include <random>
12
13using namespace yarp::os;
14using namespace yarp::sig;
15using namespace yarp::sig::draw;
16using namespace yarp::sig::file;
17using namespace yarp::dev;
18
19YARP_LOG_COMPONENT(FAKEBOT, "yarp.devices.FakeBot")
20
21
22#define MAXRND 50000
23namespace {
24int rnds[MAXRND];
25}
26
27void FakeBot::init() {
28 int m_w = 640;
29 int m_h = 480;
30 int rr = 30;
31
32 back.resize(m_w,m_h);
33 PixelRgb w{255,255,255};
34 PixelRgb r{128,128,255};
35 PixelRgb b{0,0,255};
36 IMGFOR(back,x,y) {
37 back(x,y) = r;
38 if (y>=m_h*0.75) {
39 back(x,y) = b;
40 }
41 }
42 for (int i=0; i<=m_w; i+=m_w/10) {
43 addCircle(back,b,i,int(m_h*0.77),rr);
44 }
45
46 std::default_random_engine randengine;
47 std::uniform_real_distribution<double> udist1(-m_w/4,m_w/4);
48 std::uniform_real_distribution<double> udist2(-int(m_h*0.2),rr);
49 for (int j=0; j<40; j++) {
50 int xx = m_w/2 + udist1(randengine);
51 int yy = int(m_h*0.2) - udist2(randengine);
52 addCircle(back,w,xx,yy,rr);
53 }
54
55 m_x = (back.width()-this->m_w)/2;
56 m_y = (back.height()-this->m_h)/2;
57 m_dx = m_dy = 0;
58
59 std::normal_distribution<double> ndist(0,255);
60 for (int & rnd : rnds) {
61 rnd = int(ndist(randengine));
62 }
63
64 fore.resize(64,64);
65 fore.zero();
66 m_tx = back.width()/2;
67 m_ty = back.height()*0.4;
68 m_tdx = 1;
69 m_tdy = 0;
70}
71
72
73void scramble(unsigned char& ch, float f) {
74 int x = ch;
75 static int idx = 0;
76 //x += (int)(Random::normal(0,x*f));
77 x += (int)(rnds[idx]*f);
78 idx = (idx+17)%MAXRND;
79 if (x < 0) {
80 x = 0;
81 }
82 if (x > 255) {
83 x = 255;
84 }
85 ch = (unsigned char) x;
86}
87
88
90 std::string backFile = config.check("background",Value("textures/back.ppm"),
91 "background image to use").asString();
92 if (backFile!="") {
93 yarp::sig::file::read(back,backFile);
94 }
95 std::string foreFile = config.check("target",Value("textures/fore.ppm"),
96 "target image to use").asString();
97 if (foreFile!="") {
98 yarp::sig::file::read(fore,foreFile);
99 }
100 noiseLevel = config.check("noise",Value(0.05),
101 "pixel noise level").asFloat64();
102
103 xScale = config.check("sx",Value(1.0),
104 "scaling for x coordinate").asFloat64();
105 yScale = config.check("sy",Value(1.0),
106 "scaling for y coordinate").asFloat64();
107
108 lifetime = config.check("lifetime",Value(-1.0),
109 "device should exist for this length of time (in seconds)").asFloat64();
110 if (lifetime>=0) {
111 start();
112 }
113 return true;
114}
115
116
117// IFrameGrabberImage
119 if (fabs(dpos[0])>0.001||fabs(dpos[0])>0.001) {
120 pos[0] = m_dx+dpos[0];
121 dpos[0] = 0;
122 pos[1] = m_dy+dpos[1];
123 dpos[1] = 0;
124 }
125 pos[0] += vel[0];
126 pos[1] += vel[1];
127 double xx = pos[0];
128 double yy = pos[1];
129 double dx = (xx-m_dx)*5;
130 double dy = (yy-m_dy)*5;
131 m_tx += m_tdx;
132 m_ty += m_tdy;
133 if (m_tdx>0 && m_tx>back.width()*0.75) {
134 m_tdx *= -1;
135 }
136 if (m_tdx<0 && m_tx<back.width()*0.25) {
137 m_tdx *= -1;
138 }
139 dx /= 40;
140 dy /= 40;
141 if (amp[0]>0.5) {
142 m_dx += dx;
143 }
144 if (amp[1]>0.5) {
145 m_dy += dy;
146 }
147 image.resize(m_w,m_h);
148 back.safePixel(-1,-1) = PixelRgb{255,0,0};
149 loc[0] = m_dx;
150 loc[1] = m_dy;
151
152 double m_dx_scaled = m_dx*xScale;
153 double m_dy_scaled = m_dy*yScale;
154 IMGFOR(image,x,y) {
155 int x0 = int(x+m_x+m_dx_scaled*0.5+0.5);
156 int y0 = int(y+m_y+m_dy_scaled*0.5+0.5);
157 image(x,y) = back.safePixel(x0,y0);
158
159 if (fore.isPixel(int(x0-m_tx),int(y0-m_ty))) {
160 PixelRgb& pix = fore(int(x0-m_tx),int(y0-m_ty));
161 if (pix.r<200||pix.g>100||pix.b>100) {
162 image(x,y) = pix;
163 }
164 }
165 }
166 IMGFOR(image,x2,y2) {
167 PixelRgb& pix = image(x2,y2);
168 auto f = (float)(noiseLevel);
169 scramble(pix.r,f);
170 scramble(pix.g,f);
171 scramble(pix.b,f);
172 }
173 Time::delay(0.1); // simulated hardware delay, using mutable clock
174 return true;
175}
176
178 if (lifetime>=0) {
179 Time::delay(lifetime);
180 std::exit(0);
181 }
182}
const yarp::os::LogComponent & FAKEBOT()
Definition: FakeBot.cpp:19
void scramble(unsigned char &ch, float f)
Definition: FakeBot.cpp:73
#define MAXRND
Definition: FakeBot.cpp:22
#define IMGFOR(img, i, j)
Definition: ImageDraw.h:143
std::default_random_engine randengine
Definition: Random.cpp:13
bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &image) override
Get an image from the frame grabber.
Definition: FakeBot.cpp:118
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
Definition: FakeBot.cpp:89
void run() override
Main body of the new thread.
Definition: FakeBot.cpp:177
A base class for nested structures that can be searched.
Definition: Searchable.h:56
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
bool start()
Start the new thread running.
Definition: Thread.cpp:93
A single value (typically within a Bottle).
Definition: Value.h:43
T & safePixel(size_t x, size_t y)
Definition: Image.h:685
size_t width() const
Gets width of image in pixels.
Definition: Image.h:163
void resize(size_t imgWidth, size_t imgHeight)
Reallocate an image to be of a desired size, throwing away its current contents.
Definition: Image.cpp:453
bool isPixel(size_t x, size_t y) const
Check whether a coordinate lies within the image.
Definition: Image.h:247
void zero()
Set all pixels to 0.
Definition: Image.cpp:446
size_t height() const
Gets height of image in pixels.
Definition: Image.h:169
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
For streams capable of holding different kinds of content, check what they actually have.
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.
void addCircle(ImageOf< T > &dest, const T &pix, int i, int j, int r)
Definition: ImageDraw.h:32
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)
Definition: ImageFile.cpp:915
Packed RGB pixel type.
Definition: Image.h:460
unsigned char g
Definition: Image.h:462
unsigned char r
Definition: Image.h:461
unsigned char b
Definition: Image.h:463