LegacyRGBDOpenPoseEstimation.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::RGBDOpenPoseEstimation
 * @author     Fabian Peller <fabian.peller@kit.edu>
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LegacyRGBDOpenPoseEstimation.h"
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
 
using namespace visionx;
 
namespace armarx
{
    armarx::PropertyDefinitionsPtr
    LegacyRGBDOpenPoseEstimation::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def{new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
 
        def->required(providerName, "ImageProviderName");
 
        RGBDOpenPoseEstimationComponentPluginUser::postCreatePropertyDefinitions(def);
        return def;
    }
 
    void LegacyRGBDOpenPoseEstimation::onInitImageProcessor()
    {
        RGBDOpenPoseEstimationComponentPluginUser::preOnInitImageProcessor();
        usingImageProvider(providerName);
 
        timeoutCounter2d = 0;
        readErrorCounter2d = 0;
        sucessCounter2d = 0;
    }
 
    void LegacyRGBDOpenPoseEstimation::onConnectImageProcessor()
    {
        RGBDOpenPoseEstimationComponentPluginUser::preOnConnectImageProcessor();
 
        visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
        imageProviderInfo = getImageProvider(providerName, imageDisplayType);
        rgbImageFormat = imageProviderInfo.imageFormat;
 
        numImages = static_cast<unsigned int>(imageProviderInfo.numberImages);
        if (numImages != 2)
        {
            ARMARX_FATAL << "invalid number of images. aborting";
            return;
        }
 
        imageBuffer = new CByteImage*[2];
        openposeResultImage = new CByteImage*[1];
        imageBuffer[0] = visionx::tools::createByteImage(imageProviderInfo);
        imageBuffer[1] = visionx::tools::createByteImage(imageProviderInfo);
        rgbImageBuffer = visionx::tools::createByteImage(imageProviderInfo);
        depthImageBuffer = visionx::tools::createByteImage(imageProviderInfo);
 
        enableResultImages(1, imageProviderInfo.imageFormat.dimension, imageProviderInfo.imageFormat.type);
 
        ARMARX_INFO << getName() << " connect done";
 
        RGBDOpenPoseEstimationComponentPluginUser::postOnConnectImageProcessor();
    }
 
    void LegacyRGBDOpenPoseEstimation::onDisconnectImageProcessor()
    {
        RGBDOpenPoseEstimationComponentPluginUser::preOnDisconnectImageProcessor();
 
        delete[] imageBuffer;
 
        RGBDOpenPoseEstimationComponentPluginUser::postOnDisconnectImageProcessor();
    }
 
    void LegacyRGBDOpenPoseEstimation::onExitImageProcessor()
    {
    }
 
    void LegacyRGBDOpenPoseEstimation::process()
    {
        if (running2D)
        {
            // check for result images
            if (result_image_ready)
            {
                std::lock_guard outputImage_lock(openposeResultImageMutex);
                ARMARX_DEBUG << deactivateSpam() << "publish result openpose image";
                provideResultImages(openposeResultImage, imageMetaInfo);
            }
 
            // check for new images
            if (!waitForImages(providerName))
            {
                ++timeoutCounter2d;
                ARMARX_WARNING << "Timeout or error in wait for images"
                               << " (#timeout " << timeoutCounter2d
                               << ", #read error " << readErrorCounter2d
                               << ", #success " << sucessCounter2d << ")";
            }
            else
            {
                std::lock_guard lock_images(imageBufferMutex);
                if (static_cast<unsigned int>(getImages(providerName, imageBuffer, imageMetaInfo)) != numImages)
                {
                    ++readErrorCounter2d;
                    ARMARX_WARNING << "Unable to transfer or read images"
                                   << " (#timeout " << timeoutCounter2d
                                   << ", #read error " << readErrorCounter2d
                                   << ", #success " << sucessCounter2d << ")";
                    return;
                }
                else
                {
                    ARMARX_DEBUG << "Received an Image.";
                    ++sucessCounter2d;
 
                    std::lock_guard lock_rgb(rgbImageBufferMutex);
                    std::lock_guard lock_depth(depthImageBufferMutex);
                    ::ImageProcessor::CopyImage(imageBuffer[0], rgbImageBuffer);
                    ::ImageProcessor::CopyImage(imageBuffer[1], depthImageBuffer);
 
                    timestamp_of_update = imageMetaInfo->timeProvided;
                    update_ready = true;
                }
            }
        }
        else
        {
            ARMARX_DEBUG << deactivateSpam() << "Not running. Wait until start signal comes";
            usleep(10000);
        }
    }
}