PersonInstanceReader.cpp

Go to the documentation of this file.

#include "PersonInstanceReader.h"
 
#include <algorithm>
#include <iterator>
#include <string>
#include <utility>
#include <vector>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/to_container.hpp>
#include <range/v3/view/transform.hpp>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "RobotAPI/libraries/armem/client/Query.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
#include "RobotAPI/libraries/armem/client/query/selectors.h"
#include "RobotAPI/libraries/armem/client/util/SimpleReaderBase.h"
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem/core/wm/memory_definitions.h"
#include <RobotAPI/libraries/armem/util/util.h>
 
#include "VisionX/libraries/armem_human/types.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/PersonInstance.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
 
namespace armarx::armem::human::client
{
    PersonInstanceReader::~PersonInstanceReader() = default;
 
    armarx::armem::client::query::Builder
    PersonInstanceReader::buildQuery(const Query& query) const
    {
        armarx::armem::client::query::Builder qb;
 
        armarx::armem::client::query::CoreSegmentSelector& coreSegmentQuery =
            qb.coreSegments().withName(properties().coreSegmentName);
 
        armarx::armem::client::query::ProviderSegmentSelector& providerQuery =
            [&]() -> armem::client::query::ProviderSegmentSelector&
        {
            if (not query.providerName.empty())
            {
                return coreSegmentQuery.providerSegments().withName(query.providerName);
            }
            return coreSegmentQuery.providerSegments().all();
        }();
 
        providerQuery.entities().all().snapshots().beforeOrAtTime(query.timestamp);
 
        return qb;
    }
 
    std::string
    PersonInstanceReader::propertyPrefix() const
    {
        return "mem.human.person";
    }
 
    armarx::armem::client::util::SimpleReaderBase::Properties
    PersonInstanceReader::defaultProperties() const
    {
        return {.memoryName = "Human", .coreSegmentName = "PersonInstance"};
    }
 
    std::vector<PersonInstance>
    asPersonInstances(const wm::ProviderSegment& providerSegment,
                      const DateTime& timestamp,
                      const Duration& maxAge)
    {
        ARMARX_TRACE;
 
        // ARMARX_CHECK(not providerSegment.empty()) << "No entities";
        // ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
 
        std::vector<PersonInstance> personInstances;
        providerSegment.forEachEntity(
            [&](const wm::Entity& entity)
            {
                ARMARX_TRACE;
 
                const auto& entitySnapshot = entity.getLatestSnapshot();
                // Not clear yet why, but it seems that this sometimes happens (not frequently).
                ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
 
                entitySnapshot.forEachInstance(
                    [&personInstances, &timestamp, &maxAge](
                        const armarx::armem::wm::EntityInstance& entityInstance)
                    {
                        ARMARX_TRACE;
 
                        const core::time::Duration dtToNow =
                            timestamp - entityInstance.metadata().referencedTime;
 
                        if (dtToNow < maxAge and dtToNow.isPositive())
                        {
                            const auto aronDto =
                                tryCast<armarx::human::arondto::PersonInstance>(entityInstance);
                            ARMARX_CHECK(aronDto) << "Failed casting to PersonInstance";
 
                            PersonInstance personInstance;
                            fromAron(*aronDto, personInstance);
 
                            personInstances.push_back(personInstance);
                        }
                        else
                        {
                            ARMARX_DEBUG << "Skipping person instance "
                                         << QUOTED(entityInstance.id()) << " that is too old ("
                                         << dtToNow << " > " << maxAge << ")";
                        }
                    });
            });
 
        // when no humans are in the scene, this is empty
        //ARMARX_CHECK_NOT_EMPTY(humanPoses);
 
        ARMARX_DEBUG << "Found " << personInstances.size() << " person instances.";
 
        return personInstances;
    }
 
