OpenNIImageProvider.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::Component
 * @author     David Gonzalez (david dot gonzalez at kit dot edu)
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "OpenNIImageProvider.h"
 
#include "Helpers/helpers.h"
 
// boost
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
 
// VisionXCore
#include <VisionX/core/exceptions/user/FrameRateNotSupportedException.h>
#include <VisionX/core/exceptions/user/StartingCaptureFailedException.h>
 
// VisionXTools
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
// ArmarXCore
#include <ArmarXCore/core/application/properties/Property.h>
 
using namespace armarx;
using namespace visionx::tools;
 
namespace visionx
{
 
 
    bool OpenNIImageProvider::isBackEndInitialized = false;
 
    void
    OpenNIImageProvider::onInitCapturingImageProvider()
    {
        //-------------------------------------------------------------------------
 
        coloredPoint3DBuffer = NULL;
 
        normalizedDepthCells = NULL;
 
        isModuleInitialized = false;
 
        //-------------------------------------------------------------------------
 
        uri = getProperty<std::string>("DeviceURI").getValue();
 
        videoDimension = getProperty<ImageDimension>("VideoMode").getValue();
 
        frameRate = getProperty<float>("FrameRate").getValue();
 
        imageType = getProperty<ImageType>("ImageType").getValue();
 
        ARMARX_INFO << "Information: URI : " << uri << flush;
 
        ARMARX_INFO << "Information: Video Dimension : (" << videoDimension.width << ","
                    << videoDimension.height << ")" << flush;
 
        ARMARX_INFO << "Information: Frame Rate : " << frameRate << flush;
 
        ARMARX_INFO << "Information: Image Type : " << imageType << flush;
 
        setNumberImages(1);
 
        setImageFormat(videoDimension, imageType, eBayerPatternBg);
 
        setImageSyncMode(eCaptureSynchronization);
 
        //-------------------------------------------------------------------------
 
        if (!isBackEndInitialized)
        {
            if (openni::OpenNI::initialize() == openni::STATUS_OK)
            {
                isBackEndInitialized = true;
 
                ARMARX_INFO << "Information: OpenNI Initialized successfully." << flush;
            }
            else
            {
                ARMARX_ERROR << "Error: While intializing OpenNI : "
                             << openni::OpenNI::getExtendedError() << flush;
            }
        }
    }
 
    void
    OpenNIImageProvider::onExitCapturingImageProvider()
    {
        StopDevice();
 
        DestroyDataStructure();
 
        ARMARX_INFO << "Information : Exit Capturing." << flush;
    }
 
