HumanRobotInteractionReader.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/>.
 *
 * @author     Joana Plewnia ( joana dot plewnia at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "HumanRobotInteractionReader.h"
 
#include <iterator>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
 
#include <RobotAPI/libraries/armem/util/util.h>
 
#include <VisionX/libraries/armem_human/aron/HumanRobotInteraction.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
 
namespace armarx::armem::human::client
{
    InteractionReader::~InteractionReader() = default;
 
    armarx::armem::client::query::Builder
    InteractionReader::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;
    }
 
    ::armarx::armem::client::query::Builder
    InteractionReader::buildLatestQuery(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().latest();
 
        return qb;
    }
 
    std::vector<HumanRobotInteraction>
    InteractionReader::asHumanRobotInteractions(const wm::ProviderSegment& providerSegment,
                                                 const DateTime& timestamp,
                                                 const Duration& maxAge) const
    {
        ARMARX_TRACE;
        std::vector<HumanRobotInteraction> interactions;
 
        providerSegment.forEachEntity(
            [&](const wm::Entity& entity)
            {
                ARMARX_TRACE;
 
                const auto& entitySnapshot = entity.getLatestSnapshot();
 
                entitySnapshot.forEachInstance(
                    [&interactions, &timestamp, &maxAge](const auto& 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::HumanRobotInteraction>(
                                    entityInstance);
                            ARMARX_CHECK(aronDto) << "Failed casting to HumanRobotInteraction";
 
                            HumanRobotInteraction interaction;
                            fromAron(*aronDto, interaction);
 
                            interactions.push_back(interaction);
                        };
                    });
            });
 
        return interactions;
    }
 
    std::string
    InteractionReader::propertyPrefix() const
    {
        return "mem.human.interaction.";
    }
 
    armarx::armem::client::util::SimpleReaderBase::Properties
    InteractionReader::defaultProperties() const
    {
        return {.memoryName = "Human", .coreSegmentName = "HumanRobotInteraction"};
    }
 
    InteractionReader::Result
    InteractionReader::query(const Query& query, bool latest) const
    {
        ARMARX_TRACE;
 
        ::armarx::armem::client::query::Builder qb;
        if (latest)
        {
            qb = buildLatestQuery(query);
        }
        else
        {
            qb = buildQuery(query);
        }
 
        ARMARX_INFO << "[InteractionReader] Querying memory...";
        ARMARX_INFO << "[InteractionReader] Memory name: " << properties().memoryName;
        ARMARX_INFO << "[InteractionReader] Core segment: " << properties().coreSegmentName;
        ARMARX_INFO << "[InteractionReader] Provider: " << query.providerName;
 
        const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
 
        ARMARX_INFO << "[InteractionReader] Query result: success=" << qResult.success;
        if (!qResult.success)
        {
            ARMARX_INFO << "[InteractionReader] Error: " << qResult.errorMessage;
        }
 
        if (not qResult.success)
        {
            ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
            return {.interactions = {},
                    .status = Result::Status::Error,
                    .errorMessage = qResult.errorMessage};
        }
 
        ARMARX_TRACE;
 
        // Check if core segment exists
        if (!qResult.memory.hasCoreSegment(properties().coreSegmentName))
        {
            ARMARX_WARNING << "[InteractionReader] Core segment '" << properties().coreSegmentName
                           << "' does not exist in memory!";
            ARMARX_INFO << "[InteractionReader] Available core segments:";
            qResult.memory.forEachCoreSegment([](const auto& segment)
                                              { ARMARX_INFO << "  - " << segment.name(); });
            return {.interactions = {},
                    .status = Result::Status::NoData,
                    .errorMessage = "Core segment not found"};
        }
 
        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
 
        if (query.providerName.empty())
        {
            ARMARX_TRACE;
 
            std::vector<HumanRobotInteraction> allInteractions;
 
            coreSegment.forEachProviderSegment(
                [this, &allInteractions, &query](const auto& providerSegment)
                {
                    const std::vector<HumanRobotInteraction> interactions =
                        asHumanRobotInteractions(providerSegment, query.timestamp, query.maxAge);
                    std::copy(interactions.begin(),
                              interactions.end(),
                              std::back_inserter(allInteractions));
                });
 
            if (allInteractions.empty())
            {
                return {.interactions = {},
                        .status = Result::Status::NoData,
                        .errorMessage = "No entities"};
            }
 
            return Result{.interactions = allInteractions, .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 {.interactions = {}, .status = Result::Status::NoData};
        }
 
        ARMARX_TRACE;
 
        const wm::ProviderSegment& providerSegment =
            coreSegment.getProviderSegment(query.providerName);
 
        if (providerSegment.empty())
        {
            ARMARX_WARNING << "No entities.";
            return {.interactions = {},
                    .status = Result::Status::NoData,
                    .errorMessage = "No entities"};
        }
 
        try
        {
            ARMARX_TRACE;
 
            const auto interactions =
                asHumanRobotInteractions(providerSegment, query.timestamp, query.maxAge);
            return Result{.interactions = interactions, .status = Result::Status::Success};
        }
        catch (...)
        {
            return Result{.status = Result::Status::Error,
                          .errorMessage = GetHandledExceptionString()};
        }
    }
 
    std::vector<HumanRobotInteraction>
    InteractionReader::queryInteractionsForPerson(const std::string& firstName,
                                                  const std::string& lastName,
                                                  const Duration& maxAge,
                                                  const std::string& providerName)
    {
        Query queryDefinition = Query{.providerName = providerName,
                                      .timestamp = armem::Time::Now(),
                                      .maxAge = maxAge};
 
        auto queryResult = query(queryDefinition, true);
 
        std::vector<HumanRobotInteraction> matchingInteractions;
 
        if (queryResult.status == Result::Status::Success)
        {
            for (const auto& interaction : queryResult.interactions)
            {
                if (interaction.personID.has_value() &&
                    interaction.personID.value().firstName == firstName &&
                    interaction.personID.value().lastName == lastName)
                {
                    matchingInteractions.push_back(interaction);
                }
            }
 
            if (matchingInteractions.empty())
            {
                ARMARX_INFO << "No interactions found for person " << firstName << " "
                            << lastName;
            }
        }
        else
        {
            ARMARX_WARNING << "Failed to query interactions from memory";
        }
 
        return matchingInteractions;
    }
 
    std::optional<HumanRobotInteraction>
    InteractionReader::queryLatestInteractionForPerson(const std::string& firstName,
                                                       const std::string& lastName,
                                                       const std::string& providerName)
    {
        Query queryDefinition = Query{.providerName = providerName,
                                      .timestamp = armem::Time::Now(),
                                      .maxAge = armarx::Duration::Minutes(1000)};
 
        auto queryResult = query(queryDefinition, true);
 
        if (queryResult.status == Result::Status::Success)
        {
            // Find the latest interaction for the given person
            std::optional<HumanRobotInteraction> latestInteraction;
            DateTime latestTime;
 
            for (const auto& interaction : queryResult.interactions)
            {
                if (interaction.personID.has_value() &&
                    interaction.personID.value().firstName == firstName &&
                    interaction.personID.value().lastName == lastName)
                {
                    if (!latestInteraction.has_value() || interaction.startTime > latestTime)
                    {
                        latestInteraction = interaction;
                        latestTime = interaction.startTime;
                    }
                }
            }
 
            if (latestInteraction.has_value())
            {
                return latestInteraction;
            }
            else
            {
                ARMARX_INFO << "No interactions found for person " << firstName << " "
                            << lastName;
            }
        }
        else
        {
            ARMARX_WARNING << "Failed to query interactions from memory";
        }
 
        return std::nullopt;
    }
 
} // namespace armarx::armem::human::client