YARP
Yet Another Robot Platform
AnalogSensorClient.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 "AnalogSensorClient.h"
8 #include <yarp/os/Log.h>
9 #include <yarp/os/LogComponent.h>
10 #include <yarp/os/LogStream.h>
11 #include <yarp/os/Value.h>
12 
15 using namespace yarp::dev;
16 using namespace yarp::os;
17 using namespace yarp::sig;
18 
19 namespace
20 {
21 YARP_LOG_COMPONENT(ANALOGSENSORCLIENT, "yarp.device.analogsensorclient")
22 
23 inline int checkResponse(bool ok, const yarp::os::Bottle& response)
24 {
25  const yarp::os::Value & v = response.get(0);
26  return ok && v.isInt32() ? v.asInt32() : IAnalogSensor::AS_ERROR;
27 }
28 
29 } // namespace
30 
32 {
33  mutex.lock();
34  count=0;
35  deltaT=0;
36  deltaTMax=0;
37  deltaTMin=1e22;
38  now=Time::now();
39  prev=now;
40  mutex.unlock();
41 }
42 
44 {
46  resetStat();
47 }
48 
50 {
51  now=Time::now();
52  mutex.lock();
53 
54  if (count>0)
55  {
56  double tmpDT=now-prev;
57  deltaT+=tmpDT;
58  if (tmpDT > deltaTMax) {
59  deltaTMax = tmpDT;
60  }
61  if (tmpDT < deltaTMin) {
62  deltaTMin = tmpDT;
63  }
64 
65  //compare network time
66  if (tmpDT*1000<ANALOG_TIMEOUT)
67  {
69  }
70  else
71  {
73  }
74  }
75 
76  prev=now;
77  count++;
78 
79  lastVector=v;
80  Stamp newStamp;
81  getEnvelope(newStamp);
82 
83  //initialialization (first received data)
84  if (lastStamp.isValid()==false)
85  {
86  lastStamp = newStamp;
87  }
88 
89  //now compare timestamps
90  if ((1000*(newStamp.getTime()-lastStamp.getTime()))<ANALOG_TIMEOUT)
91  {
93  }
94  else
95  {
97  }
98  lastStamp = newStamp;
99 
100  mutex.unlock();
101 }
102 
104 {
105  mutex.lock();
106  int ret=state;
108  {
109  data=lastVector;
110  stmp = lastStamp;
111  }
112  mutex.unlock();
113 
114  return ret;
115 }
116 
118 {
119  mutex.lock();
120  int ret=count;
121  mutex.unlock();
122  return ret;
123 }
124 
125 // time is in ms
126 void InputPortProcessor::getEstFrequency(int &ite, double &av, double &min, double &max)
127 {
128  mutex.lock();
129  ite=count;
130  min=deltaTMin*1000;
131  max=deltaTMax*1000;
132  if (count<1)
133  {
134  av=0;
135  }
136  else
137  {
138  av=deltaT/count;
139  }
140  av=av*1000;
141  mutex.unlock();
142 }
143 
145 {
146  return state;
147 }
148 
150 {
151  if (state==IAnalogSensor::AS_ERROR)
152  {
153  return 0;
154  }
155  else
156  {
157  return (int)lastVector.size();
158  }
159 }
160 
162 {
163  bool done = false;
164  while(!done)
165  {
166  std::size_t found = name.find('/');
167  // if no '/' found, I'm done
168  if (found == std::string::npos)
169  {
170  done = true;
171  continue;
172  }
173 
174  yCDebug(ANALOGSENSORCLIENT) << "found is " << found << "; length is : " << name.length();
175  // remove leading or trailing '/'
176  if( (found == 0) || (found == name.length()-1 ) /*found starts from 0, length doesn't*/ )
177  {
178  done = false; // we could have both leading and trailing, so let's check again
179  name.erase(found,1);
180  } else {
181  done = true; // there is some '/', but their are in the middle and they are allowed
182  }
183  }
184 
185  yCDebug(ANALOGSENSORCLIENT) << name;
186 }
187 
189 {
190  yCWarning(ANALOGSENSORCLIENT) << "The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient'";
191  yCWarning(ANALOGSENSORCLIENT) << "The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.";
192  yCWarning(ANALOGSENSORCLIENT) << "Please update your scripts.";
193 
194  std::string carrier = config.check("carrier", Value("udp"), "default carrier for streaming robot state").asString();
195 
196  local.clear();
197  remote.clear();
198 
199  local = config.find("local").asString();
200  remote = config.find("remote").asString();
201 
202  if (local=="")
203  {
204  yCError(ANALOGSENSORCLIENT, "AnalogSensorClient::open() error you have to provide valid local name");
205  return false;
206  }
207  if (remote=="")
208  {
209  yCError(ANALOGSENSORCLIENT, "AnalogSensorClient::open() error you have to provide valid remote name\n");
210  return false;
211  }
212 
213  std::string local_rpc = local;
214  local_rpc += "/rpc:o";
215  std::string remote_rpc = remote;
216  remote_rpc += "/rpc:i";
217 
218  if (!inputPort.open(local))
219  {
220  yCError(ANALOGSENSORCLIENT, "AnalogSensorClient::open() error could not open port %s, check network", local.c_str());
221  return false;
222  }
223  inputPort.useCallback();
224 
225  if (!rpcPort.open(local_rpc))
226  {
227  yCError(ANALOGSENSORCLIENT, "AnalogSensorClient::open() error could not open rpc port %s, check network", local_rpc.c_str());
228  return false;
229  }
230 
231  bool ok=Network::connect(remote.c_str(), local.c_str(), carrier.c_str());
232  if (!ok)
233  {
234  yCError(ANALOGSENSORCLIENT, "AnalogSensorClient::open() error could not connect to %s", remote.c_str());
235  return false;
236  }
237 
238  ok=Network::connect(local_rpc, remote_rpc);
239  if (!ok)
240  {
241  yCError(ANALOGSENSORCLIENT, "AnalogSensorClient::open() error could not connect to %s\n", remote_rpc.c_str());
242  return false;
243  }
244 
245  return true;
246 }
247 
249 {
250  rpcPort.close();
251  inputPort.close();
252  return true;
253 }
254 
256 {
257  return inputPort.getLast(out, lastTs);
258 }
259 
261 {
262  //not implemented yet
263  return AS_OK;
264 }
265 
267 {
268  return inputPort.getChannels();
269 }
270 
272 {
273  Bottle cmd, response;
276  bool ok = rpcPort.write(cmd, response);
277  return checkResponse(ok, response);
278 }
279 
281 {
282  Bottle cmd, response;
285  Bottle& l = cmd.addList();
286  for (int i = 0; i < this->getChannels(); i++) {
287  l.addFloat64(value[i]);
288  }
289  bool ok = rpcPort.write(cmd, response);
290  return checkResponse(ok, response);
291 }
292 
294 {
295  Bottle cmd, response;
298  cmd.addInt32(ch);
299  bool ok = rpcPort.write(cmd, response);
300  return checkResponse(ok, response);
301 }
302 
303 int AnalogSensorClient::calibrateChannel(int ch, double value)
304 {
305  Bottle cmd, response;
308  cmd.addInt32(ch);
309  cmd.addFloat64(value);
310  bool ok = rpcPort.write(cmd, response);
311  return checkResponse(ok, response);
312 }
313 
315 {
316  return lastTs;
317 }
const int ANALOG_TIMEOUT
constexpr yarp::conf::vocab32_t VOCAB_IANALOG
Definition: IAnalogSensor.h:14
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE
constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_CHANNEL
bool ret
void removeLeadingTrailingSlashesOnly(std::string &name)
yarp::os::Stamp getLastInputStamp() override
Get the time stamp for the last read data.
bool close() override
Close the DeviceDriver.
int calibrateChannel(int ch) override
Calibrates one single channel.
int calibrateSensor() override
Calibrates the whole sensor.
int read(yarp::sig::Vector &out) override
Read a vector from the sensor.
int getState(int ch) override
Check the state value of a given channel.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
int getChannels() override
Get the number of channels of the sensor.
void onRead(yarp::sig::Vector &v) override
int getLast(yarp::sig::Vector &data, yarp::os::Stamp &stmp)
void getEstFrequency(int &ite, double &av, double &min, double &max)
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
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition: Bottle.cpp:182
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition: Bottle.cpp:158
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition: Bottle.cpp:140
A base class for nested structures that can be searched.
Definition: Searchable.h:63
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:21
double getTime() const
Get the time stamp.
Definition: Stamp.cpp:34
A single value (typically within a Bottle).
Definition: Value.h:43
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:204
virtual bool isInt32() const
Checks if value is a 32-bit integer.
Definition: Value.cpp:132
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
For streams capable of holding different kinds of content, check what they actually have.
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.
The main, catch-all namespace for YARP.
Definition: dirs.h:16