14YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSCLIENT,
"yarp.device.multipleanalogsensorsclient")
19 std::lock_guard<std::mutex> guard(
dataMutex);
32 yCError(MULTIPLEANALOGSENSORSCLIENT,
33 "No data received in the last %lf seconds, timeout enabled.",
42 m_carrier = config.
check(
"carrier",
yarp::os::Value(
"tcp"),
"the carrier used for the connection with the server").asString();
45 if (!config.
check(
"remote"))
47 yCError(MULTIPLEANALOGSENSORSCLIENT,
"Missing name parameter, exiting.");
51 if (!config.
check(
"local"))
53 yCError(MULTIPLEANALOGSENSORSCLIENT,
"Missing local parameter, exiting.");
59 yCError(MULTIPLEANALOGSENSORSCLIENT,
"'timeout' parameter is present but is not double, exiting.");
69 m_localRPCPortName = local +
"/rpc:i";
70 m_localStreamingPortName = local +
"/measures:i";
71 m_remoteRPCPortName = remote +
"/rpc:o";
72 m_remoteStreamingPortName = remote +
"/measures:o";
78 bool ok = m_rpcPort.
open(m_localRPCPortName);
81 yCError(MULTIPLEANALOGSENSORSCLIENT,
82 "Failure to open the port %s.",
83 m_localRPCPortName.c_str());
88 ok = m_streamingPort.
open(m_localStreamingPortName);
92 yCError(MULTIPLEANALOGSENSORSCLIENT,
93 "Failure to open the port %s.",
94 m_localStreamingPortName.c_str());
100 if (!m_externalConnection) {
103 yCError(MULTIPLEANALOGSENSORSCLIENT,
104 "Failure connecting port %s to %s.",
105 m_localRPCPortName.c_str(),
106 m_remoteRPCPortName.c_str());
107 yCError(MULTIPLEANALOGSENSORSCLIENT,
"Check that the specified MultipleAnalogSensorsServer is up.");
111 m_RPCConnectionActive =
true;
115 yCError(MULTIPLEANALOGSENSORSCLIENT,
116 "Failure connecting port %s to %s.",
117 m_remoteStreamingPortName.c_str(),
118 m_localStreamingPortName.c_str());
119 yCError(MULTIPLEANALOGSENSORSCLIENT,
"Check that the specified MultipleAnalogSensorsServer is up.");
123 m_StreamingConnectionActive =
true;
128 yCError(MULTIPLEANALOGSENSORSCLIENT,
"Failure opening Thrift-based RPC interface.");
145 if (m_StreamingConnectionActive)
149 if (m_RPCConnectionActive)
154 m_streamingPort.
close();
160size_t MultipleAnalogSensorsClient::genericGetNrOfSensors(
const std::vector<SensorMetadata>& metadataVector,
163 if (!m_externalConnection) {
164 return metadataVector.size();
166 std::lock_guard<std::mutex> guard(m_streamingPort.
dataMutex);
178 std::lock_guard<std::mutex> guard(m_streamingPort.
dataMutex);
180 return m_streamingPort.
status;
183bool MultipleAnalogSensorsClient::genericGetName(
const std::vector<SensorMetadata>& metadataVector,
const std::string& tag,
184 size_t sens_index, std::string& name)
const
186 if (m_externalConnection) {
187 yCError(MULTIPLEANALOGSENSORSCLIENT,
188 "Missing metadata, the device has been configured with the option"
189 "externalConnection set to true.");
192 if (sens_index >= metadataVector.size())
194 yCError(MULTIPLEANALOGSENSORSCLIENT,
195 "No sensor of type %s with index %lu (nr of sensors: %lu).",
198 metadataVector.size());
202 name = metadataVector[sens_index].name;
206bool MultipleAnalogSensorsClient::genericGetFrameName(
const std::vector<SensorMetadata>& metadataVector,
const std::string& tag,
207 size_t sens_index, std::string& frameName)
const
209 if (m_externalConnection) {
210 yCError(MULTIPLEANALOGSENSORSCLIENT,
211 "Missing metadata, the device has been configured with the option"
212 "externalConnection set to true.");
215 if (sens_index >= metadataVector.size())
217 yCError(MULTIPLEANALOGSENSORSCLIENT,
218 "No sensor of type %s with index %lu (nr of sensors: %lu).",
221 metadataVector.size());
225 frameName = metadataVector[sens_index].frameName;
229bool MultipleAnalogSensorsClient::genericGetMeasure(
const std::vector<SensorMetadata>& metadataVector,
const std::string& tag,
234 std::lock_guard<std::mutex> guard(m_streamingPort.
dataMutex);
238 yCError(MULTIPLEANALOGSENSORSCLIENT,
239 "Sensor of type %s with index %lu has non-MAS_OK status.",
247 yCError(MULTIPLEANALOGSENSORSCLIENT,
248 "No sensor of type %s with index %lu (nr of sensors: %lu).",
251 metadataVector.size());
255 if (!m_externalConnection) {
256 assert(metadataVector.size() == measurementsVector.
measurements.size());
259 timestamp = measurementsVector.
measurements[sens_index].timestamp;
260 out = measurementsVector.
measurements[sens_index].measurement;
265size_t MultipleAnalogSensorsClient::genericGetSize(
const std::vector<SensorMetadata>& metadataVector,
266 const std::string& tag,
const SensorMeasurements& measurementsVector,
size_t sens_index)
const
268 std::lock_guard<std::mutex> guard(m_streamingPort.
dataMutex);
271 yCError(MULTIPLEANALOGSENSORSCLIENT,
"No data received, no information on the size of the specified sensor.");
276 if (sens_index >= measurementsVector.
measurements.size())
278 yCError(MULTIPLEANALOGSENSORSCLIENT,
279 "No sensor of type %s with index %lu (nr of sensors: %lu).",
282 metadataVector.size());
286 return measurementsVector.
measurements[sens_index].measurement.size();
333 return genericGetStatus();
338 return genericGetName(m_sensorsMetadata.
ThreeAxisGyroscopes,
"ThreeAxisGyroscopes", sens_index, name);
343 return genericGetFrameName(m_sensorsMetadata.
ThreeAxisGyroscopes,
"ThreeAxisGyroscopes", sens_index, frameName);
360 return genericGetStatus();
387 return genericGetStatus();
392 return genericGetName(m_sensorsMetadata.
ThreeAxisMagnetometers,
"ThreeAxisMagnetometers", sens_index, name);
397 return genericGetFrameName(m_sensorsMetadata.
ThreeAxisMagnetometers,
"ThreeAxisMagnetometers", sens_index, frameName);
414 return genericGetStatus();
419 return genericGetName(m_sensorsMetadata.
OrientationSensors,
"OrientationSensors", sens_index, name);
424 return genericGetFrameName(m_sensorsMetadata.
OrientationSensors,
"OrientationSensors", sens_index, frameName);
441 return genericGetStatus();
446 return genericGetName(m_sensorsMetadata.
PositionSensors,
"PositionSensors", sens_index, name);
451 return genericGetFrameName(m_sensorsMetadata.
PositionSensors,
"PositionSensors", sens_index, frameName);
467 return genericGetStatus();
472 return genericGetName(m_sensorsMetadata.
TemperatureSensors,
"TemperatureSensors", sens_index, name);
477 return genericGetFrameName(m_sensorsMetadata.
TemperatureSensors,
"TemperatureSensors", sens_index, frameName);
502 return genericGetStatus();
512 return genericGetFrameName(m_sensorsMetadata.
SixAxisForceTorqueSensors,
"SixAxisForceTorqueSensors", sens_index, frameName);
529 return genericGetStatus();
534 return genericGetName(m_sensorsMetadata.
ContactLoadCellArrays,
"ContactLoadCellArrays", sens_index, name);
551 return genericGetNrOfSensors(m_sensorsMetadata.
EncoderArrays,
557 return genericGetStatus();
562 return genericGetName(m_sensorsMetadata.
EncoderArrays,
"EncoderArrays", sens_index, name);
567 return genericGetMeasure(m_sensorsMetadata.
EncoderArrays,
"EncoderArrays",
573 return genericGetSize(m_sensorsMetadata.
EncoderArrays,
"EncoderArrays",
579 return genericGetNrOfSensors(m_sensorsMetadata.
SkinPatches,
585 return genericGetStatus();
590 return genericGetName(m_sensorsMetadata.
SkinPatches,
"SkinPatches", sens_index, name);
595 return genericGetMeasure(m_sensorsMetadata.
SkinPatches,
"SkinPatches",
601 return genericGetSize(m_sensorsMetadata.
SkinPatches,
"SkinPatches",
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.
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 getSixAxisForceTorqueSensorStatus(size_t sens_index) const override
Get the status 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 getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double ×tamp) 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 ×tamp) 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 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 ×tamp) const override
Get the last reading 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 ×tamp) 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.
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 getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) 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 ×tamp) const override
Get the last reading of the specified sensor.
bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double ×tamp) 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 ×tamp) 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 ×tamp) 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 ×tamp) 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.
std::vector< SensorMeasurement > measurements
std::vector< SensorMetadata > SkinPatches
std::vector< SensorMetadata > ContactLoadCellArrays
std::vector< SensorMetadata > EncoderArrays
std::vector< SensorMetadata > PositionSensors
std::vector< SensorMetadata > ThreeAxisGyroscopes
std::vector< SensorMetadata > ThreeAxisLinearAccelerometers
std::vector< SensorMetadata > ThreeAxisMagnetometers
std::vector< SensorMetadata > OrientationSensors
std::vector< SensorMetadata > SixAxisForceTorqueSensors
std::vector< SensorMetadata > TemperatureSensors
SensorMeasurements EncoderArrays
SensorMeasurements ThreeAxisLinearAccelerometers
SensorMeasurements PositionSensors
SensorMeasurements OrientationSensors
SensorMeasurements ThreeAxisMagnetometers
SensorMeasurements ThreeAxisGyroscopes
SensorMeasurements SixAxisForceTorqueSensors
SensorMeasurements SkinPatches
SensorMeasurements ContactLoadCellArrays
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.
static bool disconnect(const std::string &src, const std::string &dest, bool quiet)
Request that an output port disconnect from an input port.
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.
A base class for nested structures that can be searched.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
A single value (typically within a Bottle).
virtual bool isFloat64() const
Checks if value is a 64-bit floating point number.
virtual std::string asString() const
Get string value.
bool attachAsClient(yarp::os::UnbufferedContactable &port)
Tag this WireLink as a client, sending data via the specified port.
yarp::os::WireLink & yarp()
Get YARP state associated with this object.
#define yCError(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.