MemoryRGBDOpenPoseEstimation.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 "MemoryRGBDOpenPoseEstimation.h"
 
 
using namespace visionx;
using namespace armarx;
 
 
armarx::PropertyDefinitionsPtr
RGBDOpenPoseEstimation::createPropertyDefinitions()
{
    armarx::PropertyDefinitionsPtr def{new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
 
 
    setupPropertyDefinitions(def);
    def->optional(providerName, "ImageProviderName");
    return def;
}
 
void RGBDOpenPoseEstimation::onInitImageProcessor()
{
    usingImageProvider(providerName);
 
    timeoutCounter2d = 0;
    readErrorCounter2d = 0;
    sucessCounter2d = 0;
}
 
void RGBDOpenPoseEstimation::onConnectImageProcessor()
{
    visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
    imageProviderInfo = getImageProvider(providerName, imageDisplayType);
    openposeInputImageFormat = imageProviderInfo.imageFormat;
 
    setupLocalVariables();
 
    numImages = static_cast<unsigned int>(imageProviderInfo.numberImages);
    if (numImages < 1 || numImages > 2)
    {
        ARMARX_FATAL << "invalid number of images. aborting";
        return;
    }
 
    imageBuffer = new CByteImage*[numImages];
    for (unsigned int i = 0 ; i < numImages ; i++)
    {
        imageBuffer[i] = visionx::tools::createByteImage(imageProviderInfo);
    }
 
    enableResultImages(1, imageProviderInfo.imageFormat.dimension, imageProviderInfo.imageFormat.type);
 
    ARMARX_INFO << "OpenPoseEstimationImageProcessor connect done";
}
 
void RGBDOpenPoseEstimation::onDisconnectImageProcessor()
{
    delete[] imageBuffer;
 
    destroyLocalVariables();
}
 
void RGBDOpenPoseEstimation::onExitImageProcessor()
{
 
}
 
void RGBDOpenPoseEstimation::process()
{
    if (running2D)
    {
        if (!waitForImages(providerName))
        {
            ++timeoutCounter2d;
            ARMARX_WARNING << "Timeout or error in wait for images"
                           << " (#timeout " << timeoutCounter2d
                           << ", #read error " << readErrorCounter2d
                           << ", #success " << sucessCounter2d << ")";
        }
        else
        {
            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
            {
                ++sucessCounter2d;
 
                {
                    std::unique_lock lock_rgb(rgbImageBufferMutex);
                    ::ImageProcessor::CopyImage(imageBuffer[0], rgbImageBuffer);
                }
 
                {
                    std::unique_lock lock_depth(depthImageBufferMutex);
                    ::ImageProcessor::CopyImage(imageBuffer[1], depthImageBuffer);
                }
 
                timestamp_of_update = imageMetaInfo->timeProvided;
                update_ready = true;
            }
        }
    }
    else
    {
        usleep(10000);
    }
}