depthCameraDriver.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: LGPL-2.1-or-later
 */
 
#ifndef DEPTHCAMERA_DRIVER_H
#define DEPTHCAMERA_DRIVER_H
 
#include <string>
#include <map>
 
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/IFrameGrabberControls.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/sig/all.h>
#include <yarp/sig/Matrix.h>
#include <yarp/os/Stamp.h>
#include <yarp/dev/IRGBDSensor.h>
#include <yarp/dev/RGBDSensorParamParser.h>
#include <OpenNI.h>
 
 
#ifndef RAD2DEG
#define RAD2DEG (180/3.14159265359)
#endif
 
#ifndef DEG2RAD
#define DEG2RAD (3.14159265359/180.0)
#endif
 
class  streamFrameListener;
 
class depthCameraDriver :
        public yarp::dev::DeviceDriver,
        public yarp::dev::IRGBDSensor,
        public yarp::dev::IFrameGrabberControls
{
private:
    typedef yarp::sig::ImageOf<yarp::sig::PixelFloat> depthImage;
    typedef yarp::os::Stamp                           Stamp;
    typedef yarp::os::Property                        Property;
    typedef yarp::sig::FlexImage                      FlexImage;
 
public:
    depthCameraDriver();
    ~depthCameraDriver();
    static int pixFormatToCode(openni::PixelFormat p);
 
    // DeviceDriver
    bool open(yarp::os::Searchable& config) override;
    bool close() override;
 
    // IRGBDSensor
    int                      getRgbHeight() override;
    int                      getRgbWidth() override;
    yarp::dev::ReturnValue   getRgbSupportedConfigurations(std::vector<yarp::dev::CameraConfig>& configurations) override;
    yarp::dev::ReturnValue   getRgbResolution(int& width, int& height) override;
    yarp::dev::ReturnValue   setRgbResolution(int width, int height) override;
    yarp::dev::ReturnValue   getRgbFOV(double& horizontalFov, double& verticalFov) override;
    yarp::dev::ReturnValue   setRgbFOV(double horizontalFov, double verticalFov) override;
    yarp::dev::ReturnValue   getRgbMirroring(bool& mirror) override;
    yarp::dev::ReturnValue   setRgbMirroring(bool mirror) override;
 
    int                      getDepthHeight() override;
    int                      getDepthWidth() override;
    yarp::dev::ReturnValue   getRgbIntrinsicParam(Property& intrinsic) override;
    yarp::dev::ReturnValue   setDepthResolution(int width, int height) override;
    yarp::dev::ReturnValue   getDepthFOV(double& horizontalFov, double& verticalFov) override;
    yarp::dev::ReturnValue   setDepthFOV(double horizontalFov, double verticalFov) override;
    yarp::dev::ReturnValue   getDepthIntrinsicParam(Property& intrinsic) override;
    yarp::dev::ReturnValue   getDepthAccuracy(double& accuracy) override;
    yarp::dev::ReturnValue   setDepthAccuracy(double accuracy) override;
    yarp::dev::ReturnValue   getDepthClipPlanes(double& nearPlane, double& farPlane) override;
    yarp::dev::ReturnValue   setDepthClipPlanes(double nearPlane, double farPlane) override;
    yarp::dev::ReturnValue   getDepthMirroring(bool& mirror) override;
    yarp::dev::ReturnValue   setDepthMirroring(bool mirror) override;
 
    yarp::dev::ReturnValue   getExtrinsicParam(yarp::sig::Matrix& extrinsic) override;
    yarp::dev::ReturnValue   getRgbImage(FlexImage& rgbImage, Stamp* timeStamp = NULL) override;
    yarp::dev::ReturnValue   getDepthImage(depthImage& depthImage, Stamp* timeStamp = NULL) override;
    yarp::dev::ReturnValue   getImages(FlexImage& colorFrame, depthImage& depthFrame, Stamp* colorStamp=NULL, Stamp* depthStamp=NULL) override;
 
    RGBDSensor_status        getSensorStatus() override;
    std::string              getLastErrorMsg(Stamp* timeStamp = NULL) override;
 
    //IFrameGrabberControls
    yarp::dev::ReturnValue   getCameraDescription(CameraDescriptor *camera) override;
    yarp::dev::ReturnValue   hasFeature(int feature, bool*   hasFeature) override;
    yarp::dev::ReturnValue   setFeature(int feature, double  value) override;
    yarp::dev::ReturnValue   getFeature(int feature, double* value) override;
    yarp::dev::ReturnValue   setFeature(int feature, double  value1,  double  value2) override;
    yarp::dev::ReturnValue   getFeature(int feature, double* value1,  double* value2) override;
    yarp::dev::ReturnValue   hasOnOff(  int feature, bool*   HasOnOff) override;
    yarp::dev::ReturnValue   setActive( int feature, bool    onoff) override;
    yarp::dev::ReturnValue   getActive( int feature, bool*   isActive) override;
    yarp::dev::ReturnValue   hasAuto(   int feature, bool*   hasAuto) override;
    yarp::dev::ReturnValue   hasManual( int feature, bool*   hasManual) override;
    yarp::dev::ReturnValue   hasOnePush(int feature, bool*   hasOnePush) override;
    yarp::dev::ReturnValue   setMode(   int feature, FeatureMode mode) override;
    yarp::dev::ReturnValue   getMode(   int feature, FeatureMode *mode) override;
    yarp::dev::ReturnValue   setOnePush(int feature) override;
 
private:
    //method
    inline bool initializeOpeNIDevice();
    inline bool setParams();
    bool        getImage(FlexImage& Frame, Stamp* Stamp, streamFrameListener *sourceFrame);
    bool        getImage(depthImage& Frame, Stamp* Stamp, streamFrameListener *sourceFrame);
    bool        setResolution(int w, int h, openni::VideoStream& stream);
    bool        setFOV(double horizontalFov, double verticalFov, openni::VideoStream& stream);
    bool        setIntrinsic(yarp::os::Property& intrinsic, const yarp::sig::IntrinsicParams& values);
    void        settingErrorMsg(const std::string& error, bool& ret);
 
    //properties
    openni::VideoStream             m_depthStream;
    openni::VideoStream             m_imageStream;
    openni::Device                  m_device;
    streamFrameListener*      m_depthFrame;
    streamFrameListener*      m_imageFrame;
    std::string           m_lastError;
    yarp::dev::RGBDSensorParamParser* m_paramParser;
    bool                            m_depthRegistration;
    std::vector<cameraFeature_id_t> m_supportedFeatures;
};
#endif