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 <VirtualRobot/Nodes/RobotNode.h>
 
#include <ArmarXCore/core/time/Clock.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 <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
#include <VisionX/libraries/armem_human/types.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.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.");
 
        humanPoseWriter.registerPropertyDefinitions(def);
 
        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));
 
            std::vector<armem::human::HumanPose> humanPoses;
            humanPoses.reserve(openposeResult3D.size());
            for (const auto& humanPose3D : openposeResult3D)
            {
                armarx::armem::human::HumanPose humanPose;
                // Per-frame OpenPose person key. Not a true cross-frame track, but at least
                // keeps simultaneously-visible people from collapsing into one identity.
                humanPose.humanTrackingId = humanPose3D.first;
                humanPose.cameraFrameName = cameraNodeName;
                humanPose.poseModelId = op_settings.model_pose;
 
                float meanZ = 0.;
                for (const auto& keypoint3D : humanPose3D.second.keypointMap)
                {
                    humanPose.keypoints[keypoint3D.first] = armarx::armem::human::PoseKeypoint{
                        .label = keypoint3D.first,
                        .confidence =
                            static_cast<float>(static_cast<int>(keypoint3D.second.confidence)),
                        .positionCamera = armarx::FramedPosition(
                            Eigen::Vector3f{
                                keypoint3D.second.x, keypoint3D.second.y, keypoint3D.second.z},
                            cameraNodeName,
                            localRobot->getName())};
                    meanZ += keypoint3D.second.z;
                }
                meanZ /= humanPose3D.second.keypointMap.size();
 
                if (meanZ > 100)
                {
                    humanPoses.emplace_back(humanPose);
                }
                else
                {
                    ARMARX_INFO << deactivateSpam(10) << "Ignoring human with mean depth < 100mm";
                }
            }
 
            if (not humanPoseWriter.commitHumanPosesInCameraFrame(humanPoses, getName(), ts))
            {
                ARMARX_ERROR << "Failed to commit poses to memory";
            }
        }
 
        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;
            // }
            humanPoseWriter.connect(memoryNameSystem());
        }
        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);
        }
    }
} // namespace armarx