MultiImageProvider.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::MultiImageProvider
 * @author     Mirko Waechter
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MultiImageProvider.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Image/ImageProcessor.h>
 
namespace armarx
{
 
 
    void
    MultiImageProvider::onInitImageProcessor()
    {
        auto providersRaw = getProperty<std::string>("ImageProviders").getValue();
        auto providerSplit = armarx::Split(providersRaw, ";", true, true);
        ARMARX_CHECK_POSITIVE(providerSplit.size());
        for (std::string providerStr : providerSplit)
        {
            auto split = armarx::Split(providerStr, ":", true, true);
            ARMARX_CHECK_POSITIVE(split.size());
            auto name = split[0];
            std::set<size_t> imageSelection;
            if (split.size() >= 2)
            {
                auto imageSelectionVec = armarx::Split<size_t>(split.at(1), ",", true, true);
                imageSelection =
                    std::set<size_t>(imageSelectionVec.begin(), imageSelectionVec.end());
            }
            else
 
            {
                ARMARX_LOG << "name" << name;
            }
            usingImageProvider(name);
            imageProviderData[name].imageSelection = imageSelection;
        }
    }
 
    void
    MultiImageProvider::onConnectImageProcessor()
    {
        std::unique_lock lock(mutex);
        visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
        ::visionx::ImageDimension maxDim;
        for (auto& pair : imageProviderData)
        {
            auto name = pair.first;
            auto& data = pair.second;
            auto& selection = data.imageSelection;
            data.providerInfo = getImageProvider(name, imageDisplayType);
            const auto& imageProviderInfo = data.providerInfo;
            if (selection.empty())
            {
                for (int i = 0; i < imageProviderInfo.numberImages; i++)
                {
                    selection.insert(i);
                }
            }
            data.numImages = imageProviderInfo.numberImages;
            if (imageProviderInfo.imageFormat != imageProviderInfo.imageFormat)
            {
                ARMARX_ERROR << "image format does not match.";
            }
            maxDim.width = std::max(imageProviderInfo.imageFormat.dimension.width, maxDim.width);
            maxDim.height = std::max(imageProviderInfo.imageFormat.dimension.height, maxDim.height);
 
            data.srcCameraImages.clear();
 
            for (auto& index : selection)
            {
                (void)index; // Unused.
                data.srcCameraImages.push_back(visionx::CByteImageUPtr(
                    new CByteImage(imageProviderInfo.imageFormat.dimension.width,
                                   imageProviderInfo.imageFormat.dimension.height,
                                   CByteImage::eRGB24)));
            }
        }
        ARMARX_INFO << "max image dimension: " << maxDim.width << "x" << maxDim.height;
        totalNumberOfImages = 0;
        for (auto& pair : imageProviderData)
        {
            auto name = pair.first;
            auto& data = pair.second;
            auto& selection = data.imageSelection;
            data.targetCameraImages.clear();
            data.resize = maxDim.width != data.providerInfo.imageFormat.dimension.width ||
                          maxDim.height != data.providerInfo.imageFormat.dimension.height;
            ARMARX_INFO << name
                        << ": image dimension: " << data.providerInfo.imageFormat.dimension.width
                        << "x" << data.providerInfo.imageFormat.dimension.height
                        << " resizing: " << data.resize;
 
            for (auto& index : selection)
            {
                (void)index; // Unused.
                data.targetCameraImages.push_back(visionx::CByteImageUPtr(
                    new CByteImage(maxDim.width, maxDim.height, CByteImage::eRGB24)));
                totalNumberOfImages++;
            }
        }
        enableResultImages(totalNumberOfImages, maxDim, imageDisplayType);
    }
 
    void
    MultiImageProvider::onExitImageProcessor()
    {
        //    std::unique_lock lock(mutex);
        //    for (int i = 0; i < numImages; i++)
        //    {
        //        delete cameraImages[i];
        //    }
 
        //    delete [] cameraImages;
 
        //    if (scaleFactorX > 0.0)
        //    {
        //        for (int i = 0; i < numImages; i++)
        //        {
        //            delete scaledCameraImages[i];
        //        }
        //    }
        //    delete [] scaledCameraImages;
    }
 
    void
    MultiImageProvider::process()
    {
        std::unique_lock lock(mutex);
        std::vector<CByteImage*> images;
        for (auto& pair : imageProviderData)
        {
            const auto name = pair.first;
            auto& data = pair.second;
            //        const auto& selection = data.imageSelection;
            if (!isNewImageAvailable(name))
            {
                if (!waitForImages(name))
                {
                    ARMARX_WARNING << "Timeout while waiting for images";
                    return;
                }
            }
            armarx::MetaInfoSizeBasePtr info;
            std::vector<CByteImage*> tmpImageVec;
            for (auto& image : data.srcCameraImages)
            {
                tmpImageVec.push_back(image.get());
            }
            auto numImg = getImages(name, tmpImageVec.data(), info);
 
            if (numImg != data.numImages)
            {
                ARMARX_WARNING << deactivateSpam(5)
                               << "Unable to transfer or read images: returned number of images "
                               << numImg << " expected: " << data.numImages;
                return;
            }
            size_t i = 0;
            for (auto& image : data.srcCameraImages)
            {
                if (!data.resize)
                {
                    images.push_back(image.get());
                }
                else
                {
                    auto targetImg = data.targetCameraImages.at(i).get();
                    ::ImageProcessor::Resize(image.get(), targetImg);
                    images.push_back(targetImg);
                }
                i++;
            }
        }
        ARMARX_CHECK_EQUAL(totalNumberOfImages, images.size());
        provideResultImages(images.data());
    }
 
    armarx::PropertyDefinitionsPtr
    MultiImageProvider::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new MultiImageProviderPropertyDefinitions(getConfigIdentifier()));
    }
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::MultiImageProvider, ::armarx::MultiImageProvider::GetDefaultName());
 
} // namespace armarx