YARP
Yet Another Robot Platform
Device.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7
11
12#include <yarp/os/LogStream.h>
13#include <yarp/os/Property.h>
14#include <yarp/os/Semaphore.h>
15
19#include <yarp/dev/IWrapper.h>
20#include <yarp/dev/PolyDriver.h>
21
22#include <string>
23
24
26{
27
28public:
29 struct Driver
30 {
32 driver(d),
33 ref(1)
34 {
35 }
40 int ref;
41 };
42
43 Private(Device* /*parent*/) :
44 driver(new Driver(new yarp::dev::PolyDriver()))
45 {
46 }
47
48 Private(const Private& other) :
49 driver(other.driver)
50 {
51 ++driver->ref;
52 }
53
54 Private& operator=(const Private& other)
55 {
56 YARP_UNUSED(other);
57 YARP_FIXME_NOTIMPLEMENTED("operator=")
58 return *this;
59 }
60
62 {
63 if (!--driver->ref) {
65
66 if (driver->driver->isValid()) {
67 if (!driver->driver->close()) {
68 yWarning() << "Cannot close device" << name;
69 }
70 }
71
72 delete driver->driver;
73 delete driver;
74 driver = nullptr;
75 }
76 }
77
78 inline yarp::dev::PolyDriver* drv() const
79 {
80 return driver->driver;
81 }
83 {
84 return &driver->runningThreads;
85 }
87 {
89 }
91 {
93 }
94
95 inline bool isValid() const
96 {
97 return drv()->isValid();
98 }
99 inline bool open()
100 {
102 return drv()->open(p);
103 }
104 inline bool close()
105 {
106 return drv()->close();
107 }
108
109 inline void registerThread(yarp::os::Thread* thread) const
110 {
111 reg_sem()->wait();
112 lst_sem()->wait();
113 thr()->push_back(thread);
114 reg_sem()->post();
115 lst_sem()->post();
116 }
117
118 inline void joinThreads() const
119 {
120 // The semafore will not allow other thread to be registered while
121 // joining threads.
122 // Other calls to joinThread() will stop at the semaphore and will
123 // be restarted as soon as all the threads are joined and the list
124 // is empty. This will avoid calling delete twice.
125 // stopThreads() must pass this semaphore, but in order to avoid to
126 // stop an already deleted thread we need a second semaphore.
127 reg_sem()->wait();
128 auto tit = thr()->begin();
129 while (tit != thr()->end()) {
130 yarp::os::Thread* thread = *tit;
131 thread->join();
132 lst_sem()->wait();
133 thr()->erase(tit++);
134 delete thread;
135 lst_sem()->post();
136 }
137 reg_sem()->post();
138 }
139
140 inline void stopThreads() const
141 {
142 lst_sem()->wait();
143 for (auto* thread : *thr()) {
144 thread->stop();
145 }
146 lst_sem()->post();
147 }
148
150 {
152
154 prop.put("device", type);
155 prop.put("id", name);
156
157 for (yarp::robotinterface::ParamList::const_iterator it = p.begin(); it != p.end(); ++it) {
158 const yarp::robotinterface::Param& param = *it;
159
160 // check if parentheses are balanced
161 std::string stringFormatValue = param.value();
162 int counter = 0;
163 for (size_t i = 0; i < stringFormatValue.size() && counter >= 0; i++) {
164 if (stringFormatValue[i] == '(') {
165 counter++;
166 } else if (stringFormatValue[i] == ')') {
167 counter--;
168 }
169 }
170 if (counter != 0) {
171 yWarning() << "Parentheses not balanced for param " << param.name();
172 }
173
174 std::string s = "(" + param.name() + " " + param.value() + ")";
175 prop.fromString(s, false);
176 }
177
178 return prop;
179 }
180
181 std::string name;
182 std::string type;
186};
187
189{
190 dbg << "(name = \"" << t.name() << "\", type = \"" << t.type() << "\"";
191 if (!t.params().empty()) {
192 dbg << ", params = [";
193 dbg << t.params();
194 dbg << "]";
195 }
196 if (!t.actions().empty()) {
197 dbg << ", actions = [";
198 dbg << t.actions();
199 dbg << "]";
200 }
201 dbg << ")";
202 return dbg;
203}
204
206 mPriv(new Private(this))
207{
208 mPriv->close();
209}
210
212 const std::string& type,
214 const yarp::robotinterface::ActionList& actions) :
215 mPriv(new Private(this))
216{
217 mPriv->name = name;
218 mPriv->type = type;
219 mPriv->params = params;
220 mPriv->actions = actions;
221}
222
224 mPriv(new Private(*other.mPriv))
225{
226 mPriv->name = other.mPriv->name;
227 mPriv->type = other.mPriv->type;
228 mPriv->params = other.mPriv->params;
229 mPriv->actions = other.mPriv->actions;
230 *mPriv->driver = *other.mPriv->driver;
231}
232
234{
235 if (&other != this) {
236 mPriv->name = other.mPriv->name;
237 mPriv->type = other.mPriv->type;
238
239 mPriv->params.clear();
240 mPriv->params = other.mPriv->params;
241
242 mPriv->actions.clear();
243 mPriv->actions = other.mPriv->actions;
244
245 *mPriv->driver = *other.mPriv->driver;
246 }
247 return *this;
248}
249
251{
252 delete mPriv;
253}
254
256{
257 return mPriv->name;
258}
259
261{
262 return mPriv->type;
263}
264
266{
267 return mPriv->params;
268}
269
271{
272 return mPriv->actions;
273}
274
275const std::string& yarp::robotinterface::Device::name() const
276{
277 return mPriv->name;
278}
279
280const std::string& yarp::robotinterface::Device::type() const
281{
282 return mPriv->type;
283}
284
286{
287 return mPriv->params;
288}
289
291{
292 return mPriv->actions;
293}
294
296{
297 if (mPriv->isValid()) {
298 yError() << "Trying to open an already opened device.";
299 return false;
300 }
301
302 if (!mPriv->open()) {
303 yWarning() << "Cannot open device" << mPriv->name;
304 return false;
305 }
306
307 return true;
308}
309
311{
312 if (!mPriv->isValid()) {
313 // Trying to close an already closed device. Perhaps open()
314 // Failed... Nothing to do and not worth sending an error.
315 return true;
316 }
317
318 if (!mPriv->close()) {
319 yWarning() << "Cannot close device" << mPriv->name;
320 return false;
321 }
322
323 return true;
324}
325
326bool yarp::robotinterface::Device::hasParam(const std::string& name) const
327{
328 return yarp::robotinterface::hasParam(mPriv->params, name);
329}
330
331std::string yarp::robotinterface::Device::findParam(const std::string& name) const
332{
333 return yarp::robotinterface::findParam(mPriv->params, name);
334}
335
337{
338 return mPriv->drv();
339}
340
342{
343 mPriv->registerThread(thread);
344}
345
347{
348 mPriv->joinThreads();
349}
350
352{
353 mPriv->stopThreads();
354}
355
357{
358 if (!driver()) {
359 yDebug() << "Device do not exists, cannot do " << ActionTypeToString(ActionTypeDetach) << "action";
360 return false;
361 }
362
363 yarp::dev::ICalibrator* calibrator;
364 if (!driver()->view(calibrator)) {
365 yError() << name() << "is not a yarp::dev::ICalibrator, therefore it cannot have" << ActionTypeToString(ActionTypeCalibrate) << "actions";
366 return false;
367 }
368
369 yarp::dev::IControlCalibration* controlCalibrator;
370 if (!target.poly->view(controlCalibrator)) {
371 yError() << target.key << "is not a yarp::dev::IControlCalibration2, therefore it cannot have" << ActionTypeToString(ActionTypeCalibrate) << "actions";
372 return false;
373 }
374
375 controlCalibrator->setCalibrator(calibrator);
376
377 // Saving pointer to Calibrator device into Wrapper to later use
378 // (NOTE this make sense if the target device is a controlBoard_nws_yarp, as it should be)
379 yarp::dev::IRemoteCalibrator* rem_calibrator_wrap;
380 yarp::dev::IRemoteCalibrator* rem_calibrator_calib;
381 bool rem_calibrator_available = true;
382
383 if (!target.poly->view(rem_calibrator_wrap)) {
384 yWarning() << "Device " << target.key << "is not implementing a yarp::dev::IRemoteCalibrator, therefore it cannot attach to a Calibrator device. \n \
385 Please verify that the target of calibrate action is a controlBoard_nws_yarp device. \
386 This doesn't prevent the yarprobotinterface to correctly operate, but no remote calibration and homing will be available";
387 rem_calibrator_available = false;
388 }
389
390 if ((rem_calibrator_available) && (!driver()->view(rem_calibrator_calib))) {
391 yWarning() << "Device " << name() << "is not implementing a yarp::dev::IRemoteCalibrator, therefore it cannot be used as a remote calibration device. \n \
392 This doesn't prevent the yarprobotinterface to correctly operate, but no remote calibration and homing will be available";
393 rem_calibrator_available = false;
394 }
395
396 if (rem_calibrator_available) {
397 rem_calibrator_wrap->setCalibratorDevice(rem_calibrator_calib);
398 }
399
400 // Start the calibrator thread
401 yarp::os::Thread* calibratorThread = new yarp::robotinterface::impl::CalibratorThread(calibrator,
402 name(),
403 target.poly,
404 target.key,
406 registerThread(calibratorThread);
407
408 if (!calibratorThread->start()) {
409 yError() << "Device" << name() << "cannot execute" << ActionTypeToString(ActionTypeCalibrate) << "on device" << target.key;
410 return false;
411 }
412
413 return true;
414}
415
417{
418 if (!driver()) {
419 yDebug() << "Device do not exists, cannot do " << ActionTypeToString(ActionTypeDetach) << "action";
420 return false;
421 }
422
423 yarp::dev::IMultipleWrapper* multiplewrapper;
424 driver()->view(multiplewrapper);
425
426 if (drivers.size() == 1) {
427 yarp::dev::IWrapper* wrapper;
428 if (!driver()->view(wrapper)) {
429 yInfo() << name() << "is not an IWrapper. Trying IMultipleWrapper";
430 } else if (wrapper->attach(drivers[0]->poly)) {
431 return true;
432 } else if (!multiplewrapper) {
433 yError() << "Device" << name() << "cannot execute" << ActionTypeToString(ActionTypeAttach);
434 return false;
435 } else {
436 yInfo() << name() << "IWrapper::attach() failed. Trying IMultipleWrapper::attach().";
437 }
438 }
439
440 if (!multiplewrapper) {
441 yError() << name() << "is not a multiplewrapper, therefore it cannot have" << ActionTypeToString(ActionTypeAttach) << "actions";
442 return false;
443 }
444
445 if (!multiplewrapper->attachAll(drivers)) {
446 yError() << "Device" << name() << "cannot execute" << ActionTypeToString(ActionTypeAttach);
447 return false;
448 }
449
450 return true;
451}
452
454{
455 if (!driver()) {
456 yDebug() << "Device do not exists, cannot do " << ActionTypeToString(ActionTypeDetach) << "action";
457 return false;
458 }
459
460 yarp::dev::IWrapper* wrapper;
461 yarp::dev::IMultipleWrapper* multiplewrapper;
462 driver()->view(wrapper);
463 driver()->view(multiplewrapper);
464
465 if (!wrapper && !multiplewrapper) {
466 yError() << name() << "is neither a wrapper nor a multiplewrapper, therefore it cannot have" << ActionTypeToString(ActionTypeDetach) << "actions";
467 return false;
468 }
469
470 if (multiplewrapper) {
471 if (multiplewrapper->detachAll()) {
472 return true;
473 }
474 if (!wrapper) {
475 yError() << "Device" << name() << "cannot execute" << ActionTypeToString(ActionTypeDetach);
476 return false;
477 }
478 yInfo() << name() << "IMultipleWrapper::detachAll() failed. Trying IWrapper::detach().";
479 }
480
481 if (wrapper && !wrapper->detach()) {
482 yError() << "Device" << name() << "cannot execute" << ActionTypeToString(ActionTypeDetach);
483 return false;
484 }
485
486 return true;
487}
488
490{
491 yarp::dev::ICalibrator* calibrator;
492 if (!driver()) {
493 yDebug() << "Device do not exists, cannot do " << ActionTypeToString(ActionTypeDetach) << "action";
494 return false;
495 }
496
497 if (!driver()->isValid()) {
498 yError() << "park device do not exists";
499 return false;
500 }
501
502 if (!driver()->view(calibrator)) {
503 yError() << name() << "is not a yarp::dev::ICalibrator, therefore it cannot have" << ActionTypeToString(ActionTypePark) << "actions";
504 return false;
505 }
506
507 yarp::dev::IControlCalibration* controlCalibrator;
508 if (!target.poly->view(controlCalibrator)) {
509 yError() << target.key << "is not a yarp::dev::IControlCalibration2, therefore it cannot have" << ActionTypeToString(ActionTypePark) << "actions";
510 return false;
511 }
512
513 controlCalibrator->setCalibrator(calibrator); // TODO Check if this should be removed
514
516 name(),
517 target.poly,
518 target.key,
520 registerThread(parkerThread);
521
522 if (!parkerThread->start()) {
523 yError() << "Device" << name() << "cannot execute" << ActionTypeToString(ActionTypePark) << "on device" << target.key;
524 return false;
525 }
526
527 return true;
528}
define control board standard interfaces
yarp::os::LogStream operator<<(yarp::os::LogStream dbg, const yarp::robotinterface::Device &t)
Definition: Device.cpp:188
float t
#define yInfo(...)
Definition: Log.h:314
#define yError(...)
Definition: Log.h:356
#define YARP_FIXME_NOTIMPLEMENTED(what)
Definition: Log.h:406
#define yDebug(...)
Definition: Log.h:270
#define yWarning(...)
Definition: Log.h:335
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
Interface for control devices, calibration commands.
virtual bool setCalibrator(ICalibrator *c)
Set the calibrator object to be used to calibrate the robot.
Interface for an object that can wrap/attach to to another.
virtual bool detachAll()=0
Detach the object (you must have first called attach).
virtual bool attachAll(const PolyDriverList &drivers)=0
Attach to a list of objects.
IRemoteCalibrator interface is meant to remotize the access of the calibration device in order to all...
virtual bool setCalibratorDevice(yarp::dev::IRemoteCalibrator *dev)
setCalibratorDevice: store the pointer to the calibrator device.
Interface for an object that can wrap/or "attach" to another.
Definition: IWrapper.h:25
virtual bool detach()=0
Detach the object (you must have first called attach).
virtual bool attach(PolyDriver *driver)=0
Attach to another object.
A container for a device driver.
Definition: PolyDriver.h:23
bool close() override
Close the DeviceDriver.
Definition: PolyDriver.cpp:173
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
bool open(const std::string &txt)
Construct and configure a device by its common name.
Definition: PolyDriver.cpp:140
A class for storing options and configuration information.
Definition: Property.h:33
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1063
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
A class for thread synchronization and mutual exclusion.
Definition: Semaphore.h:25
void wait()
Decrement the counter, even if we must wait to do that.
Definition: Semaphore.cpp:96
void post()
Increment the counter.
Definition: Semaphore.cpp:111
An abstraction for a thread of execution.
Definition: Thread.h:21
bool join(double seconds=-1)
The function returns when the thread execution has completed.
Definition: Thread.cpp:76
bool start()
Start the new thread running.
Definition: Thread.cpp:93
Private & operator=(const Private &other)
Definition: Device.cpp:54
yarp::os::Semaphore * reg_sem() const
Definition: Device.cpp:86
yarp::dev::PolyDriver * drv() const
Definition: Device.cpp:78
Private(const Private &other)
Definition: Device.cpp:48
yarp::os::Property paramsAsProperty() const
Definition: Device.cpp:149
yarp::os::Semaphore * lst_sem() const
Definition: Device.cpp:90
void registerThread(yarp::os::Thread *thread) const
Definition: Device.cpp:109
yarp::robotinterface::ThreadList * thr() const
Definition: Device.cpp:82
ActionList & actions()
Definition: Device.cpp:270
bool attach(const yarp::dev::PolyDriverList &drivers) const
Definition: Device.cpp:416
bool park(const yarp::dev::PolyDriverDescriptor &target) const
Definition: Device.cpp:489
yarp::dev::PolyDriver * driver() const
Definition: Device.cpp:336
bool hasParam(const std::string &name) const
Definition: Device.cpp:326
std::string & type()
Definition: Device.cpp:260
std::string & name()
Definition: Device.cpp:255
bool calibrate(const yarp::dev::PolyDriverDescriptor &target) const
Definition: Device.cpp:356
Device & operator=(const Device &other)
Definition: Device.cpp:233
std::string findParam(const std::string &name) const
Definition: Device.cpp:331
void registerThread(yarp::os::Thread *thread) const
Definition: Device.cpp:341
std::string & name()
Definition: Param.cpp:80
std::string & value()
Definition: Param.cpp:85
bool isValid()
Check if time is valid (non-zero).
Definition: Time.cpp:314
robotinterface::ParamList mergeDuplicateGroups(const robotinterface::ParamList &list)
Definition: Types.cpp:58
std::vector< robotinterface::Action > ActionList
Definition: Types.h:31
std::string ActionTypeToString(robotinterface::ActionType actiontype)
Definition: Types.cpp:147
bool hasParam(const robotinterface::ParamList &list, const std::string &name)
Definition: Types.cpp:16
std::vector< robotinterface::Param > ParamList
Definition: Types.h:30
std::string findParam(const robotinterface::ParamList &list, const std::string &name)
Definition: Types.cpp:26
std::list< yarp::os::Thread * > ThreadList
Definition: Types.h:33
The main, catch-all namespace for YARP.
Definition: dirs.h:16
Driver(yarp::dev::PolyDriver *d)
Definition: Device.cpp:31
#define YARP_UNUSED(var)
Definition: api.h:162