    PersonInstanceReader::Result
    PersonInstanceReader::query(const Query& query) const
    {
        ARMARX_TRACE;
        const auto qb = buildQuery(query);
 
        ARMARX_DEBUG << "[MappingDataReader] query ... ";
 
        const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
 
        if (not qResult.success)
        {
            ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
            return {.personInstances = {},
                    .status = Result::Status::Error,
                    .errorMessage = qResult.errorMessage};
        }
 
        ARMARX_TRACE;
        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
 
        if (query.providerName.empty())
        {
            ARMARX_TRACE;
 
            std::vector<PersonInstance> allPersonInstances;
 
            coreSegment.forEachProviderSegment(
                [&allPersonInstances, &query](const auto& providerSegment)
                {
                    const std::vector<PersonInstance> personInstances =
                        asPersonInstances(providerSegment, query.timestamp, query.maxAge);
                    if (personInstances.size() > 0)
                    {
                        ARMARX_INFO << deactivateSpam(10) << "Found " << personInstances.size()
                                    << " person instances in provider segment `"
                                    << providerSegment.name() << "` that are not too old.";
                    }
                    else
                    {
                        ARMARX_DEBUG << deactivateSpam(5) << "Found " << personInstances.size()
                                     << " person instances in provider segment `"
                                     << providerSegment.name() << "` that are not too old.";
                    }
 
                    std::copy(personInstances.begin(),
                              personInstances.end(),
                              std::back_inserter(allPersonInstances));
                });
 
            if (allPersonInstances.empty())
            {
                // ARMARX_WARNING << "No entities.";
                return {.personInstances = {},
                        .status = Result::Status::NoData,
                        .errorMessage = "No entities"};
            }
 
            return Result{.personInstances = allPersonInstances, .status = Result::Status::Success};
        }
 
        ARMARX_TRACE;
 
        // -> provider segment name is set
        if (not coreSegment.hasProviderSegment(query.providerName))
        {
            ARMARX_INFO << deactivateSpam(5) << "Provider segment `" << query.providerName
                        << "` does not exist (yet).";
            return {.personInstances = {}, .status = Result::Status::NoData};
        }
 
        ARMARX_TRACE;
 
        const wm::ProviderSegment& providerSegment =
            coreSegment.getProviderSegment(query.providerName);
 
        if (providerSegment.empty())
        {
            ARMARX_WARNING << "No entities.";
            return {.personInstances = {},
                    .status = Result::Status::NoData,
                    .errorMessage = "No entities"};
        }
 
        try
        {
            ARMARX_TRACE;
 
            const auto personInstances =
                asPersonInstances(providerSegment, query.timestamp, query.maxAge);
            return Result{.personInstances = personInstances, .status = Result::Status::Success};
        }
        catch (...)
        {
            return Result{.status = Result::Status::Error,
                          .errorMessage = GetHandledExceptionString()};
        }
    }
 
