RGBDPoseEstimationWithMemoryWriter.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 "RGBDPoseEstimationWithMemoryWriter.h"
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/time/Clock.h>
 
 
using namespace visionx;
 
namespace armarx
{
 
    const std::string
    RGBDPoseEstimationWithMemoryWriter::defaultName = "RGBDPoseEstimationWithMemoryWriter";
 
 
    armarx::PropertyDefinitionsPtr
    RGBDPoseEstimationWithMemoryWriter::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def{new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
 
        def->required(properties.providerName, "ImageProviderName");
 
        def->optional(properties.enableMemory, "humanMemory.enable", "Enables or disables writing to the human memory.");
        def->optional(properties.humanMemory.memoryName, "humanMemory.memory_name", "Name of the human memory.");
        def->optional(properties.humanMemory.coreSegmentName, "humanMemory.core_segment_name", "Name of the core segment in the human memory.");
 
        RGBDOpenPoseEstimationComponentPluginUser::postCreatePropertyDefinitions(def);
        return def;
    }
 
 
    std::string
    RGBDPoseEstimationWithMemoryWriter::getDefaultName() const
    {
        return RGBDPoseEstimationWithMemoryWriter::defaultName;
    }
 
 
    std::string
    RGBDPoseEstimationWithMemoryWriter::GetDefaultName()
    {
        return RGBDPoseEstimationWithMemoryWriter::defaultName;
    }
 
 
    void
    RGBDPoseEstimationWithMemoryWriter::reportEntities()
    {
        if (properties.enableMemory)
        {
            armem::Time ts = armem::Time(armem::Duration::MicroSeconds(timestamp_of_update));
 
            armem::EntityUpdate update;
            update.entityID = armem::MemoryID()
                                  .withMemoryName(properties.humanMemory.memoryName)
                                  .withCoreSegmentName(properties.humanMemory.coreSegmentName)
                                  .withProviderSegmentName(getName())
                                  .withEntityName("3DDetections")
                                  .withTimestamp(ts);
            update.referencedTime = ts;
 
            for (auto& [_, humanPose] : openposeResult3D)
            {
                auto pose = humanPose.keypointMap;
 
                armarx::human::arondto::HumanPose dto;
                dto.poseModelId = op_settings.model_pose;
                // OpenPose does not allow for tracking.
                dto.humanTrackingId = std::nullopt;
 
                for (auto& [name, keypoint] : pose)
                {
                    armarx::human::arondto::PoseKeypoint kp;
                    kp.confidence = keypoint.confidence;
 
                    armarx::arondto::FramedPosition posGlobal;
                    posGlobal.header.agent = localRobot->getName();
                    posGlobal.header.frame = armarx::GlobalFrame;
                    posGlobal.position.x() = keypoint.globalX;
                    posGlobal.position.y() = keypoint.globalY;
                    posGlobal.position.z() = keypoint.globalZ;
                    FramedPosition positionGlobal(posGlobal.position, armarx::GlobalFrame, posGlobal.header.agent);
                    toAron(kp.positionGlobal.emplace(), positionGlobal);
 
                    FramedPosition pos(Eigen::Vector3f(keypoint.x, keypoint.y, keypoint.z), cameraNodeName, localRobot->getName());
                    armarx::FramedPosition posRobot(pos.toRootFrame(localRobot)->toEigen(), localRobot->getRootNode()->getName(), localRobot->getName());
                    toAron(kp.positionRobot.emplace(), posRobot);
 
                    dto.keypoints[keypoint.label] = kp;
                }
 
                update.instancesData.push_back(dto.toAron());
            }
 
            const armem::EntityUpdateResult updateResult = humanMemoryWriter.commit(update);
 
            if (not updateResult.success)
            {
                ARMARX_ERROR << updateResult.errorMessage;
            }
        }
 
        RGBDOpenPoseEstimationComponentPluginUser::reportEntities();
    }
 
    void RGBDPoseEstimationWithMemoryWriter::onInitImageProcessor()
    {
        RGBDOpenPoseEstimationComponentPluginUser::preOnInitImageProcessor();
        usingImageProvider(properties.providerName);
 
        timeoutCounter2d = 0;
        readErrorCounter2d = 0;
        sucessCounter2d = 0;
    }
 
    void RGBDPoseEstimationWithMemoryWriter::onConnectImageProcessor()
    {
        RGBDOpenPoseEstimationComponentPluginUser::preOnConnectImageProcessor();
 
        if (properties.enableMemory)
        {
            ARMARX_IMPORTANT << "Waiting for memory ...";
            try
            {
                humanMemoryWriter = memoryNameSystem().useWriter(properties.humanMemory);
            }
            catch (const armarx::armem::error::CouldNotResolveMemoryServer& e)
            {
                ARMARX_ERROR << e.what();
                return;
            }
        }
        else
        {
            ARMARX_IMPORTANT << "Writing to memroy disabled via properties.";
        }
 
        visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
        imageProviderInfo = getImageProvider(properties.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 RGBDPoseEstimationWithMemoryWriter::onDisconnectImageProcessor()
    {
        RGBDOpenPoseEstimationComponentPluginUser::preOnDisconnectImageProcessor();
 
        delete[] imageBuffer;
 
        RGBDOpenPoseEstimationComponentPluginUser::postOnDisconnectImageProcessor();
    }
 
    void RGBDPoseEstimationWithMemoryWriter::onExitImageProcessor()
    {
    }
 
    void RGBDPoseEstimationWithMemoryWriter::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(properties.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(properties.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);
        }
    }
}