YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
MultipleAnalogSensorsClient.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
10
12
13namespace {
14YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSCLIENT, "yarp.device.multipleanalogsensorsclient")
15}
16
24
26{
28 {
29 double now = yarp::os::Time::now();
31 {
33 "No data received in the last %lf seconds, timeout enabled.",
36 }
37 }
38}
39
41{
42 if (!parseParams(config)) { return false; }
43
44 m_streamingPort.timeoutInSeconds = m_timeout;
45
46 std::string localRPCPortName = m_local + "/rpc:i";
47 std::string localStreamingPortName = m_local + "/measures:i";
48 std::string remoteRPCPortName = m_remote + "/rpc:o";
49 std::string remoteStreamingPortName = m_remote + "/measures:o";
50
51 // TODO(traversaro) : as soon as the method for checking port names validity
52 // are available in YARP ( https://github.com/robotology/yarp/pull/1508 ) add some checks
53
54 // Open ports
55 bool ok = m_rpcPort.open(localRPCPortName);
56 if (!ok)
57 {
58 yCError(MULTIPLEANALOGSENSORSCLIENT,
59 "Failure to open the port %s.",
60 localRPCPortName.c_str());
61 close();
62 return false;
63 }
64
65 ok = m_streamingPort.open(localStreamingPortName);
66 m_streamingPort.useCallback();
67 if (!ok)
68 {
69 yCError(MULTIPLEANALOGSENSORSCLIENT,
70 "Failure to open the port %s.",
71 localStreamingPortName.c_str());
72 close();
73 return false;
74 }
75
76 // Connect ports
78 // RPC port needs to be tcp, therefore no carrier option is added here
79 ok = yarp::os::Network::connect(localRPCPortName, remoteRPCPortName);
80 if (!ok) {
81 yCError(MULTIPLEANALOGSENSORSCLIENT,
82 "Failure connecting port %s to %s.",
83 localRPCPortName.c_str(),
84 remoteRPCPortName.c_str());
85 yCError(MULTIPLEANALOGSENSORSCLIENT, "Check that the specified MultipleAnalogSensorsServer is up.");
86 close();
87 return false;
88 }
89
90 ok = yarp::os::Network::connect(remoteStreamingPortName, localStreamingPortName, m_carrier);
91 if (!ok) {
92 yCError(MULTIPLEANALOGSENSORSCLIENT,
93 "Failure connecting port %s to %s.",
94 remoteStreamingPortName.c_str(),
95 localStreamingPortName.c_str());
96 yCError(MULTIPLEANALOGSENSORSCLIENT, "Check that the specified MultipleAnalogSensorsServer is up.");
97 close();
98 return false;
99 }
100
101 // Once the connection is active, we just the metadata only once
102 ok = m_RPCInterface.yarp().attachAsClient(m_rpcPort);
103 if (!ok) {
104 yCError(MULTIPLEANALOGSENSORSCLIENT, "Failure opening Thrift-based RPC interface.");
105 return false;
106 }
107
108 // TODO(traversaro): there is a limitation on the thrift-generated
109 // YARP structures related to how to get connection errors during the call
110 // If this is ever solved at YARP level, we should properly handle errors
111 // here
112 m_sensorsMetadata = m_RPCInterface.getMetadata();
113
114 }
115
116 yCDebug(MULTIPLEANALOGSENSORSCLIENT,"Open complete");
117 return true;
118}
119
121{
122 m_streamingPort.close();
123 m_rpcPort.close();
124
125 yCDebug(MULTIPLEANALOGSENSORSCLIENT, "Close complete");
126 return true;
127}
128
129size_t MultipleAnalogSensorsClient::genericGetNrOfSensors(const std::vector<SensorMetadata>& metadataVector,
130 const SensorMeasurements& measurementsVector) const
131{
133 return metadataVector.size();
134 } else {
135 std::lock_guard<std::mutex> guard(m_streamingPort.dataMutex);
136 m_streamingPort.updateTimeoutStatus();
137 if (m_streamingPort.status == yarp::dev::MAS_OK) {
138 return measurementsVector.measurements.size();
139 } else {
140 return 0;
141 }
142 }
143}
144
145yarp::dev::MAS_status MultipleAnalogSensorsClient::genericGetStatus() const
146{
147 std::lock_guard<std::mutex> guard(m_streamingPort.dataMutex);
148 m_streamingPort.updateTimeoutStatus();
149 return m_streamingPort.status;
150}
151
152bool MultipleAnalogSensorsClient::genericGetName(const std::vector<SensorMetadata>& metadataVector, const std::string& tag,
153 size_t sens_index, std::string& name) const
154{
156 yCError(MULTIPLEANALOGSENSORSCLIENT,
157 "Missing metadata, the device has been configured with the option"
158 "externalConnection set to true.");
159 return false;
160 }
161 if (sens_index >= metadataVector.size())
162 {
163 yCError(MULTIPLEANALOGSENSORSCLIENT,
164 "No sensor of type %s with index %lu (nr of sensors: %lu).",
165 tag.c_str(),
166 sens_index,
167 metadataVector.size());
168 return false;
169 }
170
171 name = metadataVector[sens_index].name;
172 return true;
173}
174
175bool MultipleAnalogSensorsClient::genericGetFrameName(const std::vector<SensorMetadata>& metadataVector, const std::string& tag,
176 size_t sens_index, std::string& frameName) const
177{
179 yCError(MULTIPLEANALOGSENSORSCLIENT,
180 "Missing metadata, the device has been configured with the option"
181 "externalConnection set to true.");
182 return false;
183 }
184 if (sens_index >= metadataVector.size())
185 {
186 yCError(MULTIPLEANALOGSENSORSCLIENT,
187 "No sensor of type %s with index %lu (nr of sensors: %lu).",
188 tag.c_str(),
189 sens_index,
190 metadataVector.size());
191 return false;
192 }
193
194 frameName = metadataVector[sens_index].frameName;
195 return true;
196}
197
198bool MultipleAnalogSensorsClient::genericGetMeasure(const std::vector<SensorMetadata>& metadataVector, const std::string& tag,
199 const SensorMeasurements& measurementsVector,
200 size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
201{
202
203 std::lock_guard<std::mutex> guard(m_streamingPort.dataMutex);
204 m_streamingPort.updateTimeoutStatus();
205 if (m_streamingPort.status != yarp::dev::MAS_OK)
206 {
207 yCError(MULTIPLEANALOGSENSORSCLIENT,
208 "Sensor of type %s with index %lu has non-MAS_OK status.",
209 tag.c_str(),
210 sens_index);
211 return false;
212 }
213
214 if (m_streamingPort.status != (sens_index >= measurementsVector.measurements.size()))
215 {
216 yCError(MULTIPLEANALOGSENSORSCLIENT,
217 "No sensor of type %s with index %lu (nr of sensors: %lu).",
218 tag.c_str(),
219 sens_index,
220 metadataVector.size());
221 return false;
222 }
223
225 assert(metadataVector.size() == measurementsVector.measurements.size());
226 }
227
228 timestamp = measurementsVector.measurements[sens_index].timestamp;
229 out = measurementsVector.measurements[sens_index].measurement;
230
231 return true;
232}
233
234size_t MultipleAnalogSensorsClient::genericGetSize(const std::vector<SensorMetadata>& metadataVector,
235 const std::string& tag, const SensorMeasurements& measurementsVector, size_t sens_index) const
236{
237 std::lock_guard<std::mutex> guard(m_streamingPort.dataMutex);
238 if (m_streamingPort.status != yarp::dev::MAS_OK)
239 {
240 yCError(MULTIPLEANALOGSENSORSCLIENT, "No data received, no information on the size of the specified sensor.");
241 return 0;
242 }
243
244
245 if (sens_index >= measurementsVector.measurements.size())
246 {
247 yCError(MULTIPLEANALOGSENSORSCLIENT,
248 "No sensor of type %s with index %lu (nr of sensors: %lu).",
249 tag.c_str(),
250 sens_index,
251 metadataVector.size());
252 return 0;
253 }
254
255 return measurementsVector.measurements[sens_index].measurement.size();
256}
257
258/*
259All the sensor specific methods (excluding the IOrientationSensor and the ISkinPatches) are just
260an instantiation of the following template (note: we avoid code generation for the sake of readability):
261
262{{SensorTag}} : ThreeAxisGyroscopes, ThreeAxisLinearAccelerometers, etc
263{{SensorSingular}} : ThreeAxisGyroscope, ThreeAxisLinearAccelerometer, etc
264
265size_t MultipleAnalogSensorsClient::getNrOf{{SensorTag}}() const
266{
267 return genericGetNrOfSensors(m_sensorsMetadata.{{SensorTag}});
268}
269
270MAS_status MultipleAnalogSensorsClient::get{{SensorSingular}}Status(size_t sens_index) const
271{
272 return genericGetStatus();
273}
274
275bool MultipleAnalogSensorsClient::get{{SensorSingular}}Name(size_t sens_index, std::string& name) const
276{
277 return genericGetName(m_sensorsMetadata.{{SensorTag}}, "{{SensorTag}}", sens_index, name);
278}
279
280bool MultipleAnalogSensorsClient::get{{SensorSingular}}Measure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
281{
282 return genericGetMeasure(m_sensorsMetadata.{{SensorTag}}, "{{SensorTag}}",
283 m_streamingPort.receivedData.{{SensorTag}}, sens_index, out, timestamp);
284}
285
286For the sensors (EncoderArray and SkinPatch) of which the measurements can change size, we also have:
287size_t MultipleAnalogSensorsClient::get{{SensorSingular}}Size(size_t sens_index) const
288{
289 return genericGetSize({{SensorTag}}, sens_index, m_i{{SensorTag}}, &I{{SensorTag}}::get{{SensorTag}}Size);
290}
291
292*/
293
295{
296 return genericGetNrOfSensors(m_sensorsMetadata.ThreeAxisGyroscopes,
297 m_streamingPort.receivedData.ThreeAxisGyroscopes);
298}
299
301{
302 return genericGetStatus();
303}
304
305bool MultipleAnalogSensorsClient::getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const
306{
307 return genericGetName(m_sensorsMetadata.ThreeAxisGyroscopes, "ThreeAxisGyroscopes", sens_index, name);
308}
309
310bool MultipleAnalogSensorsClient::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const
311{
312 return genericGetFrameName(m_sensorsMetadata.ThreeAxisGyroscopes, "ThreeAxisGyroscopes", sens_index, frameName);
313}
314
315bool MultipleAnalogSensorsClient::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
316{
317 return genericGetMeasure(m_sensorsMetadata.ThreeAxisGyroscopes, "ThreeAxisGyroscopes",
318 m_streamingPort.receivedData.ThreeAxisGyroscopes, sens_index, out, timestamp);
319}
320
322{
323 return genericGetNrOfSensors(m_sensorsMetadata.ThreeAxisLinearAccelerometers,
325}
326
328{
329 return genericGetStatus();
330}
331
332bool MultipleAnalogSensorsClient::getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const
333{
334 return genericGetName(m_sensorsMetadata.ThreeAxisLinearAccelerometers, "ThreeAxisLinearAccelerometers", sens_index, name);
335}
336
337bool MultipleAnalogSensorsClient::getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const
338{
339 return genericGetFrameName(m_sensorsMetadata.ThreeAxisLinearAccelerometers, "ThreeAxisLinearAccelerometers", sens_index, frameName);
340}
341
343{
344 return genericGetMeasure(m_sensorsMetadata.ThreeAxisLinearAccelerometers, "ThreeAxisLinearAccelerometers",
345 m_streamingPort.receivedData.ThreeAxisLinearAccelerometers, sens_index, out, timestamp);
346}
347
349{
350 return genericGetNrOfSensors(m_sensorsMetadata.ThreeAxisAngularAccelerometers,
352}
353
355{
356 return genericGetStatus();
357}
358
359bool MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const
360{
361 return genericGetName(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers", sens_index, name);
362}
363
364bool MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const
365{
366 return genericGetFrameName(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers", sens_index, frameName);
367}
368
370{
371 return genericGetMeasure(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers",
372 m_streamingPort.receivedData.ThreeAxisAngularAccelerometers, sens_index, out, timestamp);
373}
374
376{
377 return genericGetNrOfSensors(m_sensorsMetadata.ThreeAxisMagnetometers,
378 m_streamingPort.receivedData.ThreeAxisMagnetometers);
379}
380
382{
383 return genericGetStatus();
384}
385
386bool MultipleAnalogSensorsClient::getThreeAxisMagnetometerName(size_t sens_index, std::string& name) const
387{
388 return genericGetName(m_sensorsMetadata.ThreeAxisMagnetometers, "ThreeAxisMagnetometers", sens_index, name);
389}
390
391bool MultipleAnalogSensorsClient::getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const
392{
393 return genericGetFrameName(m_sensorsMetadata.ThreeAxisMagnetometers, "ThreeAxisMagnetometers", sens_index, frameName);
394}
395
396bool MultipleAnalogSensorsClient::getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
397{
398 return genericGetMeasure(m_sensorsMetadata.ThreeAxisMagnetometers, "ThreeAxisMagnetometers",
399 m_streamingPort.receivedData.ThreeAxisMagnetometers, sens_index, out, timestamp);
400}
401
403{
404 return genericGetNrOfSensors(m_sensorsMetadata.OrientationSensors,
405 m_streamingPort.receivedData.OrientationSensors);
406}
407
409{
410 return genericGetStatus();
411}
412
413bool MultipleAnalogSensorsClient::getOrientationSensorName(size_t sens_index, std::string& name) const
414{
415 return genericGetName(m_sensorsMetadata.OrientationSensors, "OrientationSensors", sens_index, name);
416}
417
418bool MultipleAnalogSensorsClient::getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const
419{
420 return genericGetFrameName(m_sensorsMetadata.OrientationSensors, "OrientationSensors", sens_index, frameName);
421}
422
424{
425 return genericGetMeasure(m_sensorsMetadata.OrientationSensors, "OrientationSensors",
426 m_streamingPort.receivedData.OrientationSensors, sens_index, out, timestamp);
427}
428
430{
431 return genericGetNrOfSensors(m_sensorsMetadata.PositionSensors,
432 m_streamingPort.receivedData.PositionSensors);
433}
434
436{
437 return genericGetStatus();
438}
439
440bool MultipleAnalogSensorsClient::getPositionSensorName(size_t sens_index, std::string& name) const
441{
442 return genericGetName(m_sensorsMetadata.PositionSensors, "PositionSensors", sens_index, name);
443}
444
445bool MultipleAnalogSensorsClient::getPositionSensorFrameName(size_t sens_index, std::string& frameName) const
446{
447 return genericGetFrameName(m_sensorsMetadata.PositionSensors, "PositionSensors", sens_index, frameName);
448}
449
450bool MultipleAnalogSensorsClient::getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
451{
452 return genericGetMeasure(m_sensorsMetadata.PositionSensors, "PositionSensors", m_streamingPort.receivedData.PositionSensors, sens_index, out, timestamp);
453}
454
456{
457 return genericGetNrOfSensors(m_sensorsMetadata.LinearVelocitySensors,
458 m_streamingPort.receivedData.LinearVelocitySensors);
459}
460
462{
463 return genericGetStatus();
464}
465
466bool MultipleAnalogSensorsClient::getLinearVelocitySensorName(size_t sens_index, std::string& name) const
467{
468 return genericGetName(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", sens_index, name);
469}
470
471bool MultipleAnalogSensorsClient::getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const
472{
473 return genericGetFrameName(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", sens_index, frameName);
474}
475
476bool MultipleAnalogSensorsClient::getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
477{
478 return genericGetMeasure(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", m_streamingPort.receivedData.LinearVelocitySensors, sens_index, out, timestamp);
479}
480
482{
483 return genericGetNrOfSensors(m_sensorsMetadata.TemperatureSensors,
484 m_streamingPort.receivedData.TemperatureSensors);
485}
486
488{
489 return genericGetStatus();
490}
491
492bool MultipleAnalogSensorsClient::getTemperatureSensorName(size_t sens_index, std::string& name) const
493{
494 return genericGetName(m_sensorsMetadata.TemperatureSensors, "TemperatureSensors", sens_index, name);
495}
496
497bool MultipleAnalogSensorsClient::getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const
498{
499 return genericGetFrameName(m_sensorsMetadata.TemperatureSensors, "TemperatureSensors", sens_index, frameName);
500}
501
502bool MultipleAnalogSensorsClient::getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
503{
504 return genericGetMeasure(m_sensorsMetadata.TemperatureSensors, "TemperatureSensors",
505 m_streamingPort.receivedData.TemperatureSensors, sens_index, out, timestamp);
506}
507
508bool MultipleAnalogSensorsClient::getTemperatureSensorMeasure(size_t sens_index, double& out, double& timestamp) const
509{
510 yarp::sig::Vector dummy(1);
511 bool ok = this->getTemperatureSensorMeasure(sens_index, dummy, timestamp);
512 out = dummy[0];
513 return ok;
514}
515
517{
518 return genericGetNrOfSensors(m_sensorsMetadata.SixAxisForceTorqueSensors,
520}
521
523{
524 return genericGetStatus();
525}
526
527bool MultipleAnalogSensorsClient::getSixAxisForceTorqueSensorName(size_t sens_index, std::string& name) const
528{
529 return genericGetName(m_sensorsMetadata.SixAxisForceTorqueSensors, "SixAxisForceTorqueSensors", sens_index, name);
530}
531
532bool MultipleAnalogSensorsClient::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const
533{
534 return genericGetFrameName(m_sensorsMetadata.SixAxisForceTorqueSensors, "SixAxisForceTorqueSensors", sens_index, frameName);
535}
536
537bool MultipleAnalogSensorsClient::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
538{
539 return genericGetMeasure(m_sensorsMetadata.SixAxisForceTorqueSensors, "SixAxisForceTorqueSensors",
540 m_streamingPort.receivedData.SixAxisForceTorqueSensors, sens_index, out, timestamp);
541}
542
544{
545 return genericGetNrOfSensors(m_sensorsMetadata.ContactLoadCellArrays,
546 m_streamingPort.receivedData.ContactLoadCellArrays);
547}
548
550{
551 return genericGetStatus();
552}
553
554bool MultipleAnalogSensorsClient::getContactLoadCellArrayName(size_t sens_index, std::string& name) const
555{
556 return genericGetName(m_sensorsMetadata.ContactLoadCellArrays, "ContactLoadCellArrays", sens_index, name);
557}
558
559bool MultipleAnalogSensorsClient::getContactLoadCellArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
560{
561 return genericGetMeasure(m_sensorsMetadata.ContactLoadCellArrays, "ContactLoadCellArrays",
562 m_streamingPort.receivedData.ContactLoadCellArrays, sens_index, out, timestamp);
563}
564
566{
567 return genericGetSize(m_sensorsMetadata.ContactLoadCellArrays, "ContactLoadCellArrays",
568 m_streamingPort.receivedData.ContactLoadCellArrays, sens_index);
569}
570
572{
573 return genericGetNrOfSensors(m_sensorsMetadata.EncoderArrays,
574 m_streamingPort.receivedData.EncoderArrays);
575}
576
578{
579 return genericGetStatus();
580}
581
582bool MultipleAnalogSensorsClient::getEncoderArrayName(size_t sens_index, std::string& name) const
583{
584 return genericGetName(m_sensorsMetadata.EncoderArrays, "EncoderArrays", sens_index, name);
585}
586
587bool MultipleAnalogSensorsClient::getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
588{
589 return genericGetMeasure(m_sensorsMetadata.EncoderArrays, "EncoderArrays",
590 m_streamingPort.receivedData.EncoderArrays, sens_index, out, timestamp);
591}
592
594{
595 return genericGetSize(m_sensorsMetadata.EncoderArrays, "EncoderArrays",
596 m_streamingPort.receivedData.EncoderArrays, sens_index);
597}
598
600{
601 return genericGetNrOfSensors(m_sensorsMetadata.SkinPatches,
602 m_streamingPort.receivedData.SkinPatches);
603}
604
606{
607 return genericGetStatus();
608}
609
610bool MultipleAnalogSensorsClient::getSkinPatchName(size_t sens_index, std::string& name) const
611{
612 return genericGetName(m_sensorsMetadata.SkinPatches, "SkinPatches", sens_index, name);
613}
614
615bool MultipleAnalogSensorsClient::getSkinPatchMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
616{
617 return genericGetMeasure(m_sensorsMetadata.SkinPatches, "SkinPatches",
618 m_streamingPort.receivedData.SkinPatches, sens_index, out, timestamp);
619}
620
622{
623 return genericGetSize(m_sensorsMetadata.SkinPatches, "SkinPatches",
624 m_streamingPort.receivedData.SkinPatches, sens_index);
625}
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::MAS_status getContactLoadCellArrayStatus(size_t sens_index) const override
Get the status of the specified sensor.
size_t getNrOfPositionSensors() const override
Get the number of position sensors exposed by this device.
yarp::dev::MAS_status getPositionSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getPositionSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getSkinPatchName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
size_t getNrOfLinearVelocitySensors() const override
Get the number of linear velocity sensors exposed by this device.
yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getEncoderArrayName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
bool getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double &timestamp) const override
Get the last reading of the position sensor as x y z.
yarp::dev::MAS_status getSkinPatchStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
size_t getNrOfTemperatureSensors() const override
Get the number of temperature sensors exposed by this device.
size_t getNrOfThreeAxisAngularAccelerometers() const override
Get the number of three axis angular accelerometers exposed by this device.
bool getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double &timestamp) const override
Get the last reading of the linear velocity sensor as x y z.
size_t getEncoderArraySize(size_t sens_index) const override
Get the size of the specified encoder array.
size_t getNrOfEncoderArrays() const override
Get the number of encoder arrays exposed by this device.
bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
bool getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getLinearVelocitySensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Get the status of the specified sensor.
size_t getContactLoadCellArraySize(size_t sens_index) const override
Get the size of the specified contact load cell array.
size_t getNrOfSixAxisForceTorqueSensors() const override
Get the number of six axis force torque sensors exposed by this device.
bool getTemperatureSensorMeasure(size_t sens_index, double &out, double &timestamp) const override
Get the last reading of the specified sensor.
size_t getSkinPatchSize(size_t sens_index) const override
Get the size of the specified skin patch.
size_t getNrOfThreeAxisLinearAccelerometers() const override
Get the number of three axis linear accelerometers exposed by this device.
yarp::dev::MAS_status getLinearVelocitySensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool close() override
Close the DeviceDriver.
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getLinearVelocitySensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double &timestamp) const override
Get the last reading of the orientation sensor as roll pitch yaw.
bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the gyroscope.
bool getContactLoadCellArrayName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
yarp::dev::MAS_status getEncoderArrayStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getContactLoadCellArrayMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getOrientationSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getPositionSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getTemperatureSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frame) const override
Get the name of the frame of the specified sensor.
size_t getNrOfThreeAxisGyroscopes() const override
Get the number of three axis gyroscopes exposed by this sensor.
size_t getNrOfThreeAxisMagnetometers() const override
Get the number of magnetometers exposed by this device.
size_t getNrOfOrientationSensors() const override
Get the number of orientation sensors exposed by this device.
yarp::dev::MAS_status getTemperatureSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getSkinPatchMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
size_t getNrOfSkinPatches() const override
Get the number of skin patches exposed by this device.
size_t getNrOfContactLoadCellArrays() const override
Get the number of contact load cell array exposed by this device.
bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
virtual SensorRPCData getMetadata()
Read the sensor metadata necessary to configure the MultipleAnalogSensorsClient device.
std::vector< SensorMeasurement > measurements
std::vector< SensorMetadata > SkinPatches
std::vector< SensorMetadata > ContactLoadCellArrays
std::vector< SensorMetadata > ThreeAxisAngularAccelerometers
std::vector< SensorMetadata > EncoderArrays
std::vector< SensorMetadata > PositionSensors
std::vector< SensorMetadata > ThreeAxisGyroscopes
std::vector< SensorMetadata > ThreeAxisLinearAccelerometers
std::vector< SensorMetadata > LinearVelocitySensors
std::vector< SensorMetadata > ThreeAxisMagnetometers
std::vector< SensorMetadata > OrientationSensors
std::vector< SensorMetadata > SixAxisForceTorqueSensors
std::vector< SensorMetadata > TemperatureSensors
void onRead(SensorStreamingData &v) override
SensorMeasurements EncoderArrays
SensorMeasurements ThreeAxisLinearAccelerometers
SensorMeasurements PositionSensors
SensorMeasurements OrientationSensors
SensorMeasurements ThreeAxisMagnetometers
SensorMeasurements ThreeAxisGyroscopes
SensorMeasurements SixAxisForceTorqueSensors
SensorMeasurements SkinPatches
SensorMeasurements ThreeAxisAngularAccelerometers
SensorMeasurements ContactLoadCellArrays
SensorMeasurements LinearVelocitySensors
SensorMeasurements TemperatureSensors
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void useCallback(TypedReaderCallback< T > &callback) override
Set an object whose onRead method will be called when data is available.
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
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
A base class for nested structures that can be searched.
Definition Searchable.h:31
yarp::os::WireLink & yarp()
Get YARP state associated with this object.
Definition Wire.h:28
#define yCError(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.
@ MAS_TIMEOUT
The sensor is read through the network, and the latest measurement was received before an implementat...
@ MAS_OK
The sensor is working correctly.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121