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 )
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "PersonInstanceUpdater.h"
 
#include <memory>
#include <string>
#include <vector>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/util/Throttler.h>
 
#include "RobotAPI/libraries/armem/client/Query.h"
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem/core/wm/memory_definitions.h"
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.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/Person.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
#include <VisionX/libraries/armem_human/memory_ids.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->optional(properties.updateConsumer.maxFaceHeadDistance, "maxFaceHeadDistance");
 
        return def;
    }
 
    void
    PersonInstanceUpdater::onInitComponent()
    {
    }
 
    void
    PersonInstanceUpdater::onConnectComponent()
    {
        this->updateConsumer =
            std::make_unique<UpdateConsumer>(memoryNameSystem(), properties.updateConsumer);
 
        registerMemorySubscriptions();
    }
 
    void
    PersonInstanceUpdater::onDisconnectComponent()
    {
        // unsubscribe from all memory subscriptions
        for (auto& handle : subscriptionHandles)
        {
            memoryNameSystem().unsubscribe(handle);
        }
 
        this->updateConsumer.reset();
    }
 
    void
    PersonInstanceUpdater::onExitComponent()
    {
    }
 
    void
    PersonInstanceUpdater::registerMemorySubscriptions()
    {
        ARMARX_CHECK_NOT_NULL(updateConsumer);
        namespace armem = armarx::armem;
 
        // ========== Subscribe to FaceRecognition updates ==========
        // When a face is recognized (with identity information), we need to either:
        // 1. Update an existing PersonInstance with the new face data, or
        // 2. Create a new PersonInstance if this is a newly recognized person
        {
            armem::client::Reader faceRecognitionReader =
                memoryNameSystem().useReader(armarx::human::FaceRecognitionCoreSegmentID);
 
            auto faceRecognitionCallback =
                [this,
                 faceRecognitionReader](const std::vector<armem::MemoryID>& updatedSnapshotIDs)
            {
                // Callback triggered when new face recognition results arrive
                //ARMARX_INFO << "Updated faces with IDs: " << updatedSnapshotIDs;
 
                // Query the actual face recognition data for the updated IDs
                armem::client::QueryResult result =
                    faceRecognitionReader.queryMemoryIDs(updatedSnapshotIDs);
 
                if (result.success)
                {
                    // Process each face recognition instance
                    result.memory.forEachInstanceWithDataAs(
                        [this](const armem::wm::EntityInstanceBase<
                               armarx::human::arondto::FaceRecognition>& instance)
                        {
                            // Convert from ARON DTO to internal representation
                            armarx::armem::human::FaceRecognition faceRecognition;
                            fromAron(instance.data(), faceRecognition);
 
                            // Delegate to UpdateConsumer to match with poses and update PersonInstances
                            updateConsumer->consumeFaceRecognitionUpdate(faceRecognition,
                                                                         instance.id());
                        });
                }
            };
 
            subscriptionHandles.push_back(
                memoryNameSystem().subscribe(armarx::human::FaceRecognitionCoreSegmentID,
                                             faceRecognitionCallback));
        }
 
        // ========== Subscribe to Pose updates ==========
        // When a human pose is tracked (body skeleton with tracking ID), we need to either:
        // 1. Update an existing PersonInstance that has a matching tracking ID, or
        // 2. Try to match it with a recognized face based on spatial proximity, or
        // 3. Create a new PersonInstance for this tracked pose
        {
            armem::client::Reader poseReader =
                memoryNameSystem().useReader(armarx::human::PoseCoreSegmentID);
 
            auto poseCallback =
                [this, poseReader](const std::vector<armem::MemoryID>& updatedSnapshotIDs)
            {
                // Skip if UpdateConsumer is busy to prevent data accumulation
                // (pose updates come at high frequency)
                if (updateConsumer->isBusy())
                {
                    ARMARX_DEBUG << "Update consumer is currently processing data. To prevent data "
                                    "accumulation, we skip this update.";
                    return;
                }
 
                // Query the actual pose data for the updated IDs
                //ARMARX_INFO << "Updated poses with IDs: " << updatedSnapshotIDs;
                armem::client::QueryResult result = poseReader.queryMemoryIDs(updatedSnapshotIDs);
                if (result.success)
                {
                    // Process each pose instance
                    result.memory.forEachInstanceWithDataAs(
                        [this](
                            const armem::wm::EntityInstanceBase<armarx::human::arondto::HumanPose>&
                                instance)
                        {
                            // Convert from ARON DTO to internal representation
                            armarx::armem::human::HumanPose humanPose;
                            fromAron(instance.data(), humanPose);
 
                            // Delegate to UpdateConsumer to match with faces and update PersonInstances
                            updateConsumer->consumePoseUpdate(humanPose, instance.id());
                        });
                }
            };
 
            subscriptionHandles.push_back(
                memoryNameSystem().subscribe(armarx::human::PoseCoreSegmentID, poseCallback));
        }
 
        // ========== Subscribe to Profile updates (currently disabled) ==========
        // Profile updates would contain person information like name, ID, etc.
        // Currently disabled with if(false) - would update PersonInstance profile data when enabled
        if (false)
        {
            armem::client::Reader profileReader =
                memoryNameSystem().useReader(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.forEachInstanceWithDataAs(
                        [this](const armem::wm::EntityInstanceBase<armarx::human::arondto::Person>&
                                   instance)
                        { updateConsumer->consumeProfileUpdate(instance.data(), instance.id()); });
                }
            };
 
            subscriptionHandles.push_back(
                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