YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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{
91 if (!this->parseParams(config)) {return false;}
92
93 if (m_background !="") {
95 }
96
97 if (m_target !="") {
99 }
100
101 if (m_lifetime>=0) {
102 start();
103 }
104 return true;
105}
106
107
108// IFrameGrabberImage
110 if (fabs(dpos[0])>0.001||fabs(dpos[0])>0.001) {
111 pos[0] = m_dx+dpos[0];
112 dpos[0] = 0;
113 pos[1] = m_dy+dpos[1];
114 dpos[1] = 0;
115 }
116 pos[0] += vel[0];
117 pos[1] += vel[1];
118 double xx = pos[0];
119 double yy = pos[1];
120 double dx = (xx-m_dx)*5;
121 double dy = (yy-m_dy)*5;
122 m_tx += m_tdx;
123 m_ty += m_tdy;
124 if (m_tdx>0 && m_tx>back.width()*0.75) {
125 m_tdx *= -1;
126 }
127 if (m_tdx<0 && m_tx<back.width()*0.25) {
128 m_tdx *= -1;
129 }
130 dx /= 40;
131 dy /= 40;
132 if (amp[0]>0.5) {
133 m_dx += dx;
134 }
135 if (amp[1]>0.5) {
136 m_dy += dy;
137 }
138 image.resize(m_w,m_h);
139 back.safePixel(-1,-1) = PixelRgb{255,0,0};
140 loc[0] = m_dx;
141 loc[1] = m_dy;
142
143 double m_dx_scaled = m_dx* m_sx;
144 double m_dy_scaled = m_dy* m_sy;
145 IMGFOR(image,x,y) {
146 int x0 = int(x+m_x+m_dx_scaled*0.5+0.5);
147 int y0 = int(y+m_y+m_dy_scaled*0.5+0.5);
148 image(x,y) = back.safePixel(x0,y0);
149
150 if (fore.isPixel(int(x0-m_tx),int(y0-m_ty))) {
151 PixelRgb& pix = fore(int(x0-m_tx),int(y0-m_ty));
152 if (pix.r<200||pix.g>100||pix.b>100) {
153 image(x,y) = pix;
154 }
155 }
156 }
157 IMGFOR(image,x2,y2) {
158 PixelRgb& pix = image(x2,y2);
159 auto f = (float)(m_noise);
160 scramble(pix.r,f);
161 scramble(pix.g,f);
162 scramble(pix.b,f);
163 }
164 Time::delay(0.1); // simulated hardware delay, using mutable clock
165 return true;
166}
167
169 if (m_lifetime>=0) {
171 std::exit(0);
172 }
173}
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 parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &image) override
Get an image from the frame grabber.
Definition FakeBot.cpp:109
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:168
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
Definition Searchable.h:31
bool start()
Start the new thread running.
Definition Thread.cpp:93
Typed image class.
Definition Image.h:616
T & safePixel(size_t x, size_t y)
Definition Image.h:648
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,...)
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)