RGBDSensorClient.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms of the
 * BSD-3-Clause license. See the accompanying LICENSE file for details.
 */
 
#ifndef YARP_DEV_RGBDSENSORCLIENT_RGBDSENSORCLIENT_H
#define YARP_DEV_RGBDSENSORCLIENT_RGBDSENSORCLIENT_H
 
 
#include <yarp/os/Time.h>
#include <yarp/os/Network.h>
#include <yarp/os/BufferedPort.h>
 
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/IRGBDSensor.h>
#include <yarp/dev/IPreciselyTimed.h>
#include <yarp/dev/IVisualParamsImpl.h>
#include <yarp/dev/FrameGrabberControl2.h>
#include <yarp/dev/FrameGrabberControlImpl.h>
 
#define DEFAULT_THREAD_PERIOD       20    //ms
#define RGBDSENSOR_TIMEOUT_DEFAULT  100   //ms
 
 
class RGBDSensor_StreamingMsgParser;
 
 
class RGBDSensorClient :
        public yarp::dev::DeviceDriver,
        public yarp::dev::FrameGrabberControls_Sender,
        public yarp::dev::IRGBDSensor
{
protected:
    yarp::os::Port rpcPort;
private:
    yarp::dev::Implement_RgbVisualParams_Sender* RgbMsgSender{nullptr};
    yarp::dev::Implement_DepthVisualParams_Sender* DepthMsgSender{nullptr};
protected:
    std::string local_colorFrame_StreamingPort_name;
    std::string local_depthFrame_StreamingPort_name;
    std::string remote_colorFrame_StreamingPort_name;
    std::string remote_depthFrame_StreamingPort_name;
    std::string image_carrier_type;
    std::string depth_carrier_type;
    yarp::os::BufferedPort<yarp::sig::FlexImage> colorFrame_StreamingPort;
    yarp::os::BufferedPort<yarp::sig::ImageOf< yarp::sig::PixelFloat> > depthFrame_StreamingPort;
 
    // Use a single RPC port for now
    std::string local_rpcPort_name;
    std::string remote_rpcPort_name;
 
    /*
     * In case the client has to connect to 2 different wrappers/server because the rgb
     * and depth comes from two different sources.
     *
     * It should be possible to attach this guy to more than one port, try to see what
     * will happen when receiving 2 calls at the same time (receive one calls while serving
     * another one, it will result in concurrent thread most probably) and buffering issues.
     *
 
        std::string local_colorFrame_rpcPort_Name;
        std::string local_depthFrame_rpcPort_Name;
        std::string remote_colorFrame_rpcPort_Name;
        std::string remote_depthFrame_rpcPort_Name;
 
        yarp::os::Port colorFrame_rpcPort;
        yarp::os::Port depthFrame_rpcPort;
    */
 
    // Image data specs
    std::string sensorId;
    yarp::dev::IRGBDSensor *sensor_p{nullptr};
    IRGBDSensor::RGBDSensor_status sensorStatus{IRGBDSensor::RGBD_SENSOR_NOT_READY};
    int verbose{2};
 
    bool use_ROS{false};  // if false (default) read from YARP port, if true read from ROS topic instead (Both at the same time is not possible).
    bool initialize_YARP(yarp::os::Searchable &config);
    bool initialize_ROS(yarp::os::Searchable &config);
 
    yarp::os::Stamp colorStamp;
    yarp::os::Stamp depthStamp;
 
 
    // This is gonna be superseded by the synchronized when it'll be ready
    RGBDSensor_StreamingMsgParser *streamingReader{nullptr};
    bool fromConfig(yarp::os::Searchable &config);
 
public:
    RGBDSensorClient();
    RGBDSensorClient(const RGBDSensorClient&) = delete;
    RGBDSensorClient(RGBDSensorClient&&) = delete;
    RGBDSensorClient& operator=(const RGBDSensorClient&) = delete;
    RGBDSensorClient& operator=(RGBDSensorClient&&) = delete;
    ~RGBDSensorClient() override;
 
    int  getRgbHeight() override;
    int  getRgbWidth() override;
    bool getRgbSupportedConfigurations(yarp::sig::VectorOf<yarp::dev::CameraConfig> &configurations) override;
    bool getRgbResolution(int &width, int &height) override;
    bool setRgbResolution(int width, int height) override;
    bool getRgbFOV(double &horizontalFov, double &verticalFov) override;
    bool setRgbFOV(double horizontalFov, double verticalFov) override;
    bool getRgbIntrinsicParam(yarp::os::Property &intrinsic) override;
    bool getRgbMirroring(bool& mirror) override;
    bool setRgbMirroring(bool mirror) override;
 
    /*
     * IDepthVisualParams interface. Look at IVisualParams.h for documentation
     */
    int    getDepthHeight() override;
    int    getDepthWidth() override;
    bool   setDepthResolution(int width, int height) override;
    bool   getDepthFOV(double &horizontalFov, double &verticalFov) override;
    bool   setDepthFOV(double horizontalFov, double verticalFov) override;
    double getDepthAccuracy() override;
    bool   setDepthAccuracy(double accuracy) override;
    bool   getDepthClipPlanes(double &near, double &far) override;
    bool   setDepthClipPlanes(double near, double far) override;
    bool   getDepthIntrinsicParam(yarp::os::Property &intrinsic) override;
    bool   getDepthMirroring(bool& mirror) override;
    bool   setDepthMirroring(bool mirror) override;
 
    // Device Driver interface //
    bool open(yarp::os::Searchable& config) override;
 
    bool close() override;
 
    /*
     *  IRgbVisualParams interface. Look at IVisualParams.h for documentation
     */
 
    /*
     * IRGBDSensor specific interface methods
     */
 
    bool getExtrinsicParam(yarp::sig::Matrix &extrinsic) override;
 
    IRGBDSensor::RGBDSensor_status getSensorStatus() override;
 
    std::string getLastErrorMsg(yarp::os::Stamp *timeStamp = NULL) override;
 
    bool getRgbImage(yarp::sig::FlexImage &rgbImage, yarp::os::Stamp *timeStamp = NULL) override;
 
    bool getDepthImage(yarp::sig::ImageOf<yarp::sig::PixelFloat> &depthImage, yarp::os::Stamp *timeStamp = NULL) override;
 
    bool getImages(yarp::sig::FlexImage &colorFrame, yarp::sig::ImageOf<yarp::sig::PixelFloat> &depthFrame, yarp::os::Stamp *colorStamp=NULL, yarp::os::Stamp *depthStamp=NULL) override;
 
 
    // IFrame Grabber Control 2
    //
    // Implemented by FrameGrabberControls2_Sender
    //
    using FrameGrabberControls_Sender::getCameraDescription;
    using FrameGrabberControls_Sender::hasFeature;
    using FrameGrabberControls_Sender::setFeature;
    using FrameGrabberControls_Sender::getFeature;
    using FrameGrabberControls_Sender::hasOnOff;
    using FrameGrabberControls_Sender::setActive;
    using FrameGrabberControls_Sender::getActive;
    using FrameGrabberControls_Sender::hasAuto;
    using FrameGrabberControls_Sender::hasManual;
    using FrameGrabberControls_Sender::hasOnePush;
    using FrameGrabberControls_Sender::setMode;
    using FrameGrabberControls_Sender::getMode;
    using FrameGrabberControls_Sender::setOnePush;
};
 
#endif // YARP_DEV_RGBDSENSORCLIENT_RGBDSENSORCLIENT_H