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