    void
    OpenNIImageProvider::onStartCapture(float frameRate)
    {
        ARMARX_INFO << "Information: Starting capturing..." << flush;
 
        bool ERROR = false;
 
        if (isBackEndInitialized)
        {
            const bool UseAnyDevice = (!uri.length()) || (uri == std::string("ANY_DEVICE")) ||
                                      (uri == std::string("Any")) || (uri == std::string("any")) ||
                                      (uri == std::string("ANY"));
 
            const bool LookForDeviceTarget = !UseAnyDevice;
 
            bool DeviceTargetFound = false;
 
            openni::Array<openni::DeviceInfo> FoundDevices;
 
            openni::OpenNI::enumerateDevices(&FoundDevices);
 
            const int TotalDevices = FoundDevices.getSize();
 
            ARMARX_INFO << "Information: Total openni devices found : " << TotalDevices << flush;
 
            if (TotalDevices)
            {
                for (int i = 0; i < TotalDevices; ++i)
                {
                    const std::string CurrentURI = std::string(FoundDevices[i].getUri());
 
                    if (LookForDeviceTarget && (uri == CurrentURI))
                    {
                        ARMARX_INFO << "Target found: openni device[" << i
                                    << "] Uri = " << CurrentURI << flush;
 
                        DeviceTargetFound = true;
                    }
                    else
                    {
                        ARMARX_INFO << "Information: Available device OpenNI device[" << i
                                    << "] Uri = " << CurrentURI << flush;
                    }
                }
 
                const int Width = videoDimension.width;
 
                const int Height = videoDimension.height;
 
                const int Fps = EnsureFPS(int(round(frameRate)));
 
                ARMARX_INFO << "Information: Video Dimension : (" << Width << "," << Height << ")"
                            << flush;
 
                ARMARX_INFO << "Information: Frame Rate : " << Fps << flush;
 
                if (LookForDeviceTarget)
                {
                    if (DeviceTargetFound)
                    {
                        if (StartDevice(uri.c_str(), Width, Height, Fps))
                        {
                            ARMARX_INFO << "openni device[" << uri << "] started succesfully"
                                        << flush;
 
                            if (CreateDataStructures())
                            {
                                ARMARX_ERROR << "Error: Data structures created succesffully"
                                             << flush;
 
                                ERROR = false;
                            }
                            else
                            {
                                ARMARX_ERROR << "Error: Data structures creation failure" << flush;
 
                                ERROR = true;
                            }
                        }
                        else
                        {
                            ARMARX_ERROR << "Error: openni device[" << uri << "] started failure"
                                         << flush;
 
                            ERROR = true;
                        }
                    }
                    else
                    {
                        ARMARX_ERROR << "Error: openni device Uri = " << uri << " not found"
                                     << flush;
 
                        ERROR = true;
                    }
                }
                else
                {
                    if (StartDevice(openni::ANY_DEVICE, Width, Height, Fps))
                    {
                        ARMARX_INFO << "Infromation: OpenNI device started succesfully." << flush;
 
                        if (CreateDataStructures())
                        {
                            ARMARX_INFO << " Data structures created succesffully" << flush;
 
                            ERROR = false;
                        }
                        else
                        {
                            ARMARX_ERROR << " Error: Data structures creation failure" << flush;
 
                            ERROR = true;
                        }
                    }
                    else
                    {
                        ARMARX_ERROR << "Error: openni device started failure" << flush;
 
                        ERROR = true;
                    }
                }
            }
            else
            {
                ARMARX_ERROR << "Error: No openni devices are available" << flush;
 
                ERROR = true;
            }
        }
        else
        {
            ARMARX_ERROR << "Error: Openni backend not initialized" << flush;
 
            ERROR = true;
        }
 
        if (ERROR)
        {
            throw visionx::exceptions::user::StartingCaptureFailedException(
                "Opening cameras failed!");
        }
    }
 
    void
    OpenNIImageProvider::onStopCapture()
    {
        StopDevice();
 
        DestroyDataStructure();
    }
 
    bool
    OpenNIImageProvider::capture(void** ppImageBuffers)
    {
        if (!isModuleInitialized)
        {
            return false;
        }
 
        bool succeeded = false;
 
        switch (getImageFormat().type)
        {
                /*case visionx::ePointsScan:
                succeeded = true;
                break;*/
 
            case visionx::eColoredPointsScan:
                succeeded = CaptureColoredPoint();
                break;
 
            /*
             * Handle image types which are not supported by OpenNIImageProvider
             */
            default:
                ARMARX_ERROR << "Error: Image type not supported by OpenNIImageProvider!" << flush;
                return false;
        }
 
        if (!succeeded)
        {
            ARMARX_ERROR << "Error: Capturing failed!" << flush;
            return false;
        }
 
        {
            armarx::SharedMemoryScopedWriteLockPtr lock = getScopedWriteLock();
 
            ImageFormatInfo imageFormat = getImageFormat();
 
            const int BufferSize = imageFormat.dimension.width * imageFormat.dimension.height *
                                   imageFormat.bytesPerPixel;
 
            memcpy(ppImageBuffers[0], coloredPoint3DBuffer, BufferSize);
        }
 
        return true;
    }
 
