19YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSSERVER,
"yarp.device.multipleanalogsensorsserver")
102 return "MAS_WAITING_FOR_FIRST_READ";
105 return "MAS_UNKNOWN";
130 if (m_periodInS <= 0)
132 yCError(MULTIPLEANALOGSENSORSSERVER,
133 "Period parameter is present (%f) but it is not a positive integer, exiting.",
145 m_RPCPortName =
m_name +
"/rpc:o";
146 m_streamingPortName =
m_name +
"/measures:o";
160#define MAS_CALL_MEMBER_FN(object, ptrToMember) ((*object).*(ptrToMember))
162template<
typename Interface>
163ReturnValue MultipleAnalogSensorsServer::populateSensorsMetadata(Interface * wrappedDeviceInterface,
164 std::vector<SensorMetadata>& metadataVector,
const std::string& tag,
169 if (wrappedDeviceInterface)
171 size_t nrOfSensors = 0;
175 yCError(MULTIPLEANALOGSENSORSSERVER,
176 "Failure in reading number of sensors of type %s.",
180 metadataVector.resize(nrOfSensors);
181 for (
size_t i=0; i < nrOfSensors; i++)
183 std::string sensorName;
187 yCError(MULTIPLEANALOGSENSORSSERVER,
188 "Failure in reading name of sensor of type %s at index %zu.",
193 std::string frameName;
194 ret =
MAS_CALL_MEMBER_FN(wrappedDeviceInterface, getFrameNameMethodPtr)(i, frameName);
197 yCError(MULTIPLEANALOGSENSORSSERVER,
198 "Failure in reading frame name of sensor of type %s at index %zu.",
204 metadataVector[i].name = sensorName;
205 metadataVector[i].frameName = frameName;
206 metadataVector[i].additionalMetadata =
"";
212 metadataVector.resize(0);
217template<
typename Interface>
218ReturnValue MultipleAnalogSensorsServer::populateSensorsMetadataNoFrameName(Interface * wrappedDeviceInterface,
219 std::vector<SensorMetadata>& metadataVector,
const std::string& tag,
223 if (wrappedDeviceInterface)
225 size_t nrOfSensors = 0;
229 yCError(MULTIPLEANALOGSENSORSSERVER,
230 "Failure in reading number of sensors of type %s.",
234 metadataVector.resize(nrOfSensors);
235 for (
size_t i=0; i < nrOfSensors; i++)
237 std::string sensorName;
241 yCError(MULTIPLEANALOGSENSORSSERVER,
242 "Failure in reading name of sensor of type %s at index %zu.",
248 metadataVector[i].name = sensorName;
249 metadataVector[i].frameName =
"";
250 metadataVector[i].additionalMetadata =
"";
256 metadataVector.resize(0);
262ReturnValue MultipleAnalogSensorsServer::populateAllSensorsMetadata()
265 ok = ok && populateSensorsMetadata(m_iThreeAxisGyroscopes, m_sensorMetadata.
ThreeAxisGyroscopes,
"ThreeAxisGyroscopes",
269 ok = ok && populateSensorsMetadata(m_iThreeAxisLinearAccelerometers, m_sensorMetadata.
ThreeAxisLinearAccelerometers,
"ThreeAxisLinearAccelerometers",
273 ok = ok && populateSensorsMetadata(m_iThreeAxisAngularAccelerometers, m_sensorMetadata.
ThreeAxisAngularAccelerometers,
"ThreeAxisAngularAccelerometers",
277 ok = ok && populateSensorsMetadata(m_iThreeAxisMagnetometers, m_sensorMetadata.
ThreeAxisMagnetometers,
"ThreeAxisMagnetometers",
281 ok = ok && populateSensorsMetadata(m_iPositionSensors, m_sensorMetadata.
PositionSensors,
"PositionSensors",
285 ok = ok && populateSensorsMetadata(m_iLinearVelocitySensors, m_sensorMetadata.
LinearVelocitySensors,
"LinearVelocitySensors",
289 ok = ok && populateSensorsMetadata(m_iOrientationSensors, m_sensorMetadata.
OrientationSensors,
"OrientationSensors",
293 ok = ok && populateSensorsMetadata(m_iTemperatureSensors, m_sensorMetadata.
TemperatureSensors,
"TemperatureSensors",
297 ok = ok && populateSensorsMetadata(m_iSixAxisForceTorqueSensors, m_sensorMetadata.
SixAxisForceTorqueSensors,
"SixAxisForceTorqueSensors",
301 ok = ok && populateSensorsMetadataNoFrameName(m_iContactLoadCellArrays, m_sensorMetadata.
ContactLoadCellArrays,
"ContactLoadCellArrays",
304 ok = ok && populateSensorsMetadataNoFrameName(m_iEncoderArrays, m_sensorMetadata.
EncoderArrays,
"EncoderArrays",
307 ok = ok && populateSensorsMetadataNoFrameName(m_iSkinPatches, m_sensorMetadata.
SkinPatches,
"ISkinPatches",
315template<
typename Interface>
316bool MultipleAnalogSensorsServer::resizeMeasureVectors(Interface* wrappedDeviceInterface,
317 const std::vector< SensorMetadata >& metadataVector,
318 std::vector< yarp::dev::SensorMeasurement >& streamingDataVector,
319 size_t (Interface::*getMeasureSizePtr)(
size_t) const)
321 if (wrappedDeviceInterface)
323 size_t nrOfSensors = metadataVector.size();
324 streamingDataVector.resize(nrOfSensors);
325 for (
size_t i=0; i < nrOfSensors; i++)
327 size_t measureSize =
MAS_CALL_MEMBER_FN(wrappedDeviceInterface, getMeasureSizePtr)(i);
328 streamingDataVector[i].measurement.resize(measureSize, std::numeric_limits<double>::quiet_NaN());
336template<
typename Interface>
337bool MultipleAnalogSensorsServer::resizeMeasureVectors(Interface* wrappedDeviceInterface,
338 const std::vector< SensorMetadata >& metadataVector,
339 std::vector< yarp::dev::SensorMeasurement >& streamingDataVector,
342 if (wrappedDeviceInterface)
344 size_t nrOfSensors = metadataVector.size();
345 streamingDataVector.resize(nrOfSensors);
346 for (
size_t i=0; i < nrOfSensors; i++)
348 streamingDataVector[i].measurement.resize(measureSize, std::numeric_limits<double>::quiet_NaN());
359 ok = ok && resizeMeasureVectors(m_iThreeAxisGyroscopes, m_sensorMetadata.
ThreeAxisGyroscopes,
367 ok = ok && resizeMeasureVectors(m_iPositionSensors, m_sensorMetadata.
PositionSensors,
371 ok = ok && resizeMeasureVectors(m_iOrientationSensors, m_sensorMetadata.
OrientationSensors,
373 ok = ok && resizeMeasureVectors(m_iTemperatureSensors, m_sensorMetadata.
TemperatureSensors,
379 ok = ok && resizeMeasureVectors(m_iEncoderArrays, m_sensorMetadata.
EncoderArrays,
381 ok = ok && resizeMeasureVectors(m_iSkinPatches, m_sensorMetadata.
SkinPatches,
391 yCError(MULTIPLEANALOGSENSORSSERVER,
"Null pointer passed to attach.");
397 poly->
view(m_iThreeAxisGyroscopes);
398 poly->
view(m_iThreeAxisLinearAccelerometers);
399 poly->
view(m_iThreeAxisAngularAccelerometers);
400 poly->
view(m_iThreeAxisMagnetometers);
401 poly->
view(m_iPositionSensors);
402 poly->
view(m_iLinearVelocitySensors);
403 poly->
view(m_iOrientationSensors);
404 poly->
view(m_iTemperatureSensors);
405 poly->
view(m_iSixAxisForceTorqueSensors);
406 poly->
view(m_iContactLoadCellArrays);
407 poly->
view(m_iEncoderArrays);
408 poly->
view(m_iSkinPatches);
412 bool ok = populateAllSensorsMetadata();
416 yCError(MULTIPLEANALOGSENSORSSERVER,
"Failure in populateAllSensorsMetadata()");
422 ok = m_streamingPort.
open(m_streamingPortName);
425 yCError(MULTIPLEANALOGSENSORSSERVER,
"Failure in opening port named %s.", m_streamingPortName.c_str());
433 yCError(MULTIPLEANALOGSENSORSSERVER,
"Failure in attaching RPC port to thrift RPC interface.");
438 ok = m_rpcPort.
open(m_RPCPortName);
441 yCError(MULTIPLEANALOGSENSORSSERVER,
"Failure in opening port named %s.", m_RPCPortName.c_str());
448 ok = ok && this->
start();
451 yCError(MULTIPLEANALOGSENSORSSERVER,
"Failure in starting thread.");
456 yCDebug(MULTIPLEANALOGSENSORSSERVER,
"Attach complete");
469 m_streamingPort.
close();
471 yCDebug(MULTIPLEANALOGSENSORSSERVER,
"Detach complete");
477 return m_sensorMetadata;
480template<
typename Interface>
481bool MultipleAnalogSensorsServer::genericStreamData(Interface* wrappedDeviceInterface,
482 const std::vector< SensorMetadata >& metadataVector,
483 std::vector< typename yarp::dev::SensorMeasurement >& streamingDataVector,
486 const char* sensorType)
488 if (wrappedDeviceInterface)
490 size_t nrOfSensors = metadataVector.size();
491 for (
size_t i=0; i < nrOfSensors; i++)
494 double& outputTimestamp = streamingDataVector[i].timestamp;
498 MAS_CALL_MEMBER_FN(wrappedDeviceInterface, getMeasureMethodPtr)(i, outputBuffer, outputTimestamp);
501 yCError(MULTIPLEANALOGSENSORSSERVER,
502 "Failure in reading data from sensor %s of type %s with code %s, no data will be sent on the port.",
519 ok = resizeAllMeasureVectors(streamingData);
526 "ThreeAxisGyroscope");
532 "ThreeAxisLinearAccelerometer");
538 "ThreeAxisAngularAccelerometer");
544 "ThreeAxisMagnetometer");
546 ok = ok && genericStreamData(m_iPositionSensors, m_sensorMetadata.
PositionSensors,
556 "LinearVelocitySensor");
558 ok = ok && genericStreamData(m_iOrientationSensors, m_sensorMetadata.
OrientationSensors,
562 "OrientationSensor");
564 ok = ok && genericStreamData(m_iTemperatureSensors, m_sensorMetadata.
TemperatureSensors,
568 "TemperatureSensor");
574 "SixAxisForceTorqueSensor");
580 "ContactLoadCellArrays");
582 ok = ok && genericStreamData(m_iEncoderArrays, m_sensorMetadata.
EncoderArrays,
588 ok = ok && genericStreamData(m_iSkinPatches, m_sensorMetadata.
SkinPatches,
598 m_streamingPort.
write();
#define MAS_CALL_MEMBER_FN(object, ptrToMember)
@ ThreeAxisAngularAccelerometers
@ ThreeAxisLinearAccelerometers
@ SixAxisForceTorqueSensors
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
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getEncoderArrayName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfEncoderArrays(size_t &num) const =0
Get the number of encoder arrays exposed by this device.
virtual size_t getEncoderArraySize(size_t sens_index) const =0
Get the size of the specified encoder array.
virtual yarp::dev::MAS_status getEncoderArrayStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const =0
Get the last reading of the specified sensor.
virtual yarp::dev::ReturnValue getLinearVelocitySensorFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfLinearVelocitySensors(size_t &num) const =0
Get the number of linear velocity sensors exposed by this device.
virtual yarp::dev::ReturnValue getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double ×tamp) const =0
Get the last reading of the linear velocity sensor as x y z.
virtual yarp::dev::ReturnValue getLinearVelocitySensorName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::MAS_status getLinearVelocitySensorStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double ×tamp) const =0
Get the last reading of the orientation sensor as roll pitch yaw.
virtual yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfOrientationSensors(size_t &num) const =0
Get the number of orientation sensors exposed by this device.
virtual yarp::dev::ReturnValue getOrientationSensorName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::MAS_status getPositionSensorStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getPositionSensorName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::ReturnValue getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double ×tamp) const =0
Get the last reading of the position sensor as x y z.
virtual yarp::dev::ReturnValue getPositionSensorFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfPositionSensors(size_t &num) const =0
Get the number of position sensors exposed by this device.
virtual yarp::dev::ReturnValue getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const =0
Get the last reading of the specified sensor.
virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::ReturnValue getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfSixAxisForceTorqueSensors(size_t &num) const =0
Get the number of six axis force torque sensors exposed by this device.
virtual yarp::dev::ReturnValue getSkinPatchMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const =0
Get the last reading of the specified sensor.
virtual yarp::dev::ReturnValue getSkinPatchName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual size_t getSkinPatchSize(size_t sens_index) const =0
Get the size of the specified skin patch.
virtual yarp::dev::MAS_status getSkinPatchStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfSkinPatches(size_t &num) const =0
Get the number of skin patches exposed by this device.
virtual yarp::dev::MAS_status getTemperatureSensorStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::ReturnValue getTemperatureSensorMeasure(size_t sens_index, double &out, double ×tamp) const =0
Get the last reading of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfTemperatureSensors(size_t &num) const =0
Get the number of temperature sensors exposed by this device.
virtual yarp::dev::ReturnValue getTemperatureSensorName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfThreeAxisAngularAccelerometers(size_t &num) const =0
Get the number of three axis angular accelerometers exposed by this device.
virtual yarp::dev::ReturnValue getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const =0
Get the last reading of the specified sensor.
virtual yarp::dev::ReturnValue getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::ReturnValue getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::ReturnValue getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfThreeAxisGyroscopes(size_t &num) const =0
Get the number of three axis gyroscopes exposed by this sensor.
virtual yarp::dev::ReturnValue getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const =0
Get the last reading of the gyroscope.
virtual yarp::dev::ReturnValue getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
virtual yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const =0
Get the last reading of the specified sensor.
virtual yarp::dev::ReturnValue getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfThreeAxisLinearAccelerometers(size_t &num) const =0
Get the number of three axis linear accelerometers exposed by this device.
virtual yarp::dev::ReturnValue getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const =0
Get the name of the frame of the specified sensor.
virtual yarp::dev::ReturnValue getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const =0
Get the last reading of the specified sensor.
virtual yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const =0
Get the status of the specified sensor.
virtual yarp::dev::ReturnValue getNrOfThreeAxisMagnetometers(size_t &num) const =0
Get the number of magnetometers exposed by this device.
virtual yarp::dev::ReturnValue getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const =0
Get the name of the specified sensor.
A container for a device driver.
std::vector< SensorMeasurementData > measurements
SensorMeasurementsData ThreeAxisGyroscopes
SensorMeasurementsData OrientationSensors
SensorMeasurementsData PositionSensors
SensorMeasurementsData SkinPatches
SensorMeasurementsData LinearVelocitySensors
SensorMeasurementsData ThreeAxisMagnetometers
SensorMeasurementsData SixAxisForceTorqueSensors
SensorMeasurementsData ContactLoadCellArrays
SensorMeasurementsData TemperatureSensors
SensorMeasurementsData ThreeAxisLinearAccelerometers
SensorMeasurementsData EncoderArrays
SensorMeasurementsData ThreeAxisAngularAccelerometers
bool detachAll() final
Detach the object (you must have first called attach).
void close() override
Stop port activity.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
bool unprepare()
Give the last prepared object back to YARP without writing it.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
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.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
bool attachAsServer(yarp::os::UnbufferedContactable &port)
Tag this WireLink as a server, receiving commands via the specified port.
yarp::os::WireLink & yarp()
Get YARP state associated with this object.
void resize(size_t size)
Resize the vector.
#define yCError(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.
@ MAS_UNKNOWN
The sensor is in an unknown state.
@ MAS_TIMEOUT
The sensor is read through the network, and the latest measurement was received before an implementat...
@ MAS_ERROR
The sensor is in generic error state.
@ MAS_WAITING_FOR_FIRST_READ
The sensor is read through the network, and the device is waiting to receive the first measurement.
@ MAS_OK
The sensor is working correctly.
@ MAS_OVF
The sensor reached an overflow.
The main, catch-all namespace for YARP.