YARP
Yet Another Robot Platform
VirtualAnalogWrapper.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #include "VirtualAnalogWrapper.h"
10 #include <iostream>
11 #include <yarp/os/LogStream.h>
12 
13 using namespace std;
14 using namespace yarp::os;
15 using namespace yarp::dev;
16 using namespace yarp::dev::impl;
17 
18 #define MAX_ENTRIES 255
19 
20 // needed for the driver factory.
22  return new DriverCreatorOf<yarp::dev::VirtualAnalogWrapper>("virtualAnalogServer",
23  "virtualAnalogServer",
24  "yarp::dev::VirtualAnalogWrapper");
25 }
26 
27 
28 AnalogSubDevice::AnalogSubDevice() : lastRecvMsg(0)
29 {
30  detach();
31 }
32 
33 AnalogSubDevice::~AnalogSubDevice()
34 {
35  detach();
36 }
37 
38 bool AnalogSubDevice::configure(int map0, int map1, const std::string &key)
39 {
40  mIsConfigured=false;
41 
42  if (map1<map0)
43  {
44  yError()<<"VirtualAnalogWrapper: check configuration file top<base.";
45  return false;
46  }
47 
48  mMap0=map0;
49  mMap1=map1;
50 
51  mKey=key;
52 
53  mTorques.resize(mMap1-mMap0+1);
54 
55  mIsConfigured=true;
56 
57  return true;
58 }
59 
60 bool AnalogSubDevice::attach(yarp::dev::PolyDriver *device, const std::string &key)
61 {
62  if (key!=mKey)
63  {
64  yError()<<"VirtualAnalogWrapper: wrong device" << key.c_str();
65  return false;
66  }
67 
68  //configure first
69  if (!mIsConfigured)
70  {
71  yError()<<"VirtualAnalogWrapper: You need to call configure before you can attach any device";
72  return false;
73  }
74 
75  if (!device)
76  {
77  yError()<<"VirtualAnalogWrapper: Invalid device (null pointer)";
78  return false;
79  }
80 
81  mpDevice=device;
82 
83  if (mpDevice->isValid())
84  {
85  mpDevice->view(mpSensor);
86  }
87  else
88  {
89  yError()<<"VirtualAnalogWrapper: Invalid device " << key << " (isValid() returned false)";
90  return false;
91  }
92 
93  if (mpSensor)
94  {
95  mIsAttached=true;
96  return true;
97  }
98 
99  return false;
100 }
101 
102 void AnalogSubDevice::detach()
103 {
104  mMap0=mMap1=-1;
105 
106  mpDevice=nullptr;
107  mpSensor=nullptr;
108 
109  mIsConfigured=false;
110  mIsAttached=false;
111 }
112 
113 bool VirtualAnalogWrapper::open(Searchable& config)
114 {
115  yDebug() << config.toString().c_str();
116 
117  mIsVerbose = (config.check("verbose","if present, give detailed output"));
118 
119  if (mIsVerbose) yDebug() << "running with verbose output\n";
120 
121  //thus thread period is useful for output port... this input port has callback so maybe can skip it (?)
122  //thread_period = prop.check("threadrate", 20, "thread rate in ms. for streaming encoder data").asInt32();
123 
124  yDebug() << "Using VirtualAnalogServer\n";
125 
126  if (!config.check("networks", "list of networks merged by this wrapper"))
127  {
128  yError() << "VirtualAnalogWrapper: missing networks parameters";
129  return false;
130  }
131 
132  Bottle *networks=config.find("networks").asList();
133  mNSubdevs=networks->size();
134  mSubdevices.resize(mNSubdevs);
135 
136  mChan2Board.resize(MAX_ENTRIES);
137  mChan2BAddr.resize(MAX_ENTRIES);
138  for (int i=0; i< MAX_ENTRIES; i++)
139  {
140  mChan2Board[i]=-1;
141  mChan2BAddr[i]=-1;
142  }
143 
144  int totalJ=0;
145 
146  for (size_t k=0; k<networks->size(); ++k)
147  {
148  Bottle parameters=config.findGroup(networks->get(k).asString());
149 
150  if (parameters.size()!=5) // mapping joints using the paradigm: part from - to / network from - to
151  {
152  yError() << "VirtualAnalogWrapper: check network parameters in part description"
153  << " I was expecting " << networks->get(k).asString().c_str() << " followed by four integers";
154  return false;
155  }
156 
157  int map0=parameters.get(1).asInt32();
158  int map1=parameters.get(2).asInt32();
159  int map2=parameters.get(3).asInt32();
160  int map3=parameters.get(4).asInt32();
161  if (map0 >= MAX_ENTRIES || map1 >= MAX_ENTRIES || map2>= MAX_ENTRIES || map3>= MAX_ENTRIES ||
162  map0 <0 || map1 <0 || map2<0 || map3<0)
163  {
164  yError() << "VirtualAnalogWrapper: invalid map entries in networks section, failed initial check";
165  return false;
166  }
167 
168  for (int j=map0; j<=map1; ++j)
169  {
170  mChan2Board[j]=k;
171  mChan2BAddr[j]=j-map0+map2;
172  }
173 
174  if (!mSubdevices[k].configure(map2,map3,networks->get(k).asString()))
175  {
176  yError() << "VirtualAnalogWrapper: configure of subdevice ret false";
177  return false;
178  }
179 
180  totalJ+=map1-map0+1;
181  }
182 
183  // Verify minimum set of parameters required
184  if(!config.check("robotName") ) // ?? qui dentro, da dove lo pesco ??
185  {
186  yError() << "VirtualAnalogWrapper: missing robotName, check your configuration file!";
187  return false;
188  }
189 
190  if (config.check("deviceId"))
191  {
192  yError() << "VirtualAnalogWrapper: the parameter 'deviceId' has been deprecated, please use parameter 'name' instead. \n"
193  << "e.g. In the VFT wrapper configuration files of your robot, replace '<param name=""deviceId""> left_arm </param>' \n"
194  << "with '/icub/joint_vsens/left_arm:i' ";
195  return false;
196  }
197 
198  std::string port_name = config.check("name",Value("controlboard"),"Virtual analog wrapper port name, e.g. /icub/joint_vsens/left_arm:i").asString();
199  std::string robot_name = config.find("robotName").asString();
200 
201  if (!mPortInputTorques.open(port_name))
202  {
203  yError() << "VirtualAnalogWrapper: can't open port " << port_name.c_str();
204  return false;
205  }
206 
207  return true;
208 }
209 
210 bool VirtualAnalogWrapper::close()
211 {
212  mPortInputTorques.interrupt();
213  mPortInputTorques.close();
214  Thread::stop();
215  return true;
216 }
217 
218 bool VirtualAnalogWrapper::attachAll(const PolyDriverList &polylist)
219 {
220  mMutex.lock();
221 
222  for (int p=0; p<polylist.size(); ++p)
223  {
224  std::string key=polylist[p]->key;
225 
226  // find appropriate entry in list of subdevices and attach
227  for (auto& mSubdevice : mSubdevices)
228  {
229  if (mSubdevice.getKey() == key)
230  {
231  if (!mSubdevice.attach(polylist[p]->poly,key))
232  {
233  mMutex.unlock();
234  return false;
235  }
236  }
237  }
238  }
239 
240  //check if all devices are attached to the driver
241  for (auto& mSubdevice : mSubdevices)
242  {
243  if (!mSubdevice.isAttached())
244  {
245  mMutex.unlock();
246  return false;
247  }
248  }
249 
250  mMutex.unlock();
251 
252  Thread::start();
253 
254  return true;
255 }
256 
257 bool VirtualAnalogWrapper::detachAll()
258 {
259  mMutex.lock();
260 
261  for(int k=0; k<mNSubdevs; ++k)
262  {
263  mSubdevices[k].detach();
264  }
265 
266  mMutex.unlock();
267 
268 // close();
269 
270  return true;
271 }
272 
273 bool VirtualAnalogWrapper::perform_first_check(int elems)
274 {
275  if (first_check) return true;
276 
277  for (int i=0; i<elems; i++)
278  {
279  if (mChan2Board[i]==-1 || mChan2BAddr[i]==-1)
280  {
281  yError() << "VirtualAnalogWrapper: invalid map entries in networks section, failed runtime check"
282  << " i: " << i << "mChan2Board[i] is " << mChan2Board[i] << " chan2add is " << mChan2BAddr[i];
283  return false;
284  }
285  }
286 
287  yTrace() << "VirtualAnalogWrapper::perform_first_check() successfully completed";
288  first_check = true;
289  return true;
290 }
291 
292 void VirtualAnalogWrapper::run()
293 {
294  yarp::os::Bottle *pTorques;
295  bool sendLastValueBeforeTimeout = false;
296  while (!Thread::isStopping())
297  {
298  pTorques=mPortInputTorques.read(false);
299  double timeNow=Time::now();
300 
301  if (pTorques)
302  {
303  sendLastValueBeforeTimeout = false;
304  mMutex.lock();
305 
306  lastRecv=Time::now();
307  switch (pTorques->get(0).asInt32())
308  {
309  case 1: //arm torque message
310  if (perform_first_check(6)==false) break;
311  mSubdevices[mChan2Board[0]].setTorque(mChan2BAddr[0],pTorques->get(1).asFloat64()); //shoulder 1 pitch (0)
312  mSubdevices[mChan2Board[1]].setTorque(mChan2BAddr[1],pTorques->get(2).asFloat64()); //shoulder 2 roll (1)
313  mSubdevices[mChan2Board[2]].setTorque(mChan2BAddr[2],pTorques->get(3).asFloat64()); //shoulder 3 yaw (2)
314  mSubdevices[mChan2Board[3]].setTorque(mChan2BAddr[3],pTorques->get(4).asFloat64()); //elbow (3)
315  mSubdevices[mChan2Board[4]].setTorque(mChan2BAddr[4],pTorques->get(5).asFloat64()); //wrist pronosupination (4)
316  mSubdevices[mChan2Board[5]].setTorque(mChan2BAddr[5],0.0);
317  break;
318 
319  case 2: //legs torque message
320  if (perform_first_check(6)==false) break;
321  mSubdevices[mChan2Board[0]].setTorque(mChan2BAddr[0],pTorques->get(1).asFloat64()); //hip pitch
322  mSubdevices[mChan2Board[1]].setTorque(mChan2BAddr[1],pTorques->get(2).asFloat64()); //hip roll
323  mSubdevices[mChan2Board[2]].setTorque(mChan2BAddr[2],pTorques->get(3).asFloat64()); //hip yaw
324  mSubdevices[mChan2Board[3]].setTorque(mChan2BAddr[3],pTorques->get(4).asFloat64()); //knee
325  mSubdevices[mChan2Board[4]].setTorque(mChan2BAddr[4],pTorques->get(5).asFloat64()); //ankle pitch
326  mSubdevices[mChan2Board[5]].setTorque(mChan2BAddr[5],pTorques->get(6).asFloat64()); //ankle roll
327  break;
328 
329  case 3: //wrist torque message
330  if (perform_first_check(6)==false) break;
331  mSubdevices[mChan2Board[0]].setTorque(mChan2BAddr[0],pTorques->get(6).asFloat64()); //wrist yaw (6)
332  mSubdevices[mChan2Board[1]].setTorque(mChan2BAddr[1],pTorques->get(7).asFloat64()); //wrist pitch (7)
333  mSubdevices[mChan2Board[2]].setTorque(mChan2BAddr[2],0.0);
334  mSubdevices[mChan2Board[3]].setTorque(mChan2BAddr[3],0.0);
335  mSubdevices[mChan2Board[4]].setTorque(mChan2BAddr[4],0.0);
336  mSubdevices[mChan2Board[5]].setTorque(mChan2BAddr[5],0.0);
337  break;
338 
339  case 4: // torso
340  if (perform_first_check(3)==false) break;
341  mSubdevices[mChan2Board[0]].setTorque(mChan2BAddr[0],pTorques->get(1).asFloat64()); //torso yaw (respect gravity)
342  mSubdevices[mChan2Board[1]].setTorque(mChan2BAddr[1],pTorques->get(2).asFloat64()); //torso roll (lateral movement)
343  mSubdevices[mChan2Board[2]].setTorque(mChan2BAddr[2],pTorques->get(3).asFloat64()); //torso pitch (front-back movement)
344 // mSubdevices[mChan2Board[3]].setTorque(mChan2BAddr[3],0.0);
345 // mSubdevices[mChan2Board[4]].setTorque(mChan2BAddr[4],0.0);
346 // mSubdevices[mChan2Board[5]].setTorque(mChan2BAddr[5],0.0);
347  break;
348 
349  default:
350  yError() << "VirtualAnalogWrapper: got unexpected " << pTorques->get(0).asInt32() << " message on virtualAnalogServer.";
351  }
352 
353  for (int d=0; d<mNSubdevs; ++d)
354  {
355  mSubdevices[d].flushTorques();
356  }
357 
358  mMutex.unlock();
359  }
360  else
361  {
362  // sending rate from wholeBody is 10ms, if nothing is got now, wait that much time
363  yarp::os::Time::delay(0.001);
364  }
365 
366  if(first_check)
367  {
368  if ((lastRecv+0.080 < timeNow) && (!sendLastValueBeforeTimeout))
369  {
370  /* If 80ms have passed since the last received message, reset values to zero (just once).
371  * Sending time will be 1ms due to the delay above (else case).
372  */
373  for (int d=0; d<mNSubdevs; ++d)
374  {
375  mSubdevices[d].resetTorque();
376  mSubdevices[d].flushTorques();
377 
378  }
379  // Virtual Sensor status is not handled now because server DO NOT implement IVirtual AnalogSensor Interface.
380  // status=IAnalogSensor::AS_TIMEOUT;
381  yError() << "Virtual analog sensor timeout!! No new value received for more than " << timeNow - lastRecv << " secs.";
382  sendLastValueBeforeTimeout = true;
383  }
384  }
385  }
386 }
yarp::dev::DriverCreator * createVirtualAnalogWrapper()
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:228
A base class for nested structures that can be searched.
Definition: Searchable.h:68
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:207
STL namespace.
#define MAX_ENTRIES
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:77
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual std::string asString() const
Get string value.
Definition: Value.cpp:237
An interface for the device drivers.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
A factory for creating driver objects of a particular type.
Definition: Drivers.h:19
#define yTrace
Definition: Log.h:105
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:243
A simple collection of objects that can be described and transmitted in a portable way...
Definition: Bottle.h:72
A base class for factories that create driver objects.
Definition: Drivers.h:31
#define yError
Definition: Log.h:109
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
A single value (typically within a Bottle).
Definition: Value.h:46
void delay(double seconds)
Wait for a certain number of seconds.
Definition: Time.cpp:111
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:225
bool read(ConnectionReader &reader) override
Set the bottle&#39;s value based on input from a network connection.
Definition: Bottle.cpp:222
An interface to the operating system, including Port based communication.
A container for a device driver.
Definition: PolyDriver.h:25
#define yDebug
Definition: Log.h:106
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:233