OpenNIPointCloudProvider.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::ArmarXObjects::OpenNIPointCloudProvider
 * @author     David Schiebener (schiebener at kit dot edu)
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "OpenNIPointCloudProvider.h"
 
#include <pcl/io/openni2/openni2_device_manager.h>
#include <pcl/io/pcd_io.h>
 
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/Calibration.h>
#include <Image/ImageProcessor.h>
 
 
using namespace armarx;
using namespace pcl;
using namespace cv;
 
#if PCL_VERSION > PCL_VERSION_CALC(1, 12, 0)
template <typename T>
using FctT = std::function<T>;
#else
template <typename T>
using FctT = boost::function<T>;
#endif
 
namespace visionx
{
    void
    OpenNIPointCloudProvider::onInitComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onInitComponent()";
 
        offeringTopicFromProperty("RobotHealthTopicName");
        offeringTopicFromProperty("DebugObserverName");
 
        ImageProvider::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
    }
 
    void
    OpenNIPointCloudProvider::onConnectComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onConnectComponent()";
 
        getTopicFromProperty(debugObserver, "DebugObserverName");
        getTopicFromProperty(robotHealthTopic, "RobotHealthTopicName");
 
        ImageProvider::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
    }
 
    void
    OpenNIPointCloudProvider::onDisconnectComponent()
    {
        ARMARX_TRACE;
        // hack to prevent deadlock
        captureEnabled = false;
        ImageProvider::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
    }
 
    void
    OpenNIPointCloudProvider::onExitComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onExitComponent()";
 
        ImageProvider::onExitComponent();
        CapturingPointCloudProvider::onExitComponent();
        ARMARX_INFO << "Resetting grabber interface";
        grabberInterface.reset();
    }
 
    bool
    OpenNIPointCloudProvider::loadCalibrationFile(std::string fileName,
                                                  visionx::CameraParameters& calibration)
    {
        ARMARX_TRACE;
        ArmarXDataPath::getAbsolutePath(fileName, fileName);
        FileStorage fs(fileName, FileStorage::READ);
 
        if (!fs.isOpened())
        {
            ARMARX_WARNING << "unable to read calibration file " << fileName;
            return false;
        }
 
        std::string cameraName = fs["camera_name"];
        ARMARX_LOG << "loading calibration file for " << cameraName;
 
 
        Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("dimensions").getValue();
        int imageWidth = fs["image_width"];
        int imageHeight = fs["image_height"];
 
        if (imageWidth != dimensions(0) || imageHeight != dimensions(1))
        {
            ARMARX_ERROR << "invalid camera size";
            return false;
        }
 
        Mat cameraMatrix, distCoeffs;
 
        fs["camera_matrix"] >> cameraMatrix;
        fs["distortion_coefficients"] >> distCoeffs;
 
        for (int i = 0; i < 5; i++)
        {
            calibration.distortion[i] = distCoeffs.at<float>(0, i);
        }
 
        calibration.focalLength[0] = cameraMatrix.at<float>(0, 0);
        calibration.focalLength[1] = cameraMatrix.at<float>(1, 1);
 
        calibration.principalPoint[0] = cameraMatrix.at<float>(0, 2);
        calibration.principalPoint[1] = cameraMatrix.at<float>(1, 2);
 
        return true;
    }
 
    void
    OpenNIPointCloudProvider::onInitCapturingPointCloudProvider()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onInitCapturingPointCloudProvider()";
 
 
        Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("dimensions").getValue();
 
        width = dimensions(0);
        height = dimensions(1);
 
        newPointCloudCapturingRequested = true;
        newImageCapturingRequested = true;
 
        ARMARX_VERBOSE << "creating OpenNI2Grabber " << width << "x" << height << "@" << frameRate
                       << " fps";
        std::string configuredDeviceId = getProperty<std::string>("OpenNIDeviceId").getValue();
        using namespace pcl::io::openni2;
        auto connectedDeviceURIs = *OpenNI2DeviceManager::getInstance()->getConnectedDeviceURIs();
        ARMARX_INFO << "connected devices: " << connectedDeviceURIs;
        if (!configuredDeviceId.empty() && configuredDeviceId.size() > 2)
        {
            ARMARX_TRACE;
            size_t i = 1;
            std::string deviceIndexNumber;
            ARMARX_INFO << "Trying to find device with configured URI: '" << configuredDeviceId
                        << "'";
            for (auto& uri : connectedDeviceURIs)
            {
                if (uri == configuredDeviceId)
                {
                    ARMARX_INFO << "Found device with URI " << configuredDeviceId << " at index "
                                << i;
                    deviceIndexNumber = "#" + std::to_string(i);
                    break;
                }
                i++;
            }
            if (deviceIndexNumber.empty())
            {
                ARMARX_WARNING << "Could not find device with URI: " << configuredDeviceId
                               << " - Connecting now to any device";
                grabberInterface.reset(new pcl::io::OpenNI2Grabber());
            }
            else
            {
                grabberInterface.reset(new pcl::io::OpenNI2Grabber(deviceIndexNumber));
            }
        }
        else if (!configuredDeviceId.empty())
        {
            ARMARX_TRACE;
            ARMARX_INFO << "Connecting to device id " << configuredDeviceId;
            grabberInterface.reset(new pcl::io::OpenNI2Grabber("#" + configuredDeviceId));
        }
        else
        {
            ARMARX_TRACE;
            grabberInterface.reset(new pcl::io::OpenNI2Grabber());
        }
        ARMARX_TRACE;
 
        visionx::CameraParameters RGBCameraIntrinsics;
        RGBCameraIntrinsics.focalLength.resize(2, 0.0f);
        RGBCameraIntrinsics.principalPoint.resize(2, 0.0f);
        RGBCameraIntrinsics.distortion.resize(4, 0.0f);
 
        //Eigen::Matrix3f id = Eigen::Matrix3f::Identity();
        RGBCameraIntrinsics.rotation =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
        RGBCameraIntrinsics.translation =
            visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
 
        ARMARX_TRACE;
        std::string rgbCameraCalibrationFile =
            getProperty<std::string>("RGBCameraCalibrationFile").getValue();
        if (!rgbCameraCalibrationFile.empty() &&
            loadCalibrationFile(rgbCameraCalibrationFile, RGBCameraIntrinsics))
        {
            ARMARX_TRACE;
            grabberInterface->setRGBCameraIntrinsics(RGBCameraIntrinsics.focalLength[0],
                                                     RGBCameraIntrinsics.focalLength[1],
                                                     RGBCameraIntrinsics.principalPoint[0],
                                                     RGBCameraIntrinsics.principalPoint[1]);
        }
        else
        {
            ARMARX_TRACE;
            ARMARX_INFO << "unable to load calibration file. using default values.";
            double fx, fy, cx, cy;
            grabberInterface->getRGBCameraIntrinsics(fx, fy, cx, cy);
 
            if (std::isnan(fx))
            {
                if (grabberInterface->getDevice()->hasColorSensor())
                {
                    fx = fy = grabberInterface->getDevice()->getColorFocalLength();
                }
 
                cx = (width - 1) / 2.0;
                cy = (height - 1) / 2.0;
            }
            ARMARX_INFO << "rgb camera fx: " << fx << " fy: " << fy << " cx: " << cx
                        << " cy: " << cy;
 
            RGBCameraIntrinsics.principalPoint[0] = cx;
            RGBCameraIntrinsics.principalPoint[1] = cy;
            RGBCameraIntrinsics.focalLength[0] = fx;
            RGBCameraIntrinsics.focalLength[1] = fy;
            RGBCameraIntrinsics.width = width;
            RGBCameraIntrinsics.height = height;
        }
 
        visionx::CameraParameters depthCameraIntrinsics;
        depthCameraIntrinsics.focalLength.resize(2, 0.0f);
        depthCameraIntrinsics.principalPoint.resize(2, 0.0f);
        depthCameraIntrinsics.distortion.resize(4, 0.0f);
        depthCameraIntrinsics.rotation =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
        depthCameraIntrinsics.translation =
            visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
 
        std::string depthCameraCalibrationFile =
            getProperty<std::string>("depthCameraCalibrationFile").getValue();
        if (!depthCameraCalibrationFile.empty() &&
            loadCalibrationFile(depthCameraCalibrationFile, depthCameraIntrinsics))
        {
            ARMARX_TRACE;
            grabberInterface->setDepthCameraIntrinsics(depthCameraIntrinsics.focalLength[0],
                                                       depthCameraIntrinsics.focalLength[1],
                                                       depthCameraIntrinsics.principalPoint[0],
                                                       depthCameraIntrinsics.principalPoint[1]);
        }
        else
        {
            ARMARX_TRACE;
            ARMARX_INFO << "unable to load calibration file. using default values.";
            double fx, fy, cx, cy;
            grabberInterface->getDepthCameraIntrinsics(fx, fy, cx, cy);
 
            if (std::isnan(fx))
            {
                if (grabberInterface->getDevice()->hasDepthSensor())
                {
                    fx = fy = grabberInterface->getDevice()->getDepthFocalLength();
                }
                cx = (width - 1) / 2.0;
                cy = (height - 1) / 2.0;
            }
            ARMARX_INFO << "depth camera fx: " << fx << " fy: " << fy << " cx: " << cx
                        << " cy: " << cy;
 
            depthCameraIntrinsics.principalPoint[0] = cx;
            depthCameraIntrinsics.principalPoint[1] = cy;
            depthCameraIntrinsics.focalLength[0] = fx;
            depthCameraIntrinsics.focalLength[1] = fy;
            depthCameraIntrinsics.width = width;
            depthCameraIntrinsics.height = height;
        }
 
        ARMARX_TRACE;
 
        calibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
        calibration.calibrationRight = visionx::tools::createDefaultMonocularCalibration();
        calibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
        calibration.calibrationRight.cameraParam = depthCameraIntrinsics;
        calibration.rectificationHomographyLeft =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
        calibration.rectificationHomographyRight =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
 
        ARMARX_INFO << "Baseline:" << grabberInterface->getDevice()->getBaseline();
 
        pointcloudBuffer = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(
            new pcl::PointCloud<pcl::PointXYZRGBA>(width, height));
 
        rgbImages = new CByteImage*[2];
 
        rgbImages[0] = new CByteImage(width, height, CByteImage::eRGB24);
        rgbImages[1] = new CByteImage(width, height, CByteImage::eRGB24);
        ::ImageProcessor::Zero(rgbImages[0]);
        ::ImageProcessor::Zero(rgbImages[1]);
 
        if (grabberInterface->getDevice()->hasColorSensor())
        {
            FctT<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> pointCloudCallback =
                [this](auto&& PH1)
            {
                callbackFunctionForOpenNIGrabberPointCloudWithRGB(std::forward<decltype(PH1)>(PH1));
            };
            grabberInterface->registerCallback(pointCloudCallback);
 
            FctT<void(const pcl::io::Image::Ptr&, const pcl::io::DepthImage::Ptr&, float)>
                imageCallback = [this](auto&& PH1, auto&& PH2, auto&& PH3)
            {
                grabImages(std::forward<decltype(PH1)>(PH1),
                           std::forward<decltype(PH2)>(PH2),
                           std::forward<decltype(PH3)>(PH3));
            };
            grabberInterface->registerCallback(imageCallback);
        }
        else
        {
            ARMARX_INFO << "depth sensor. no color image available.";
 
            FctT<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> pointCloudCallback =
                [this](auto&& PH1)
            { callbackFunctionForOpenNIGrabberPointCloud(std::forward<decltype(PH1)>(PH1)); };
            grabberInterface->registerCallback(pointCloudCallback);
 
            FctT<void(const pcl::io::IRImage::Ptr&, const pcl::io::DepthImage::Ptr&, float)>
                imageCallback = [this](auto&& PH1, auto&& PH2, auto&& PH3)
            {
                grabDepthImage(std::forward<decltype(PH1)>(PH1),
                               std::forward<decltype(PH2)>(PH2),
                               std::forward<decltype(PH3)>(PH3));
            };
            grabberInterface->registerCallback(imageCallback);
        }
    }
 
    void
    OpenNIPointCloudProvider::onConnectPointCloudProvider()
    {
        ARMARX_CHECK_NOT_NULL(heartbeatPlugin);
        heartbeatPlugin->signUp(armarx::Duration::MilliSeconds(500),
                                armarx::Duration::MilliSeconds(1000),
                                {"Vision", "Camera"},
                                "OpenNIPointCloudProvider");
    }
 
    void
    OpenNIPointCloudProvider::onStartCapture(float frameRate)
    {
        ARMARX_INFO << "Openni grabber interface name: " << grabberInterface->getName();
        ARMARX_INFO << "Openni device interface name: " << grabberInterface->getDevice()->getName()
                    << " id: " << grabberInterface->getDevice()->getStringID();
 
 
        grabberInterface->getDevice()->setAutoExposure(
            getProperty<bool>("EnableAutoExposure").getValue());
        grabberInterface->getDevice()->setAutoWhiteBalance(
            getProperty<bool>("EnableAutoWhiteBalance").getValue());
 
        grabberInterface->start();
 
        lastReportTimestamp = TimeUtil::GetTime();
    }
 
    void
    OpenNIPointCloudProvider::onStopCapture()
    {
        ARMARX_TRACE;
        ARMARX_INFO << "Stopping OpenNI Grabber Interface";
        if (grabberInterface)
        {
            grabberInterface->stop();
        }
        ARMARX_INFO << "Stopped OpenNI Grabber Interface";
 
        std::scoped_lock lock(syncMutex);
 
        condition.notify_one();
    }
 
    void
    OpenNIPointCloudProvider::onExitCapturingPointCloudProvider()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onExitCapturingPointCloudProvider()";
 
        delete rgbImages[0];
        delete rgbImages[1];
 
        delete[] rgbImages;
    }
 
    void
    OpenNIPointCloudProvider::onInitImageProvider()
    {
        ARMARX_TRACE;
        Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("dimensions").getValue();
        setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
        setNumberImages(2);
    }
 
    StereoCalibration
    OpenNIPointCloudProvider::getStereoCalibration(const Ice::Current&)
    {
        return calibration;
    }
 
    void
    OpenNIPointCloudProvider::callbackFunctionForOpenNIGrabberPointCloudWithRGB(
        const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& newCloud)
    {
        ARMARX_TRACE;
        ARMARX_INFO << deactivateSpam(1e10) << "got first pointcloud";
        if (newPointCloudCapturingRequested)
        {
            long offset = getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f;
            timeOfLastPointCloudCapture = IceUtil::Time::now().toMicroSeconds() - offset;
            pcl::copyPointCloud(*newCloud, *pointcloudBuffer);
            std::scoped_lock lock(syncMutex);
            auto size = newCloud->size();
            pointcloudBuffer->resize(size);
            float scaling = 1000.f;
            for (size_t i = 0; i < size; i++)
            {
                pointcloudBuffer->at(i).x = newCloud->at(i).x * scaling;
                pointcloudBuffer->at(i).y = newCloud->at(i).y * scaling;
                pointcloudBuffer->at(i).z = newCloud->at(i).z * scaling;
            }
 
            newPointCloudCapturingRequested = false;
 
            condition.notify_one();
        }
    }
 
    void
    OpenNIPointCloudProvider::callbackFunctionForOpenNIGrabberPointCloud(
        const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& newCloud)
    {
        ARMARX_TRACE;
        ARMARX_INFO << deactivateSpam(1e10) << "got first pointcloud";
 
        if (newPointCloudCapturingRequested)
        {
 
            long offset = getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f;
            timeOfLastPointCloudCapture = IceUtil::Time::now().toMicroSeconds() - offset;
            Eigen::Affine3f transform(Eigen::Scaling(1000.0f));
 
            std::scoped_lock lock(syncMutex);
            pcl::copyPointCloud(*newCloud, *pointcloudBuffer);
            pcl::transformPointCloud(*pointcloudBuffer, *pointcloudBuffer, transform);
 
 
            newPointCloudCapturingRequested = false;
 
            condition.notify_one();
        }
    }
 
    void
    OpenNIPointCloudProvider::grabDepthImage(const pcl::io::IRImage::Ptr& IRImage,
                                             const pcl::io::DepthImage::Ptr& depthImage,
                                             float x)
    {
        ARMARX_TRACE;
        ARMARX_INFO << deactivateSpam(1e10) << "got first depthImage";
 
        if (newImageCapturingRequested)
        {
            long offset = getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f;
            timeOfLastImageCapture = IceUtil::Time::now().toMicroSeconds() - offset;
            cv::Mat depthBuffer(height, width, CV_16UC1);
            depthImage->fillDepthImageRaw(width, height, (unsigned short*)depthBuffer.data);
 
            std::scoped_lock lock(syncMutex);
 
            //            boost::chrono::high_resolution_clock::time_point timestamp = depthImage->getSystemTimestamp(); <-- wrong timestamp!
 
            for (int j = 0; j < depthBuffer.rows; j++)
            {
                int index = 3 * (j * width);
                for (int i = 0; i < depthBuffer.cols; i++)
                {
                    unsigned short value = depthBuffer.at<unsigned short>(j, i);
                    tools::depthValueToRGB(value,
                                           rgbImages[1]->pixels[index + 0],
                                           rgbImages[1]->pixels[index + 1],
                                           rgbImages[1]->pixels[index + 2],
                                           false);
                    //                    rgbImages[1]->pixels[3 * (j * width + i) + 0] = value & 0xFF;
                    //                    rgbImages[1]->pixels[3 * (j * width + i) + 1] = (value >> 8) & 0xFF;
                    index += 3;
                }
            }
 
 
            newImageCapturingRequested = false;
 
            condition.notify_one();
        }
    }
 
    void
    OpenNIPointCloudProvider::grabImages(const pcl::io::Image::Ptr& RGBImage,
                                         const pcl::io::DepthImage::Ptr& depthImage,
                                         float reciprocalFocalLength)
    {
        ARMARX_TRACE;
        ARMARX_INFO << deactivateSpam(1e10) << "got first image";
 
        if (newImageCapturingRequested)
        {
            long offset = getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f;
            timeOfLastImageCapture = IceUtil::Time::now().toMicroSeconds() - offset;
 
            cv::Mat depthBuffer(height, width, CV_16UC1);
            depthImage->fillDepthImageRaw(width, height, (unsigned short*)depthBuffer.data);
 
            std::scoped_lock lock(syncMutex);
 
            //            boost::chrono::high_resolution_clock::time_point timestamp = depthImage->getSystemTimestamp(); <-- wrong timestamp!
 
 
            for (int j = 0; j < depthBuffer.rows; j++)
            {
                int index = 3 * (j * width);
                for (int i = 0; i < depthBuffer.cols; i++)
                {
                    unsigned short value = depthBuffer.at<unsigned short>(j, i);
                    tools::depthValueToRGB(value,
                                           rgbImages[1]->pixels[index + 0],
                                           rgbImages[1]->pixels[index + 1],
                                           rgbImages[1]->pixels[index + 2],
                                           false);
                    //                    rgbImages[1]->pixels[3 * (j * width + i) + 0] = value & 0xFF;
                    //                    rgbImages[1]->pixels[3 * (j * width + i) + 1] = (value >> 8) & 0xFF;
                    index += 3;
                }
            }
 
            /*
 
 
                            cv::Mat depthBuffer = cv::Mat(height, width, CV_32FC1);
                            depthImage->fillDisparityImage(width, height, (float*) depthBuffer.data);
                            boost::mutex::scoped_lock lock(captureMutex);
 
                            calibration.calibrationRight.cameraParam.translation[0] = depthImage->getBaseline();
 
                            for (int j = 0; j < depthBuffer.rows; j++)
                            {
                                for (int i = 0; i < depthBuffer.cols; i++)
                                {
                                    int value = static_cast<int>(depthBuffer.at<float>(j, i));
                                    rgbImages[1]->pixels[3 * (j * width + i) + 0] = value & 0xFF;
                                    rgbImages[1]->pixels[3 * (j * width + i) + 1] = (value >> 8) & 0xFF;
                                    rgbImages[1]->pixels[3 * (j * width + i) + 2] = (value >> 16) & 0xFF;
                                }
                            }            for (int j = 0; j < depthBuffer.rows; j++)
            {
                for (int i = 0; i < depthBuffer.cols; i++)
                {
                    unsigned short value = depthBuffer.at<unsigned short>(j, i);
 
                    rgbImages[1]->pixels[3 * (j * width + i) + 0] = value & 0xFF;
                    rgbImages[1]->pixels[3 * (j * width + i) + 1] = (value >> 8) & 0xFF;
                }
            }
 
            */
 
            RGBImage->fillRGB(width, height, rgbImages[0]->pixels);
 
            newImageCapturingRequested = false;
 
            condition.notify_one();
        }
    }
 
    bool
    OpenNIPointCloudProvider::doCapture()
    {
        ARMARX_TRACE;
        std::unique_lock lock(syncMutex);
 
        newPointCloudCapturingRequested = true;
        newImageCapturingRequested = true;
 
        while (captureEnabled && (newPointCloudCapturingRequested || newImageCapturingRequested))
        {
            //            condition.wait(lock);
            condition.wait_for(lock, std::chrono::milliseconds(100));
        }
 
        if (!captureEnabled)
        {
            return false;
        }
 
        // provide images
        provideImages(rgbImages, IceUtil::Time::microSeconds(timeOfLastImageCapture));
        // provide pointclpoud
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointcloud(
            new pcl::PointCloud<PointXYZRGBA>(*pointcloudBuffer));
        //        pointcloud->header.stamp = pointcloudBuffer->header.stamp; <<- wrong timestamp!
        pointcloud->header.stamp = timeOfLastPointCloudCapture;
        providePointCloud(pointcloud);
 
        heartbeatPlugin->heartbeat();
 
        IceUtil::Time now = TimeUtil::GetTime();
        IceUtil::Time reportTime = now - lastReportTimestamp;
 
        StringVariantBaseMap durations;
        durations["report_time_ms"] = new armarx::Variant(reportTime.toMilliSecondsDouble());
        debugObserver->setDebugChannel(getName(), durations);
 
 
        lastReportTimestamp = now;
 
        return true;
    }
 
    std::vector<imrec::ChannelPreferences>
    OpenNIPointCloudProvider::getImageRecordingChannelPreferences(const Ice::Current&)
    {
        ARMARX_TRACE;
 
        imrec::ChannelPreferences rgb;
        rgb.requiresLossless = false;
        rgb.name = "rgb";
 
        imrec::ChannelPreferences depth;
        depth.requiresLossless = true;
        depth.name = "depth";
 
        return {rgb, depth};
    }
 
    PropertyDefinitionsPtr
    OpenNIPointCloudProvider::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(
            new OpenNIPointCloudProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
    OpenNIPointCloudProvider::OpenNIPointCloudProvider()
    {
        addPlugin(heartbeatPlugin);
    }
} // namespace visionx