PersonInstanceUpdater.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::ArmarXObjects::person_instance_updater
 * @author     Philipp Seidel ( uyhvq at student dot kit dot edu )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "PersonInstanceUpdater.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
 
#include <VisionX/components/person_instance_updater/UpdateConsumer.h>
#include <VisionX/libraries/armem_human/aron/FaceRecognition.aron.generated.h>
#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
#include <VisionX/libraries/armem_human/aron/Profile.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
#include <VisionX/libraries/armem_human/memory_ids.h>
#include <VisionX/libraries/armem_human/server/PoseSegment.h>
#include <VisionX/libraries/armem_human/server/face_recognition/Segment.h>
#include <VisionX/libraries/armem_human/server/profile/Segment.h>
#include <VisionX/libraries/armem_human/types.h>
 
namespace VisionX::components::person_instance_updater
{
 
    const std::string PersonInstanceUpdater::provider_name = "person_instance_updater";
 
    armarx::PropertyDefinitionsPtr
    PersonInstanceUpdater::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
        def->required(properties.max_face_head_distance, "maxFaceHeadDistance");
        return def;
    }
 
    void
    PersonInstanceUpdater::onInitComponent()
    {
        this->updateConsumer = UpdateConsumer(properties.max_face_head_distance);
    }
 
    void
    PersonInstanceUpdater::onConnectComponent()
    {
        this->task = new armarx::SimpleRunningTask<>([this]() { this->run(); });
        this->task->start();
    }
 
    void
    PersonInstanceUpdater::onDisconnectComponent()
    {
    }
 
    void
    PersonInstanceUpdater::onExitComponent()
    {
    }
 
    void
    PersonInstanceUpdater::run()
    {
        namespace armem = armarx::armem;
        // Subscribe to memory updates.
        {
            armem::client::Reader faceRecognitionReader =
                memoryNameSystem().getReader(armarx::human::FaceRecognitionCoreSegmentID);
            auto faceRecognitionCallback =
                [this,
                 faceRecognitionReader](const std::vector<armem::MemoryID>& updatedSnapshotIDs)
            {
                //ARMARX_INFO << "Updated faces with IDs: " << updatedSnapshotIDs;
                armem::client::QueryResult result =
                    faceRecognitionReader.queryMemoryIDs(updatedSnapshotIDs);
                if (result.success)
                {
                    result.memory.forEachInstance(
                        [this](const armem::wm::EntityInstance& instance)
                        {
                            armarx::human::arondto::FaceRecognition dto =
                                instance.dataAs<armarx::human::arondto::FaceRecognition>();
                            armarx::armem::human::FaceRecognition faceRecognition;
                            fromAron(dto, faceRecognition);
                            updateConsumer.consumeFaceRecognitionUpdate(
                                faceRecognition, instance.id(), this->memoryNameSystem());
                        });
                }
            };
            armem::client::Reader poseReader =
                memoryNameSystem().getReader(armarx::human::PoseCoreSegmentID);
            auto poseCallback =
                [this, poseReader](const std::vector<armem::MemoryID>& updatedSnapshotIDs)
            {
                //ARMARX_INFO << "Updated poses with IDs: " << updatedSnapshotIDs;
                armem::client::QueryResult result = poseReader.queryMemoryIDs(updatedSnapshotIDs);
                if (result.success)
                {
                    result.memory.forEachInstance(
                        [this](const armem::wm::EntityInstance& instance)
                        {
                            armarx::human::arondto::HumanPose dto =
                                instance.dataAs<armarx::human::arondto::HumanPose>();
                            armarx::armem::human::HumanPose humanPose;
                            fromAron(dto, humanPose);
                            updateConsumer.consumePoseUpdate(
                                humanPose, instance.id(), this->memoryNameSystem());
                        });
                }
            };
            armem::client::Reader profileReader =
                memoryNameSystem().getReader(armarx::human::ProfileCoreSegmentID);
            auto profileCallback =
                [this, profileReader](const std::vector<armem::MemoryID>& updatedSnapshotIDs)
            {
                //ARMARX_INFO << "Updated profiles with IDs: " << updatedSnapshotIDs;
                armem::client::QueryResult result =
                    profileReader.queryMemoryIDs(updatedSnapshotIDs);
                if (result.success)
                {
                    result.memory.forEachInstance(
                        [this](const armem::wm::EntityInstance& instance)
                        {
                            armarx::human::arondto::Profile profile =
                                instance.dataAs<armarx::human::arondto::Profile>();
                            updateConsumer.consumeProfileUpdate(
                                profile, instance.id(), this->memoryNameSystem());
                        });
                }
            };
            memoryNameSystem().subscribe(armarx::human::FaceRecognitionCoreSegmentID,
                                         faceRecognitionCallback);
            memoryNameSystem().subscribe(armarx::human::PoseCoreSegmentID, poseCallback);
            memoryNameSystem().subscribe(armarx::human::ProfileCoreSegmentID, profileCallback);
        }
    }
 
    std::string
    PersonInstanceUpdater::getDefaultName() const
    {
        return provider_name;
    }
 
    std::string
    PersonInstanceUpdater::GetDefaultName()
    {
        return PersonInstanceUpdater::provider_name;
    }
 
    //    ARMARX_REGISTER_COMPONENT_EXECUTABLE(PersonInstanceUpdater,
    //                                         PersonInstanceUpdater::GetDefaultName());
 
} // namespace VisionX::components::person_instance_updater