ArMemToImageProvider.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::ArMemToImage
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ArMemToImageProvider.h"
 
#include <algorithm>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/core/error.h>
 
 
namespace visionx
{
 
    std::string ArMemToImageProvider::getDefaultName() const
    {
        return "ArMemToImageProvider";
    }
 
 
    armarx::PropertyDefinitionsPtr ArMemToImageProvider::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->topic(debugObserver);
 
        defs->optional(p.providerSegmentName, "img.ProviderSegmentName",
                       "Name of the provider segment(s).");
        defs->optional(p.resultImagesName, "img.ResultImagesName",
                       "Name of the resulting (provided) images.\n"
                       "If '(auto)', the provider segment name is used.");
 
        defs->defineOptionalPropertyVector("img.FallbackDimensions", p.fallbackDims,
                                           "Image dimensions to use when they cannot be fetched from memory.", 'x');
 
        p.images.define(*defs);
 
        return defs;
    }
 
 
    void ArMemToImageProvider::onInitComponent()
    {
        // Parse properties.
        getProperty(p.fallbackDims, "img.FallbackDimensions");
        if (p.resultImagesName == "(auto)")
        {
            p.resultImagesName = p.providerSegmentName;
        }
        p.images.read(*this, p.providerSegmentName);
 
        // Setup images.
        armemToImage.addImagesRGB(p.images.rgbEntityID, p.images.rgbIndices);
        armemToImage.addImagesDepth(p.images.depthEntityID, p.images.depthIndices);
        ARMARX_INFO << "Result image structure: \n" << armemToImage.summarizeStructure();
 
        // Connect to memory updates.
        auto callback = [this](const armarx::armem::MemoryID & entityID, const std::vector<armarx::armem::MemoryID>& updatedSnapshotIDs)
        {
            this->fetchUpdates(entityID, updatedSnapshotIDs);
        };
        memoryNameSystem().subscribe(p.images.rgbEntityID, callback);
        memoryNameSystem().subscribe(p.images.depthEntityID, callback);
    }
 
    void ArMemToImageProvider::onConnectComponent()
    {
        armemToImage.setDebugObserver(debugObserver, getName());
        {
            try
            {
                armemToImage.memoryReader = memoryNameSystem().useReader(armemToImage.getMemoryID());
            }
            catch (const armarx::armem::error::CouldNotResolveMemoryServer& e)
            {
                ARMARX_ERROR << e.what();
            }
        }
 
        ARMARX_VERBOSE << "Trying initial fetch ...";
        Eigen::Vector2i dimensions = p.fallbackDims;
 
        armemToImage.fetchLatest();
        if (armemToImage.updated)
        {
            dimensions = armemToImage.getImageDimensions();
            ARMARX_VERBOSE << "Image formats: \n" << armemToImage.printFormats();
        }
        // ToDo: We (or the result image provider) could also wait until images are available.
        enableResultImages(armemToImage.images.size(), { dimensions(0), dimensions(1) }, eRgb, p.resultImagesName);
    }
 
    void ArMemToImageProvider::onDisconnectComponent()
    {
        if (resultImageProvider)
        {
            getArmarXManager()->removeObjectBlocking(resultImageProvider->getName());
            resultImageProvider = nullptr;
        }
    }
 
    void ArMemToImageProvider::onExitComponent()
    {
    }
 
 
 
    struct LocalResultImageProvider : public ResultImageProvider
    {
        // Allow calling setName() from outside.
        friend ArMemToImageProvider;
        virtual ~LocalResultImageProvider() override;
    };
    LocalResultImageProvider::~LocalResultImageProvider() {}
 
    void ArMemToImageProvider::enableResultImages(size_t numberImages, ImageDimension imageDimension, ImageType imageType, const std::string& name)
    {
        if (!resultImageProvider)
        {
            ARMARX_VERBOSE << "Enabling ImageProvider with " << numberImages << " result images of size "
                           << imageDimension.width << "x" << imageDimension.height << ".";
 
            IceInternal::Handle<LocalResultImageProvider> provider = Component::create<LocalResultImageProvider>();
            provider->setName(name);
            provider->setNumberResultImages(static_cast<int>(numberImages));
            provider->setResultImageFormat(imageDimension, imageType);
 
            try
            {
                getArmarXManager()->addObject(provider);
            }
            catch (const Ice::AlreadyRegisteredException& e)
            {
                ARMARX_ERROR << "The name '" << name << "' is already used. Please choose another one.\n"
                             << "Reason: " << e.what();
                getArmarXManager()->removeObjectBlocking(name);
            }
            provider->getObjectScheduler()->waitForObjectState(armarx::ManagedIceObjectState::eManagedIceObjectStarted);
 
            // Only now store it in member variable.
            this->resultImageProvider = provider;
        }
    }
 
 
    void ArMemToImageProvider::fetchUpdates(const armarx::armem::MemoryID& entityID, const std::vector<armarx::armem::MemoryID>& updatedSnapshotIDs)
    {
        std::scoped_lock lock(armemToImageMutex);
        armemToImage.fetchUpdates(entityID, updatedSnapshotIDs);
 
        if (resultImageProvider && armemToImage.updated)
        {
            std::vector<CByteImage> ivtImages;
            std::vector<CByteImage*> ivtImagesBuffer;
            ivtImages.reserve(armemToImage.images.size());
            ivtImagesBuffer.reserve(armemToImage.images.size());
            for (auto& image : armemToImage.images)
            {
                CByteImage& ivt = ivtImages.emplace_back(image->toCByteImage());
                ivtImagesBuffer.emplace_back(&ivt);
            }
            TIMING_START(Provide);
            resultImageProvider->provideResultImages(ivtImagesBuffer.data(), armemToImage.timestamp.toMicroSecondsSinceEpoch());
            TIMING_END_STREAM(Provide, ARMARX_VERBOSE);
            armemToImage.updated = false;
 
            if (debugObserver)
            {
                debugObserver->setDebugChannel(getName(),
                {
                    { "Provide [us]", new armarx::Variant(Provide.toMicroSecondsDouble()) },
                });
            }
        }
    }
 
}