HumanPoseWriter.cpp

Go to the documentation of this file.

#include "HumanPoseWriter.h"
 
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
 
#include <VisionX/libraries/armem_human/aron_conversions.h>
 
namespace armarx::armem::human::client
{
 
 
    Writer::Writer() : armem::client::util::SimpleWriterBase()
    {
    }
 
    Writer::~Writer() = default;
 
    void
    Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        armem::client::util::SimpleWriterBase::registerPropertyDefinitions(def);
    }
 
    void
    Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        ARMARX_VERBOSE << "Connecting ...";
        armem::client::util::SimpleWriterBase::connect(memoryNameSystem);
        ARMARX_VERBOSE << "Connected.";
 
        humanMemory =
            armem::human::HumanMemoryServerInterfacePrx::checkedCast(this->memoryWriter().memory);
    }
 
    bool
    Writer::commitHumanPose(const HumanPose& humanPose,
                            const std::string& provider,
                            const armem::Time& referencedTime)
    {
        ARMARX_WARNING << "NYI";
        return true;
    }
 
    bool
    Writer::commitHumanPoses(const std::vector<HumanPose>& humanPoses,
                             const std::string& provider,
                             const armem::Time& referencedTime)
    {
        ARMARX_WARNING << "NYI";
        return true;
    }
 
    bool
    Writer::commitHumanPoseInCameraFrame(const HumanPose& humanPose,
                                         const std::string& provider,
                                         const armem::Time& referencedTime)
    {
        return commitHumanPosesInCameraFrame({humanPose}, provider, referencedTime);
    }
 
    bool
    Writer::commitHumanPosesInCameraFrame(const std::vector<HumanPose>& humanPoses,
                                          const std::string& provider,
                                          const armem::Time& referencedTime)
    {
        using armarx::armem::human::toAron;
        using armarx::aron::toAron;
 
        std::vector<armarx::aron::data::dto::DictPtr> humanPosesDto;
        for (const HumanPose& humanPose : humanPoses)
        {
            armarx::human::arondto::HumanPose humanPoseAron;
            toAron(humanPoseAron, humanPose);
            humanPosesDto.push_back(humanPoseAron.toAronDTO());
        }
 
        core::time::dto::DateTime referencedTimeDto;
        toIce(referencedTimeDto, referencedTime);
 
        try
        {
            humanMemory->commitHumanPosesInCameraFrame(humanPosesDto, provider, referencedTimeDto);
            return true;
        }
        catch (...)
        {
            ARMARX_INFO << "Failed to commit human poses: " << GetHandledExceptionString();
            return false;
        }
    }
 
} // namespace armarx::armem::human::client