NerianVisionImageProvider.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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::NerianVisionImageProvider
 * @author     Julian Tusch <julian.tusch@student.kit.edu>
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "NerianVisionImageProvider.h"
 
#include <cstring>
#include <exception>
#include <memory>
#include <string>
#include <vector>
 
#include <opencv2/imgproc.hpp>
 
#include <Ice/Config.h>
#include <Ice/Current.h>
#include <IceUtil/Time.h>
 
 
// Nerian Vision SDK
#include <visiontransfer/deviceenumeration.h>
#include <visiontransfer/deviceinfo.h>
#include <visiontransfer/deviceparameters.h>
#include <visiontransfer/imageset.h>
#include <visiontransfer/imagetransfer.h>
 
// IVT
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
 
// ArmarX
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
// VisionX
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/interface/core/DataTypes.h>
#include <VisionX/libraries/imrec/helper.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
namespace visionx
{
    void
    NerianVisionImageProvider::onInitImageProvider()
    {
        using visiontransfer::DeviceEnumeration;
        using visiontransfer::DeviceInfo;
        using visiontransfer::DeviceParameters;
 
        DeviceEnumeration deviceEnum;
        DeviceEnumeration::DeviceList devices = deviceEnum.discoverDevices();
        if (devices.size() == 0)
        {
            ARMARX_INFO << "No Nerian Vision devices detected! Shutting down...";
            getArmarXManager()->asyncShutdown();
            throw armarx::LocalException("No Nerian Vision devices detected!");
        }
 
        device = devices[0];
        auto deviceModel = device.getModel();
        setMetaInfo("model", new armarx::Variant(getNameForDeviceModel(deviceModel)));
        setMetaInfo("serialNumber", new armarx::Variant(device.getSerialNumber()));
        setMetaInfo("firmwareVersion", new armarx::Variant(device.getFirmwareVersion()));
 
        ARMARX_INFO << "Using Nerian Vision device: " << device.toString();
 
        // the Ruby camera has an additional color camera
        enableColorCamera = getProperty<bool>("enableColorCamera").getValue();
        int numImages = (deviceModel == DeviceInfo::RUBY && enableColorCamera) ? 3 : 2;
        setNumberImages(numImages);
 
        visionx::ImageDimension dimensions;
 
        std::unique_ptr<DeviceParameters> parameterPtr = nullptr;
        try
        {
            parameterPtr = std::make_unique<DeviceParameters>(device);
        }
        catch (const std::exception& e)
        {
            ARMARX_INFO << "Failed to open device parameters."
                        << " (" << e.what() << ")";
            return;
        }
 
        try
        {
            auto dim = parameterPtr->getParameter("RT_output_image_size").getTensorData();
            dimensions.width = static_cast<int>(dim[0]);
            dimensions.height = static_cast<int>(dim[1]);
        }
        catch (const std::exception& e)
        {
            ARMARX_INFO << "Failed to get dimension from device parameters. Using app property "
                           "`dimensions` as fallback."
                        << " (" << e.what() << ")";
            dimensions = getProperty<visionx::ImageDimension>("dimensions").getValue();
        }
 
        ARMARX_DEBUG << "Using image dimensions: " << dimensions.width << "x" << dimensions.height;
        setImageFormat(dimensions, visionx::eRgb, visionx::eBayerPatternRg);
 
        ARMARX_DEBUG << "Configuring device.";
 
        rectifyImages = getProperty<bool>("rectifyImages").getValue();
        try
        {
            if (deviceModel == DeviceInfo::DeviceModel::RUBY)
            {
                parameterPtr->setParameter("output_channel_left_enabled", true);
                parameterPtr->setParameter("output_channel_right_enabled", true);
                parameterPtr->setParameter("output_channel_color_enabled", enableColorCamera);
                parameterPtr->setParameter("output_channel_color_mode", 0);
                parameterPtr->setParameter("output_channel_disparity_enabled", false);
            }
 
            parameterPtr->setOperationMode(rectifyImages
                                               ? visiontransfer::DeviceParameters::RECTIFY
                                               : DeviceParameters::OperationMode::PASS_THROUGH);
        }
        catch (const std::exception& e)
        {
            ARMARX_INFO << "Failed to configure device." << " (" << e.what() << ")";
            ARMARX_WARNING
                << "If the images are not displayed as expected, manual configuration of the "
                << "device may be necessary. To do this, open the web interface of the device at "
                << device.getIpAddress() << " and verify/set the following settings: "
                << (deviceModel == DeviceInfo::DeviceModel::RUBY
                        ? "Output channels [left, right, color] are enabled and the "
                          "disparity output channel is disabled. Optionally, change the "
                          "color output mode to not use projection in order to avoid artifacts."
                        : "")
                << "Operation mode is set to " << (rectifyImages ? "RECTIFY" : "PASS_THROUGH")
                << ".";
        }
    }
 
    void
    NerianVisionImageProvider::onConnectImageProvider()
    {
        ARMARX_DEBUG << "Connecting " << getName();
 
        imageProcessingTask = armarx::RunningTask<NerianVisionImageProvider>::pointer_type(
            new armarx::RunningTask<NerianVisionImageProvider>(
                this, &NerianVisionImageProvider::runImagePublishing));
        imageProcessingTask->start();
    }
 
