ImageToArMem.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::SimpleEpisodicMemoryImageConnector
 * @author     Fabian Peller ( fabian dot peller-konrad at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ImageToArMem.h"
 
// IVT
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/core/time_minimal.h>
 
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/core/error.h>
 
 
#include <VisionX/tools/ImageUtil.h>
 
#include <Image/ImageProcessor.h>
 
namespace visionx
{
 
    std::string
    ImageToArMem::getDefaultName() const
    {
        return "ImageToArMem";
    }
 
    armarx::PropertyDefinitionsPtr
    ImageToArMem::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new armarx::ComponentPropertyDefinitions{getConfigIdentifier()});
 
        defs->topic(debugObserver);
 
        defs->optional(p.imageProviderName, "img.ProviderName", "Name of the image provier.");
        defs->optional(p.providerSegmentName,
                       "img.ProviderSegmentName",
                       "The provider (segment) name (if not specified in the entity ID).\n"
                       "If '(auto)', the image provider name is used.");
 
        defs->optional(p.clearProviderSegmentWhenExists,
                       "mem.ClearProviderSegmentsWhenExisting",
                       "If true, the provider segments are cleared when this component starts.");
 
        defs->optional(p.commitCameraCalib,
                       "mem.commitCameraCalibration",
                       "Commit camera calibration to memory");
 
        defs->optional(p.maxFrequencyOfCommit,
                        "mem.maxCommitFrequency",
                        "Maximal frequency with which to commit images to Vision Memory in Hz. \n"
                        "Actual commit frequency might be lower, as we wait for next image.");
 
        p.images.define(*defs);
 
        return defs;
    }
 
    void
    visionx::ImageToArMem::onInitImageProcessor()
    {
        // Parse properties.
        if (p.providerSegmentName == "(auto)")
        {
            p.providerSegmentName = p.imageProviderName;
        }
        p.images.read(*this, p.providerSegmentName);
 
        imageToArMem.addImagesRGB(p.images.rgbEntityID, p.images.rgbIndices);
        imageToArMem.addImagesDepth(p.images.depthEntityID, p.images.depthIndices);
 
        ARMARX_INFO << "Memory image structure: \n" << imageToArMem.summarizeStructure();
        ARMARX_VERBOSE << "Using image provider '" << p.imageProviderName << "'.";
 
        usingImageProvider(p.imageProviderName);
    }
 
    void
    visionx::ImageToArMem::onConnectImageProcessor()
    {
        // Connect to image provider.
        {
            imageProviderInfo = getImageProvider(p.imageProviderName);
 
            // Init input images.
            imageToArMem.initImages(imageProviderInfo.imageFormat);
            inputImages = imageToArMem.makeCByteImageBuffer();
            inputImageBuffer.clear();
            for (CByteImage& img : inputImages)
            {
                inputImageBuffer.emplace_back(&img);
            }
        }
 
        // Connect to memory server.
        {
            try
            {
                imageToArMem.setWriter(memoryNameSystem().useWriter(imageToArMem.getMemoryID()));
            }
            catch (const armarx::armem::error::CouldNotResolveMemoryServer& e)
            {
                ARMARX_ERROR << e.what();
            }
 
            imageToArMem.setDebugObserver(debugObserver);
 
            imageToArMem.addProviderSegments();
        }
    }
 
    void
    visionx::ImageToArMem::onDisconnectImageProcessor()
    {
    }
 
    void
    visionx::ImageToArMem::onExitImageProcessor()
    {
    }
 
    void
    visionx::ImageToArMem::process()
    {
        const armarx::Duration timeout = armarx::Duration::MilliSeconds(1000);
        armarx::Duration timeGetImages;
 
        int numImages = 0;
        int millisecondsSinceLastCommit = 0;
        if (waitForImages(p.imageProviderName, static_cast<int>(timeout.toMilliSeconds())))
        {
            armarx::DateTime n = armarx::DateTime::Now();
            int timeDifference = (n.toMicroSecondsSinceEpoch() - this->timeOfLastAcceptedImage.toMicroSecondsSinceEpoch())/1000;
            ARMARX_DEBUG << "Time difference during commit is " << timeDifference;
            float maxFrequency = p.maxFrequencyOfCommit;
            float minWaitingTime = (1/maxFrequency) * 1000;
            ARMARX_DEBUG << "minWaitingTime is " << minWaitingTime;
            if (timeDifference > minWaitingTime){
                ARMARX_DEBUG << "resetting time of last accepted image";
                this->timeOfLastAcceptedImage = armarx::DateTime::Now();  //if we accept the image we reset the time
                TIMING_START(GetImages);
                numImages = getImages(p.imageProviderName, inputImageBuffer.data(), imageMetaInfo);
                TIMING_END_STREAM(GetImages, ARMARX_VERBOSE);
                timeGetImages = armarx::Duration::MicroSeconds(GetImages.toMicroSeconds());
 
                if (numImages == static_cast<int>(inputImageBuffer.size()))
                {
                    using namespace armarx::armem;
                    imageToArMem.useImageBuffers(
                        inputImageBuffer.data(),
                        Time(Duration::MicroSeconds(imageMetaInfo->timeProvided)));
                    imageToArMem.commitImages(); //TODO: limit how much images will be send to memory
                }
                else
                {
                    ARMARX_ERROR << "Received unexpected number of input images. Got " << numImages
                                << " instead of " << inputImageBuffer.size() << ".";
                }
 
 
                // Initially commit the camera calibration once. Has to be done in the processing as it doesn't wait for the proxy
                if (!commitedCameraCalib and p.commitCameraCalib and imageProviderInfo.proxy)
                {
                    if (visionx::StereoCalibrationInterfacePrx calibrationProvider = 
                        visionx::StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
                        calibrationProvider)
                    {
                        ARMARX_INFO
                            << "Found stereo camera calibration from StereoCalibrationInterface.";
                        visionx::StereoCalibration stereoCameraCalibration =
                            calibrationProvider->getStereoCalibration();
                        imageToArMem.commitCameraCalibration(stereoCameraCalibration);
                    }
                    else
                    {
                        ARMARX_INFO << "Could not cast to stereo calibration interface. Creating default monocular ";
                        visionx::MonocularCalibration monocularCameraCalibration =
                            visionx::tools::createDefaultMonocularCalibration();
 
                        imageToArMem.commitCameraCalibration(monocularCameraCalibration);
                    }
                    commitedCameraCalib = true;
                }
            }
            else{
                ARMARX_DEBUG << "Ignoring image due to high frequency of commits but lower max. Frequency set in properties";
            }
        }
        else
        {
            ARMARX_WARNING << "Timeout while waiting for camera images (> " << timeout << ").";
            return;
        }
 
        if (debugObserver)
        {
            debugObserver->setDebugChannel(
                getName(),
                {
                    {"getImages() [us]", new armarx::Variant(timeGetImages.toMicroSecondsDouble())},
                });
        }
    }
 
} // namespace visionx