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 
13 using namespace yarp::os;
14 using namespace yarp::sig;
15 using namespace yarp::sig::draw;
16 using namespace yarp::sig::file;
17 using namespace yarp::dev;
18 
19 YARP_LOG_COMPONENT(FAKEBOT, "yarp.devices.FakeBot")
20 
21 
22 #define MAXRND 50000
23 namespace {
24 int rnds[MAXRND];
25 }
26 
27 void 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 
73 void 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 
177 void FakeBot::run() {
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:151
std::default_random_engine randengine
Definition: Random.cpp:13
bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &image) override
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:66
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
A single value (typically within a Bottle).
Definition: Value.h:45
T & safePixel(size_t x, size_t y)
Definition: Image.h:690
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:452
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:77
An interface for the device drivers.
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.
Very basic drawing functions, in case you don't have anything better available.
Definition: ImageDraw.h:23
void addCircle(ImageOf< T > &dest, const T &pix, int i, int j, int r)
Definition: ImageDraw.h:40
Image file operations.
Definition: ImageFile.h:21
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)
Definition: ImageFile.cpp:923
Signal processing.
Definition: Image.h:22
Packed RGB pixel type.
Definition: Image.h:464
unsigned char g
Definition: Image.h:466
unsigned char r
Definition: Image.h:465
unsigned char b
Definition: Image.h:467