    PersonInstanceReader::ResultResolved
    PersonInstanceReader::queryResolved(const QueryResolved& queryResolved) const
    {
        ARMARX_DEBUG << "before query";
        const auto result = query(queryResolved);
        ARMARX_DEBUG << "after query";
 
        if (not result)
        {
            ARMARX_INFO << deactivateSpam(10)
                        << "Query for person instances failed: " << result.errorMessage;
            return ResultResolved{.personInstances = {},
                                  .status = result.status,
                                  .errorMessage = result.errorMessage};
        }
 
        const auto isFullySpecified = [](const armem::MemoryID& memoryId)
        {
            return memoryId.hasMemoryName() and memoryId.hasCoreSegmentName() and
                   memoryId.hasProviderSegmentName() and memoryId.hasEntityName() and
                   memoryId.hasTimestamp() and memoryId.hasInstanceIndex();
        };
 
        const auto isEntitySpecified = [&](const armem::MemoryID& memoryId)
        {
            return memoryId.hasMemoryName() and memoryId.hasCoreSegmentName() and
                   memoryId.hasProviderSegmentName() and memoryId.hasEntityName();
        };
 
 
        // to reduce the number of network calls, we query all relevant memory elements at once
        const auto [allMemoryIds, allLatestMemoryIds] = [&]
        {
            std::vector<MemoryID> allLatestMemoryIds;
            std::vector<MemoryID> allMemoryIds;
            for (const auto& personInstance : result.personInstances)
            {
                if (queryResolved.resolveFaceDetection)
                {
                    if (isFullySpecified(personInstance.faceRecognitionID))
                    {
                        allMemoryIds.push_back(personInstance.faceRecognitionID);
                    }
                    else if (isEntitySpecified(personInstance.faceRecognitionID))
                    {
                        allLatestMemoryIds.push_back(personInstance.faceRecognitionID);
                    }
                    else
                    {
                        ARMARX_DEBUG << "Face recognition ID is not even entity specified: "
                                       << QUOTED(personInstance.faceRecognitionID);
                    }
                }
 
                if (queryResolved.resolveHumanPose)
                {
                    if (isFullySpecified(personInstance.poseID))
                    {
                        allMemoryIds.push_back(personInstance.poseID);
                    }
                    else if (isEntitySpecified(personInstance.poseID))
                    {
                        allLatestMemoryIds.push_back(personInstance.poseID);
                    }
                    else
                    {
                        ARMARX_DEBUG << "Human pose ID is not even entity specified: "
                                       << QUOTED(personInstance.poseID);
                    }
                }
 
                if (queryResolved.resolveProfile)
                {
                    if (isFullySpecified(personInstance.profileID))
                    {
                        allMemoryIds.push_back(personInstance.profileID);
                    }
                    else if (isEntitySpecified(personInstance.profileID))
                    {
                        allLatestMemoryIds.push_back(personInstance.profileID);
                    }
                    else
                    {
                        ARMARX_DEBUG << "Profile ID is not even entity specified: "
                                       << QUOTED(personInstance.profileID);
                    }
                }
            }
            return std::pair{allMemoryIds, allLatestMemoryIds};
        }();
 
        ARMARX_DEBUG << "Need to query " << allMemoryIds.size() << " memory elements";
 
        ARMARX_DEBUG << "Querying memory ids";
        auto queryResult = memoryReader().queryMemoryIDs(allMemoryIds);
 
        // If the instances are not fully specified, we need to query the latest snapshots instead
        for (const auto& memId : allLatestMemoryIds)
        {
            const auto qRes = memoryReader().getLatestSnapshotsIn(memId);
            queryResult.memory.append(qRes.memory);
        }
 
        ARMARX_DEBUG << "Querying memory ids done";
 
        const auto resolve =
            [&queryResolved, &queryResult, &isEntitySpecified](
                const PersonInstance& personInstance) -> MemoryResolvedPersonInstance
        {
            MemoryResolvedPersonInstance resolvedInstance;
 
            // assign the data based on the memory result
            {
                if (queryResolved.resolveFaceDetection)
                {
                    ARMARX_DEBUG << "Face recognition: " << personInstance.faceRecognitionID;
                    const auto& memoryID = personInstance.faceRecognitionID;
 
                    const auto* instance =
                        isEntitySpecified(memoryID) and queryResult.memory.hasEntity(memoryID)
                            ? queryResult.memory.getEntity(memoryID).findLatestInstance()
                            : nullptr;
                    if (instance != nullptr)
                    {
                        ARMARX_DEBUG << "Face recognition.";
                        const auto data =
                            instance->dataAs<armarx::human::arondto::FaceRecognition>();
 
                        auto& faceRecognition = resolvedInstance.faceRecognition.emplace();
                        fromAron(data, faceRecognition);
                    }
                    else
                    {
                        ARMARX_INFO << deactivateSpam(10)
                                    << "Requested face detection but failed to resolve "
                                    << QUOTED(memoryID);
                    }
                }
 
                if (queryResolved.resolveHumanPose)
                {
                    ARMARX_DEBUG << "Human pose id: " << personInstance.poseID;
                    const auto& memoryID = personInstance.poseID;
 
                    const auto* instance =
                        isEntitySpecified(memoryID) and queryResult.memory.hasEntity(memoryID)
                            ? queryResult.memory.getEntity(memoryID).findLatestInstance()
                            : nullptr;
                    if (instance != nullptr)
                    {
                        const auto data = instance->dataAs<armarx::human::arondto::HumanPose>();
 
                        auto& humanPose = resolvedInstance.humanPose.emplace();
                        fromAron(data, humanPose);
                    }
                    else
                    {
                        ARMARX_INFO << deactivateSpam(10)
                                    << "Requested human pose but failed to resolve "
                                    << QUOTED(memoryID);
                    }
                }
 
                if (queryResolved.resolveProfile)
                {
                    ARMARX_INFO << deactivateSpam(10) << "Profile: " << personInstance.profileID;
                    const auto& memoryID = personInstance.profileID;
 
                    const auto* instance =
                        isEntitySpecified(memoryID) and queryResult.memory.hasEntity(memoryID)
                            ? queryResult.memory.getEntity(memoryID).findLatestInstance()
                            : nullptr;
                    if (instance != nullptr)
                    {
                        ARMARX_DEBUG << "Profile";
                        const auto data = instance->dataAs<armarx::human::arondto::Person>();
 
                        auto& profile = resolvedInstance.profile.emplace();
                        fromAron(data, profile);
                    }
                    else
                    {
                        ARMARX_INFO << deactivateSpam(10)
                                    << "Requested profile but failed to resolve "
                                    << QUOTED(memoryID);
                    }
                }
            }
 
            resolvedInstance.globalPose = personInstance.globalPose;
            resolvedInstance.personInstance = personInstance;
 
            return resolvedInstance;
        };
 
 
        PersonInstanceReader::ResultResolved resultResolved;
        resultResolved.personInstances =
            result.personInstances | ranges::views::transform(resolve) | ranges::to_vector;
        resultResolved.errorMessage = "";
        resultResolved.status = ResultResolved::Status::Success; // FIXME refine
 
        return resultResolved;
    }
 
 
} // namespace armarx::armem::human::client