YARP
Yet Another Robot Platform
Ping.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms of the
7  * BSD-3-Clause license. See the accompanying LICENSE file for details.
8  */
9 
10 #include <yarp/os/Ping.h>
11 
12 #include <yarp/os/Bottle.h>
13 #include <yarp/os/Log.h>
14 #include <yarp/os/Network.h>
15 #include <yarp/os/Port.h>
16 #include <yarp/os/Time.h>
17 #include <yarp/os/Vocab.h>
18 
19 #include <cmath>
20 #include <cstdio>
21 #include <mutex>
22 
23 using namespace yarp::os;
24 
26 {
27  clear();
28 }
29 
31 {
32  tot = tot2 = 0;
33  ct = at = 0;
34  mu = 0;
35  sigma = 1e10;
36  // infinity would be better, but methods of getting infinity
37  // require awkward dependencies
38 }
39 
40 void Stat::add(double val)
41 {
42  tot += val;
43  tot2 += val * val;
44  ct++;
45 }
46 
47 void Stat::add(const Stat& alt)
48 {
49  tot += alt.tot;
50  tot2 += alt.tot2;
51  ct += alt.ct;
52 }
53 
54 double Stat::mean()
55 {
56  compute();
57  return mu;
58 }
59 
61 {
62  compute();
63  return sigma;
64 }
65 
66 double Stat::count()
67 {
68  return ct;
69 }
70 
71 Stat::operator double()
72 {
73  return mean();
74 }
75 
76 void Stat::compute()
77 {
78  if (ct != at) {
79  // ct must be > 0
80  mu = tot / ct;
81  sigma = tot2 / ct - mu * mu;
82  if (sigma < 0) {
83  sigma = 0; // round-off error
84  }
85  sigma = sqrt(sigma);
86  at = ct;
87  }
88 }
89 
90 
92 {
93  totalTime.clear();
94  targetTime.clear();
95 }
96 
98 {
99  totalTime.add(alt.totalTime);
100  targetTime.add(alt.targetTime);
101 }
102 
103 
105 {
106  period.clear();
107 }
108 
109 void RateResult::add(const RateResult& alt)
110 {
111  period.add(alt.period);
112 }
113 
114 
115 Ping::Ping(const char* target)
116 {
117  if (target != nullptr) {
118  setTarget(target);
119  }
120 }
121 
122 bool Ping::setTarget(const char* target)
123 {
124  this->target = target;
125  return true;
126 }
127 
128 
130 {
131  lastConnect.clear();
132  double start = SystemClock::nowSystem();
133  Contact c = NetworkBase::queryName(target);
134  double afterQuery = SystemClock::nowSystem();
135  if (!c.isValid()) {
136  yError("Port not found during ping");
137  }
139  rpc.admin = true;
140  rpc.quiet = true;
141  Bottle cmd;
142  Bottle reply;
143  cmd.addVocab(Vocab::encode("ver"));
144  bool ok = NetworkBase::write(c, cmd, reply, rpc);
145  if (!ok) {
146  yError("Port did not respond as expected");
147  }
148  double stop = SystemClock::nowSystem();
149  lastConnect.totalTime.add(stop - start);
150  lastConnect.targetTime.add(stop - afterQuery);
151  accumConnect.add(lastConnect);
152 }
153 
155 {
156  long int ping = lround(accumConnect.targetTime.count() + 0.5);
157  if (ping > 0) {
158  printf("Ping #%ld:\n", lround(accumConnect.targetTime.count() + 0.5));
159  int space = 14;
160  int decimal = 5;
161  printf(" %s connection time (%s with name lookup)\n",
162  renderTime(lastConnect.targetTime.mean(), space, decimal).c_str(),
163  renderTime(lastConnect.totalTime.mean(), space, decimal).c_str());
164  if (accumConnect.totalTime.count() > 1) {
165  printf(" %s +/- %s on average (%s +/- %s with name lookup)\n",
166  renderTime(accumConnect.targetTime.mean(), space, decimal).c_str(),
167  renderTime(accumConnect.targetTime.deviation(), space, decimal).c_str(),
168  renderTime(accumConnect.totalTime.mean(), space, decimal).c_str(),
169  renderTime(accumConnect.totalTime.deviation(), space, decimal).c_str());
170  }
171  }
172 }
173 
174 
175 std::string Ping::renderTime(double t, int space, int decimal)
176 {
177  std::string unit;
178  double times = 1;
179  if (space < 0) {
180  yError("Negative space");
181  }
182  if (t >= 1) {
183  unit = "sec";
184  } else if (t > 1e-3) {
185  unit = " ms";
186  times = 1e3;
187  } else if (t > 1e-6) {
188  unit = " us";
189  times = 1e6;
190  } else if (t > 1e-9) {
191  unit = " ns";
192  times = 1e9;
193  }
194  char buf[512];
195  std::snprintf(buf, sizeof(buf), "%.*f%s", decimal, t * times, unit.c_str());
196  return buf;
197 }
198 
199 
200 class PingSampler : public PortReader
201 {
202 public:
203  std::mutex mutex;
204  int ct{0};
205  double lastTime{0};
207 
209  mutex()
210  {
211  }
212 
213  bool read(ConnectionReader& connection) override
214  {
215  double now = SystemClock::nowSystem();
216  Bottle b;
217  bool ok = b.read(connection);
218  if (ok) {
219  mutex.lock();
220  ct++;
221  if (ct > 1) {
222  double dt = now - lastTime;
223  period.add(dt);
224  }
225  lastTime = now;
226  printf("Period is %g +/- %g (%d)\n",
227  period.mean(),
228  period.deviation(),
229  ct);
230  mutex.unlock();
231  }
232  return ok;
233  }
234 };
235 
237 {
238  Port p;
239  PingSampler sampler;
240  p.setReader(sampler);
241  p.open("...");
242  printf("Pausing for 5 seconds...\n");
243  NetworkBase::connect(target, p.getName());
245  p.close();
246  printf("Period is %g +/- %g (%d)\n",
247  sampler.period.mean(),
248  sampler.period.deviation(),
249  sampler.ct);
250 }
251 
253 {
254  lastConnect.clear();
255  accumConnect.clear();
256 }
257 
259 {
260  return lastConnect;
261 }
262 
264 {
265  return accumConnect;
266 }
static bool rpc(const Contact &c, const char *carrier, Bottle &writer, Bottle &reader, bool verbose)
Definition: RosLookup.cpp:19
void add(const RateResult &alt)
Definition: Ping.cpp:109
bool isValid() const
Checks if a Contact is tagged as valid.
Definition: Contact.cpp:295
ConnectResult getAverageConnect()
Definition: Ping.cpp:263
void add(const ConnectResult &alt)
Definition: Ping.cpp:97
double mean()
Definition: Ping.cpp:54
void clear()
Definition: Ping.cpp:252
static double nowSystem()
Definition: SystemClock.cpp:37
A mini-server for network communication.
Definition: Port.h:49
int ct
Definition: Ping.cpp:204
void clear()
Definition: Ping.cpp:30
bool read(ConnectionReader &connection) override
Read this object from a network connection.
Definition: Ping.cpp:213
void connect()
Definition: Ping.cpp:129
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition: Port.cpp:78
void add(double val)
Definition: Ping.cpp:40
float t
Interface implemented by all objects that can read themselves from the network, such as Bottle object...
Definition: PortReader.h:27
double deviation()
Definition: Ping.cpp:60
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
Preferences for how to communicate with a contact.
Definition: ContactStyle.h:26
virtual std::string getName() const
Get name of port.
Definition: Contactable.cpp:17
static Contact queryName(const std::string &name)
Find out information about a registered name.
Definition: Network.cpp:918
bool quiet
Suppress all outputs and warnings.
Definition: ContactStyle.h:39
NetInt32 encode(const std::string &str)
Convert a string into a vocabulary identifier.
Definition: Vocab.cpp:14
static std::string renderTime(double t, int space, int decimal)
Definition: Ping.cpp:175
A simple collection of objects that can be described and transmitted in a portable way...
Definition: Bottle.h:72
void report()
Definition: Ping.cpp:154
double count()
Definition: Ping.cpp:66
void addVocab(int x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:146
#define yError
Definition: Log.h:109
An interface for reading from a network connection.
static void delaySystem(double seconds)
Definition: SystemClock.cpp:32
void sample()
Definition: Ping.cpp:236
Represents how to reach a part of a YARP network.
Definition: Contact.h:38
bool read(ConnectionReader &reader) override
Set the bottle&#39;s value based on input from a network connection.
Definition: Bottle.cpp:222
static bool connect(const std::string &src, const std::string &dest, const std::string &carrier="", bool quiet=true)
Request that an output port connect to an input port.
Definition: Network.cpp:615
ConnectResult getLastConnect()
Definition: Ping.cpp:258
Stat period
Definition: Ping.cpp:206
std::mutex mutex
Definition: Ping.cpp:203
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition: Port.cpp:499
static bool write(const Contact &contact, PortWriter &cmd, PortReader &reply, bool admin=false, bool quiet=false, double timeout=-1)
Send a single command to a port and await a single response.
Definition: Network.cpp:1149
Ping(const char *target=nullptr)
Definition: Ping.cpp:115
bool setTarget(const char *target)
Definition: Ping.cpp:122
An interface to the operating system, including Port based communication.
void close() override
Stop port activity.
Definition: Port.cpp:349
bool admin
Ask recipient to treat message as administrative.
Definition: ContactStyle.h:34
PingSampler()
Definition: Ping.cpp:208