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 <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.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
{
    namespace armem = armarx::armem;
 
    const std::string PersonInstanceUpdater::provider_name = "PersonInstanceUpdater";
 
    armarx::PropertyDefinitionsPtr
    PersonInstanceUpdater::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->optional(properties.updateConsumer.maxFaceHeadDistance, "maxFaceHeadDistance");
        def->optional(properties.updateConsumer.enableBodyTracking,
                      "enableBodyTracking",
                      "If false, body tracking (pose) data is ignored entirely. "
                      "PersonInstances are updated based on face detections only and their "
                      "poseID (and pose-derived global pose orientation) is left unset.");
 
        def->optional(properties.faceRecognitionUpdateFrequency,
                      "faceRecognitionUpdateFrequency",
                      "Max rate [Hz] at which face recognition updates are processed. "
                      "Values <= 0 disable throttling (process every update).");
        def->optional(properties.poseUpdateFrequency,
                      "poseUpdateFrequency",
                      "Max rate [Hz] at which pose updates are processed. "
                      "Values <= 0 disable throttling (process every update).");
 
        return def;
    }
 
    void
    PersonInstanceUpdater::onInitComponent()
    {
    }
 
    void
    PersonInstanceUpdater::onConnectComponent()
    {
        this->updateConsumer =
            std::make_unique<UpdateConsumer>(memoryNameSystem(), properties.updateConsumer);
 
        // Set up per-callback throttling. A non-positive frequency disables throttling.
        if (properties.faceRecognitionUpdateFrequency > 0.F)
        {
            faceRecognitionThrottler.emplace(properties.faceRecognitionUpdateFrequency);
        }
        if (properties.poseUpdateFrequency > 0.F)
        {
            poseThrottler.emplace(properties.poseUpdateFrequency);
        }
 
        faceRecognitionReader.emplace(
            memoryNameSystem().useReader(armarx::human::FaceRecognitionCoreSegmentID));
        poseReader.emplace(memoryNameSystem().useReader(armarx::human::PoseCoreSegmentID));
 
        // Start the single processing thread before subscribing, so no update is lost.
        stopProcessing = false;
        processingThread = std::thread([this] { processingLoop(); });
 
        registerMemorySubscriptions();
    }
 
    void
    PersonInstanceUpdater::onDisconnectComponent()
    {
        // unsubscribe from all memory subscriptions
        for (auto& handle : subscriptionHandles)
        {
            memoryNameSystem().unsubscribe(handle);
        }
        subscriptionHandles.clear();
 
        // Stop the processing thread.
        {
            std::lock_guard g{pendingMutex};
            stopProcessing = true;
        }
        pendingCondition.notify_all();
        if (processingThread.joinable())
        {
            processingThread.join();
        }
        pendingFaceRecognitionIDs.clear();
        pendingPoseIDs.clear();
 
        faceRecognitionReader.reset();
        poseReader.reset();
 
        faceRecognitionThrottler.reset();
        poseThrottler.reset();
 
        this->updateConsumer.reset();
    }
 
    void
    PersonInstanceUpdater::processingLoop()
    {
        std::unique_lock lock{pendingMutex};
        while (true)
        {
            pendingCondition.wait(lock,
                                  [this] {
                                      return stopProcessing or
                                             not pendingFaceRecognitionIDs.empty() or
                                             not pendingPoseIDs.empty();
                                  });
            if (stopProcessing)
            {
                return;
            }
 
            // Take the pending updates out of the mailboxes; new updates arriving while
            // we process replace/fill them again (latest wins, no accumulation).
            const std::vector<armem::MemoryID> faceIDs = std::move(pendingFaceRecognitionIDs);
            const std::vector<armem::MemoryID> poseIDs = std::move(pendingPoseIDs);
            pendingFaceRecognitionIDs.clear();
            pendingPoseIDs.clear();
 
            lock.unlock();
            // Exceptions must not escape the thread (std::terminate would abort the
            // component); previously the Ice runtime caught them in the callbacks.
            try
            {
                if (not faceIDs.empty())
                {
                    processFaceRecognitionUpdates(faceIDs);
                }
                if (not poseIDs.empty())
                {
                    processPoseUpdates(poseIDs);
                }
            }
            catch (const std::exception& e)
            {
                ARMARX_ERROR << deactivateSpam(10)
                             << "Exception while processing memory updates: " << e.what();
            }
            catch (...)
            {
                ARMARX_ERROR << deactivateSpam(10)
                             << "Unknown exception while processing memory updates.";
            }
            lock.lock();
        }
    }
 
    void
    PersonInstanceUpdater::processFaceRecognitionUpdates(
        const std::vector<armem::MemoryID>& 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());
                });
        }
    }
 
    void
    PersonInstanceUpdater::processPoseUpdates(
        const std::vector<armem::MemoryID>& updatedSnapshotIDs)
    {
        // Query the actual pose data for the updated IDs
        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());
                });
        }
    }
 
    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
        {
            // Callback triggered when new face recognition results arrive.
            // Only deposits the IDs for the processing thread — must not block.
            auto faceRecognitionCallback =
                [this](const std::vector<armem::MemoryID>& updatedSnapshotIDs)
            {
                // Throttle to the configured frequency.
                if (faceRecognitionThrottler.has_value() and
                    not faceRecognitionThrottler->check(armarx::Clock::Now()))
                {
                    return;
                }
 
                {
                    std::lock_guard g{pendingMutex};
                    // Latest wins: replace any not-yet-processed update.
                    pendingFaceRecognitionIDs = updatedSnapshotIDs;
                }
                pendingCondition.notify_one();
            };
 
            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
        //
        // Skipped entirely when body tracking is disabled: PersonInstances are then
        // maintained from face detections only.
        if (properties.updateConsumer.enableBodyTracking)
        {
            // Only deposits the IDs for the processing thread — must not block.
            // Pose updates come at high frequency; if the processing thread is still
            // busy, the pending (unprocessed) update is simply replaced (latest wins).
            auto poseCallback = [this](const std::vector<armem::MemoryID>& updatedSnapshotIDs)
            {
                // Throttle to the configured frequency.
                if (poseThrottler.has_value() and
                    not poseThrottler->check(armarx::Clock::Now()))
                {
                    return;
                }
 
                {
                    std::lock_guard g{pendingMutex};
                    pendingPoseIDs = updatedSnapshotIDs;
                }
                pendingCondition.notify_one();
            };
 
            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;
    }
 
 
} // namespace VisionX::components::person_instance_updater
 
 
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(
    ::VisionX::components::person_instance_updater::PersonInstanceUpdater,
    ::VisionX::components::person_instance_updater::PersonInstanceUpdater::GetDefaultName());