    void
    NerianVisionImageProvider::runImagePublishing()
    {
        using visiontransfer::DeviceParameters;
        using visiontransfer::ImageSet;
        using visiontransfer::ImageTransfer;
 
        ARMARX_DEBUG << "Started image processing task.";
 
        std::unique_ptr<ImageTransfer> imageTransferPtr = nullptr;
        try
        {
            imageTransferPtr = std::make_unique<ImageTransfer>(device);
        }
        catch (const std::exception& e)
        {
            ARMARX_INFO << "Failed to open device."
                        << " (" << e.what() << ")";
            return;
        }
 
        if (imageTransferPtr == nullptr)
        {
            ARMARX_INFO << "Failed to connnect to device!";
            return;
        }
 
        while (not imageProcessingTask->isStopped())
        {
            ImageSet imageSet;
            bool status = false;
            try
            {
                // receiveImageSet() has an internal timout of 100ms
                status = imageTransferPtr->receiveImageSet(imageSet);
            }
            catch (const std::exception& e)
            {
                ARMARX_INFO << deactivateSpam(30) << "Error receiving images from device."
                            << " (" << e.what() << ")";
                continue;
            }
 
            if (!status)
            {
                ARMARX_INFO << deactivateSpam(30) << "Did not get frame until timeout of 100ms.";
                continue;
            }
 
            int numImages = getNumberImages();
            if (numImages != imageSet.getNumberOfImages())
            {
                ARMARX_INFO << deactivateSpam(30) << "Expected " << numImages << " images, but got "
                            << imageSet.getNumberOfImages()
                            << " images. Please verify the device and component configurations.";
                continue;
            }
 
            if (!imageSet.hasImageType(ImageSet::ImageType::IMAGE_LEFT) ||
                !imageSet.hasImageType(ImageSet::ImageType::IMAGE_RIGHT) ||
                (numImages == 3 && !imageSet.hasImageType(ImageSet::ImageType::IMAGE_COLOR)))
            {
                ARMARX_INFO << deactivateSpam(30)
                            << "Received image set does not contain the expected image types. "
                               "Please verify the device and component configurations, then "
                               "restart this component.";
                continue;
            }
 
            bool success = true;
            std::vector<CByteImage*> imageBuffers(numImages);
 
            for (int i = 0; i < numImages; i++)
            {
                auto imageType = imageSet.getImageType(i);
                auto imageFormat = imageSet.getPixelFormat(imageType);
 
                if (imageType == ImageSet::ImageType::IMAGE_DISPARITY ||
                    imageType == ImageSet::ImageType::IMAGE_UNDEFINED ||
                    (!enableColorCamera && imageType == ImageSet::ImageType::IMAGE_COLOR))
                {
                    ARMARX_INFO << deactivateSpam(30) << "Received unexpected image type: "
                                << ImageSet::getNameForImageType(imageType);
                    success = false;
                    break;
                }
 
                cv::Mat bgr_image;
                if (imageFormat == ImageSet::ImageFormat::FORMAT_8_BIT_MONO)
                {
                    cv::Mat grayscale_image;
                    imageSet.toOpenCVImage(i, grayscale_image);
                    cv::cvtColor(grayscale_image, bgr_image, cv::COLOR_GRAY2BGR);
                }
                else if (imageFormat == ImageSet::ImageFormat::FORMAT_8_BIT_RGB)
                {
                    imageSet.toOpenCVImage(i, bgr_image, true);
                }
                else
                {
                    ARMARX_INFO << deactivateSpam(30) << "Received unexpected image format: "
                                << ImageSet::getNameForImageFormat(imageFormat);
                    success = false;
                    break;
                }
 
                imageBuffers[i] = new CByteImage();
                visionx::imrec::convert(bgr_image, *imageBuffers[i]);
            }
 
            if (success)
            {
                int t_s = 0;
                int t_us = 0;
                imageSet.getTimestamp(t_s, t_us);
                const IceUtil::Time timestamp =
                    IceUtil::Time::seconds(t_s) + IceUtil::Time::microSeconds(t_us);
                provideImages(imageBuffers.data(), timestamp);
            }
        }
 
        ARMARX_DEBUG << "Stopped image processing task.";
    }
 
    void
    NerianVisionImageProvider::onDisconnectImageProvider()
    {
        ARMARX_DEBUG << "Disconnecting " << getName();
 
        // Stop task
        {
            ARMARX_DEBUG << "Stopping image processing thread...";
            imageProcessingTask->stop(false);
            ARMARX_DEBUG << "Waiting for image processing thread to stop...";
            imageProcessingTask->waitForStop();
            ARMARX_DEBUG << "Image processing thread stopped";
        }
    }
 
    void
    NerianVisionImageProvider::onExitImageProvider()
    {
    }
 
    void
    NerianVisionImageProvider::startCapture(Ice::Float /*framesPerSecond*/,
                                            const Ice::Current& /*c*/)
    {
    }
 
    void
    NerianVisionImageProvider::stopCapture(const Ice::Current& /*c*/)
    {
    }
 
    std::string
    NerianVisionImageProvider::getReferenceFrame(const Ice::Current& /*c*/)
    {
        return {};
    }
 
    bool
    NerianVisionImageProvider::getImagesAreUndistorted(const Ice::Current& /*c*/)
    {
        return rectifyImages;
    }
 
    StereoCalibration
    NerianVisionImageProvider::getStereoCalibration(const Ice::Current& /*c*/)
    {
        StereoCalibration calibration;
 
        // TODO: find out how to retrieve the calibration data from nerian vision devices
        // calibration.calibrationLeft = calibration.calibrationRight = ...
 
        calibration.rectificationHomographyLeft = calibration.rectificationHomographyRight =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
 
        return calibration;
    }
 
    armarx::PropertyDefinitionsPtr
    NerianVisionImageProvider::createPropertyDefinitions()
    {
        return {new NerianVisionImageProviderPropertyDefinitions(getConfigIdentifier())};
    }
} // namespace visionx