MultipleAnalogSensorsServer.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
 
#ifndef YARP_DEV_MULTIPLEANALOGSENSORSSERVER_MULTIPLEANALOGSENSORSSERVER_H
#define YARP_DEV_MULTIPLEANALOGSENSORSSERVER_MULTIPLEANALOGSENSORSSERVER_H
 
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/Stamp.h>
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/IMultipleWrapper.h>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 
// Thrift-generated classes
#include "SensorStreamingData.h"
#include "MultipleAnalogSensorsMetadata.h"
 
 
class MultipleAnalogSensorsServer :
        public yarp::os::PeriodicThread,
        public yarp::dev::DeviceDriver,
        public yarp::dev::IMultipleWrapper,
        public MultipleAnalogSensorsMetadata
{
    double m_periodInS{0.01};
    yarp::os::Stamp m_stamp;
    std::string m_streamingPortName;
    std::string m_RPCPortName;
    yarp::os::BufferedPort<SensorStreamingData> m_streamingPort;
    yarp::os::Port m_rpcPort;
    // Generic vector buffer
    yarp::sig::Vector m_buffer;
 
    // Wrapped subdevices, if any
    yarp::dev::PolyDriver m_subdevice;
    bool m_isDeviceOwned{false};
 
    // Interface of the wrapped device
    yarp::dev::IThreeAxisGyroscopes* m_iThreeAxisGyroscopes{nullptr};
    yarp::dev::IThreeAxisLinearAccelerometers* m_iThreeAxisLinearAccelerometers{nullptr};
    yarp::dev::IThreeAxisMagnetometers* m_iThreeAxisMagnetometers{nullptr};
    yarp::dev::IPositionSensors* m_iPositionSensors{nullptr};
    yarp::dev::IOrientationSensors* m_iOrientationSensors{nullptr};
    yarp::dev::ITemperatureSensors* m_iTemperatureSensors{nullptr};
    yarp::dev::ISixAxisForceTorqueSensors* m_iSixAxisForceTorqueSensors{nullptr};
    yarp::dev::IContactLoadCellArrays* m_iContactLoadCellArrays{nullptr};
    yarp::dev::IEncoderArrays* m_iEncoderArrays{nullptr};
    yarp::dev::ISkinPatches* m_iSkinPatches{nullptr};
 
    // Metadata to be server via the RPC port
    SensorRPCData m_sensorMetadata;
    bool populateAllSensorsMetadata();
    template<typename Interface>
    bool populateSensorsMetadata(Interface * wrappedDeviceInterface,
                                 std::vector<SensorMetadata>& metadataVector, const std::string& tag,
                                 size_t (Interface::*getNrOfSensorsMethodPtr)() const,
                                 bool (Interface::*getNameMethodPtr)(size_t, std::string&) const,
                                 bool (Interface::*getFrameNameMethodPtr)(size_t, std::string&) const);
    template<typename Interface>
    bool populateSensorsMetadataNoFrameName(Interface * wrappedDeviceInterface,
                                            std::vector<SensorMetadata>& metadataVector, const std::string& tag,
                                            size_t (Interface::*getNrOfSensorsMethodPtr)() const,
                                            bool (Interface::*getNameMethodPtr)(size_t, std::string&) const);
 
 
    // Helper methods to resize measure buffers
    template<typename Interface>
    bool resizeMeasureVectors(Interface* wrappedDeviceInterface,
                              const std::vector< SensorMetadata >& metadataVector,
                              std::vector< SensorMeasurement >& streamingDataVector,
                              size_t (Interface::*getMeasureSizePtr)(size_t) const);
    template<typename Interface>
    bool resizeMeasureVectors(Interface* wrappedDeviceInterface,
                              const std::vector< SensorMetadata >& metadataVector,
                              std::vector< SensorMeasurement >& streamingDataVector,
                              size_t measureSize);
    bool resizeAllMeasureVectors(SensorStreamingData& streamingData);
 
 
    // Helper method used to copy the sensor measure to the measure buffers
    template<typename Interface>
    bool genericStreamData(Interface* wrappedDeviceInterface,
                           const std::vector< SensorMetadata >& metadataVector,
                           std::vector< SensorMeasurement >& streamingDataVector,
                           yarp::dev::MAS_status (Interface::*getStatusMethodPtr)(size_t) const,
                           bool (Interface::*getMeasureMethodPtr)(size_t, yarp::sig::Vector&, double&) const,
                           const char* sensorType);
 
 
public:
    MultipleAnalogSensorsServer();
    ~MultipleAnalogSensorsServer();
 
    /* DevideDriver methods */
    bool open(yarp::os::Searchable &params) override;
    bool close() override;
 
    /* IMultipleWrapper methods */
    bool attachAll(const yarp::dev::PolyDriverList &p) override;
    bool detachAll() override;
 
    /* RateThread methods */
    void threadRelease() override;
    void run() override;
 
    /* MultipleAnalogSensorsMetadata */
    SensorRPCData getMetadata() override;
};
 
#endif