    /*
    void OpenNIImageProvider::provideImages(CByteImage** images)
    {
        if(numberImages == 0)
            return;
 
        const int Width = videoDimension.width;
 
        const int Height = videoDimension.height;
 
        CByteImage Result = Display(m_pColoredPoint3DBuffer,Width,Height,true);
 
        memcpy(images[0]->pixels, Result.pixels, Width * Height * Result.bytesPerPixel);
    }
 
    void OpenNIImageProvider::provideImages(CFloatImage** images)
    {
        if(numberImages == 0)
            return;
 
        const int Width = videoDimension.width;
 
        const int Height = videoDimension.height;
 
        const visionx::ColoredPoint3D* pColoredPoint3DBuffer = pColoredPoint3DBufferBase;
 
        const visionx::ColoredPoint3D* const pEndColoredPoint3DBuffer = pColoredPoint3DBufferBase + (Width * Height);
 
        float* pDepthPixel = images[0]->pixels;
 
        while(pColoredPoint3DBuffer<pEndColoredPoint3DBuffer)
        {
            *pDepthPixel++ = pColoredPoint3DBuffer->m_Point.z;
 
            ++pColoredPoint3DBuffer;
        }
    }
    */
 
    bool
    OpenNIImageProvider::StartDevice(const char* pDeviceURI,
                                     const int Width,
                                     const int Height,
                                     const int Fps)
    {
        if (isModuleInitialized)
        {
            return false;
        }
 
        //-------------------------------------------------------------------------------------------------------------------------------------------------------------
 
        if (deviceOpenNI.open(pDeviceURI) != openni::STATUS_OK)
        {
            ARMARX_ERROR << "Error: Cannot open device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        if (!deviceOpenNI.hasSensor(openni::SENSOR_DEPTH))
        {
            ARMARX_ERROR << "Error: Device (" << pDeviceURI << ") [ has not depth sensor ]"
                         << flush;
 
            return false;
        }
 
        if (!deviceOpenNI.hasSensor(openni::SENSOR_COLOR))
        {
            ARMARX_ERROR << "Error: Device (" << pDeviceURI << ") [ has not color sensor ]"
                         << flush;
 
            return false;
        }
 
        //-------------------------------------------------------------------------------------------------------------------------------------------------------------
 
        if (depthStreamOpenNI.create(deviceOpenNI, openni::SENSOR_DEPTH) != openni::STATUS_OK)
        {
            ARMARX_ERROR << "Error: Cannot create Depth stream on Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        if (!depthStreamOpenNI.isValid())
        {
            ARMARX_ERROR << "Error: Cannot create Depth stream on Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        openni::VideoMode CurrentDepthStreamMode = depthStreamOpenNI.getVideoMode();
 
        ARMARX_INFO << "Information: Depth Stream Default Video Dimension : ("
                    << CurrentDepthStreamMode.getResolutionX() << ","
                    << CurrentDepthStreamMode.getResolutionY() << ")" << flush;
 
        ARMARX_INFO << "Information: Depth Stream Default Frame Rate : "
                    << CurrentDepthStreamMode.getFps() << flush;
 
        ARMARX_INFO << "Information: Depth Stream Target Video Dimension : (" << Width << ","
                    << Height << ")" << flush;
 
        ARMARX_INFO << "Information: Depth Stream Target Frame Rate : " << Fps << flush;
 
        CurrentDepthStreamMode.setResolution(Width, Height);
 
        CurrentDepthStreamMode.setFps(Fps);
 
        if (depthStreamOpenNI.setVideoMode(CurrentDepthStreamMode) != openni::STATUS_OK)
        {
            ARMARX_ERROR << "Error: Cannot set Depth stream settings (" << Width << "," << Height
                         << "," << Fps << ") Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        if (depthStreamOpenNI.start() != openni::STATUS_OK)
        {
            depthStreamOpenNI.destroy();
 
            ARMARX_ERROR << "Error: Cannot start Depth stream on Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        CurrentDepthStreamMode = depthStreamOpenNI.getVideoMode();
 
        ARMARX_INFO << "Information: Depth Stream Active Video Dimension : ("
                    << CurrentDepthStreamMode.getResolutionX() << ","
                    << CurrentDepthStreamMode.getResolutionY() << ")" << flush;
 
        ARMARX_INFO << "Information: Depth Stream Active Frame Rate : "
                    << CurrentDepthStreamMode.getFps() << flush;
 
        if (!depthStreamOpenNI.isValid())
        {
            ARMARX_ERROR << "Error: Cannot create Depth stream on Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        //-------------------------------------------------------------------------------------------------------------------------------------------------------------
 
        if (colorStreamOpenNI.create(deviceOpenNI, openni::SENSOR_COLOR) != openni::STATUS_OK)
        {
            ARMARX_ERROR << "Error: Cannot create Color stream on Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        if (!colorStreamOpenNI.isValid())
        {
            ARMARX_ERROR << "Error: Cannot create Color stream on Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        openni::VideoMode CurrentColorStreamMode = colorStreamOpenNI.getVideoMode();
 
        ARMARX_INFO << "Information: Color Stream Default Video Dimension : ("
                    << CurrentColorStreamMode.getResolutionX() << ","
                    << CurrentColorStreamMode.getResolutionY() << ")" << flush;
 
        ARMARX_INFO << "Information: Color Stream Default Frame Rate : "
                    << CurrentColorStreamMode.getFps() << flush;
 
        ARMARX_INFO << "Information: Color Stream Target Video Dimension : (" << Width << ","
                    << Height << ")" << flush;
 
        ARMARX_INFO << "Information: Color Stream Target Frame Rate : " << Fps << flush;
 
        CurrentColorStreamMode.setResolution(Width, Height);
 
        CurrentColorStreamMode.setFps(Fps);
 
 
        if (colorStreamOpenNI.setVideoMode(CurrentColorStreamMode) != openni::STATUS_OK)
        {
            ARMARX_ERROR << "Error: Cannot set Color stream settings (" << Width << "," << Height
                         << "," << Fps << ") Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        if (colorStreamOpenNI.start() != openni::STATUS_OK)
        {
            depthStreamOpenNI.destroy();
 
            ARMARX_ERROR << "Error: Cannot start Color stream on Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        CurrentColorStreamMode = colorStreamOpenNI.getVideoMode();
 
        ARMARX_INFO << "Information: Color Stream Active Video Dimension : ("
                    << CurrentColorStreamMode.getResolutionX() << ","
                    << CurrentColorStreamMode.getResolutionY() << ")" << flush;
 
        ARMARX_INFO << "Information: Color Stream Active Frame Rate : "
                    << CurrentColorStreamMode.getFps() << flush;
 
        if (!colorStreamOpenNI.isValid())
        {
            ARMARX_ERROR << "Error: Cannot create Color stream on Device (" << pDeviceURI << ") => "
                         << openni::OpenNI::getExtendedError() << flush;
 
            return false;
        }
 
        //-------------------------------------------------------------------------------------------------------------------------------------------------------------
 
        if (deviceOpenNI.setDepthColorSyncEnabled(true) != openni::STATUS_OK)
        {
            ARMARX_WARNING << "Warning: Cannot synchronize Depth and Color on Device ("
                           << pDeviceURI << ") => " << openni::OpenNI::getExtendedError() << flush;
        }
 
        if (deviceOpenNI.isImageRegistrationModeSupported(
                openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
        {
            if (deviceOpenNI.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) !=
                openni::STATUS_OK)
            {
                ARMARX_WARNING << "Warning: Device supports depth-to-color registration but could "
                                  "not be enabled ("
                               << pDeviceURI << ") =>" << openni::OpenNI::getExtendedError()
                               << flush;
            }
        }
 
        //-------------------------------------------------------------------------------------------------------------------------------------------------------------
 
        isModuleInitialized = true;
 
        return true;
    }
 
    bool
    OpenNIImageProvider::CreateDataStructures()
    {
        DestroyDataStructure();
 
        const int Width = videoDimension.width;
 
        const int Height = videoDimension.height;
 
        const int Area = Width * Height;
 
        coloredPoint3DBuffer = new visionx::ColoredPoint3D[Area];
 
        memset(coloredPoint3DBuffer, 0, sizeof(visionx::ColoredPoint3D) * Area);
 
        normalizedDepthCells = new NormalizedDepthCell[Area];
 
        memset(normalizedDepthCells, 0, sizeof(NormalizedDepthCell) * Area);
 
        InitializeNormalizedDepthCells();
 
        return true;
    }
 
    bool
    OpenNIImageProvider::InitializeNormalizedDepthCells()
    {
        if (!normalizedDepthCells)
        {
            return false;
        }
 
        const int Width = videoDimension.width;
 
        const int Height = videoDimension.height;
 
        const float XZFactor = std::tan(depthStreamOpenNI.getHorizontalFieldOfView() / 2.0f) * 2.0f;
 
        const float YZFactor = std::tan(depthStreamOpenNI.getVerticalFieldOfView() / 2.0f) * 2.0f;
 
        NormalizedDepthCell* pNormalizedDepthCell = normalizedDepthCells;
 
        for (int Y = 0; Y < Height; ++Y)
        {
            const float NY = (0.5f - (float(Y) / float(Height))) * YZFactor;
 
            for (int X = 0; X < Width; ++X, ++pNormalizedDepthCell)
            {
                pNormalizedDepthCell->nx = ((float(X) / float(Width)) - 0.5f) * XZFactor;
 
                pNormalizedDepthCell->ny = NY;
            }
        }
 
        return true;
    }
 
    bool
    OpenNIImageProvider::StopDevice()
    {
        if (isModuleInitialized)
        {
            depthStreamOpenNI.stop();
 
            depthStreamOpenNI.destroy();
 
            colorStreamOpenNI.stop();
 
            colorStreamOpenNI.destroy();
 
            deviceOpenNI.close();
 
            isModuleInitialized = false;
        }
 
        //TODO: Verifiy this out value
        return true;
    }
 
    bool
    OpenNIImageProvider::DestroyDataStructure()
    {
        if (coloredPoint3DBuffer)
        {
            delete[] coloredPoint3DBuffer;
 
            coloredPoint3DBuffer = NULL;
        }
 
        if (normalizedDepthCells)
        {
            delete[] normalizedDepthCells;
 
            normalizedDepthCells = NULL;
        }
 
        return true;
    }
 
    bool
    OpenNIImageProvider::CaptureColoredPoint()
    {
        if (!isModuleInitialized)
        {
            return false;
        }
 
        if (!coloredPoint3DBuffer)
        {
            return false;
        }
 
        if (!normalizedDepthCells)
        {
            return false;
        }
 
        openni::VideoStream* ppStreams[2] = {&depthStreamOpenNI, &colorStreamOpenNI};
 
        bool HasBeenCaptured[2] = {false, false};
 
        while (!(HasBeenCaptured[0] && HasBeenCaptured[1]))
        {
            int StreamIndex = -1;
 
            bool CaptureSuccesfull = false;
 
            for (int i = 0; i < 3; ++i)
            {
                if (openni::OpenNI::waitForAnyStream(ppStreams, 2, &StreamIndex, 5000) ==
                    openni::STATUS_OK)
                {
 
                    CaptureSuccesfull = true;
 
                    break;
                }
                else
                {
                    ARMARX_WARNING << "Warning: Capturing Try not succesfull ->"
                                   << openni::OpenNI::getExtendedError() << flush;
                }
            }
 
            if (!CaptureSuccesfull)
            {
                return false;
            }
 
            if (!(StreamIndex >= 0) && (StreamIndex <= 1))
            {
                return false;
            }
 
            if (ppStreams[StreamIndex]->readFrame(&frameOpenNI) != openni::STATUS_OK)
            {
                return false;
            }
 
            switch (StreamIndex)
            {
                case 0:
 
                    if (!DispatchDepthFrame())
                    {
                        return false;
                    }
 
                    break;
 
                case 1:
 
                    if (!DispatchColorFrame())
                    {
                        return false;
                    }
 
                    break;
 
                default:
                    return false;
            }
 
            HasBeenCaptured[StreamIndex] = true;
        }
 
 
        /*
          //Only for Testing and Debug
        const int Width = videoDimension.width;
 
        const int Height = videoDimension.height;
 
        CByteImage* pColorImage = Display(m_pColoredPoint3DBuffer,Width,Height,true);
 
        CByteImage* pDepthImage = Display(m_pColoredPoint3DBuffer,Width,Height,false);
 
        if(pColorImage)
        {
           pColorImage->SaveToFile("/home/gonzalez/ColorImage.bmp");
           delete pColorImage;
           pColorImage = NULL;
        }
 
        if(pDepthImage)
        {
           pDepthImage->SaveToFile("/home/gonzalez/DepthImage.bmp");
           delete pDepthImage;
           pDepthImage = NULL;
        }
        */
 
 
        return true;
    }
 
    bool
    OpenNIImageProvider::DispatchDepthFrame()
    {
        const NormalizedDepthCell* pNormalizedDepthCell = normalizedDepthCells;
 
        visionx::ColoredPoint3D* pColoredPoint3D = coloredPoint3DBuffer;
 
        const visionx::ColoredPoint3D* const pEndColoredPoint3D =
            coloredPoint3DBuffer + (videoDimension.width * videoDimension.height);
 
        uint16_t* pDepthPixel = (uint16_t*)(frameOpenNI.getData());
 
        while (pColoredPoint3D < pEndColoredPoint3D)
        {
            pColoredPoint3D->point.z = float(*pDepthPixel++);
 
            if (pColoredPoint3D->point.z)
            {
                pColoredPoint3D->point.x = pNormalizedDepthCell->nx * pColoredPoint3D->point.z;
 
                pColoredPoint3D->point.y = pNormalizedDepthCell->ny * pColoredPoint3D->point.z;
            }
            else
            {
                pColoredPoint3D->point.x = 0.0f;
 
                pColoredPoint3D->point.y = 0.0f;
            }
 
            ++pColoredPoint3D;
 
            ++pNormalizedDepthCell;
        }
 
        return true;
    }
 
    bool
    OpenNIImageProvider::DispatchColorFrame()
    {
        visionx::ColoredPoint3D* pColoredPoint3D = coloredPoint3DBuffer;
 
        const visionx::ColoredPoint3D* const pEndColoredPoint3D =
            coloredPoint3DBuffer + (videoDimension.width * videoDimension.height);
 
        unsigned char* pColorPixel = (unsigned char*)frameOpenNI.getData();
 
        while (pColoredPoint3D < pEndColoredPoint3D)
        {
            pColoredPoint3D->color.r = *pColorPixel++;
 
            pColoredPoint3D->color.g = *pColorPixel++;
 
            pColoredPoint3D->color.b = *pColorPixel++;
 
            pColoredPoint3D->color.a = 0;
 
            ++pColoredPoint3D;
        }
 
        return true;
    }
 
    int
    OpenNIImageProvider::EnsureFPS(const int FPS)
    {
        if (FPS == 30)
        {
            return FPS;
        }
 
        else if (FPS == 15)
        {
            return FPS;
        }
 
        const int DeviationTo30 = std::abs(FPS - 30);
 
        const int DeviationTo15 = std::abs(FPS - 15);
 
        ARMARX_WARNING << "Warning: Selected FPS = " << FPS << "not supported by device" << flush;
 
        if (DeviationTo30 < DeviationTo15)
        {
            ARMARX_ERROR << "Warning: Closest Fps = [30] supported will be used" << flush;
 
            return 30;
        }
        else
        {
            ARMARX_ERROR << "Warning: Closest Fps = [15] supported will be used" << flush;
 
            return 15;
        }
    }
} // namespace visionx