HumanRobotInteractionWriter.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 "HumanRobotInteractionWriter.h"
 
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
 
#include <VisionX/libraries/armem_human/aron_conversions.h>
#include <VisionX/libraries/armem_human/memory_ids.h>
 
namespace armarx::armem::human::client
{
 
    InteractionWriter::InteractionWriter() : armem::client::util::SimpleWriterBase()
    {
    }
 
    InteractionWriter::~InteractionWriter() = default;
 
    void
    InteractionWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        armem::client::util::SimpleWriterBase::registerPropertyDefinitions(def);
    }
 
    void
    InteractionWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        ARMARX_VERBOSE << "Connecting ...";
        armem::client::util::SimpleWriterBase::connect(memoryNameSystem);
        ARMARX_VERBOSE << "Connected.";
 
        reader.connect(memoryNameSystem);
    }
 
    bool
    InteractionWriter::commitHumanRobotInteraction(const HumanRobotInteraction& interaction,
                                                   const std::string& provider,
                                                   const armem::Time& referencedTime)
    {
        return this->commitHumanRobotInteractions({interaction}, provider, referencedTime);
    }
 
    bool
    InteractionWriter::commitHumanRobotInteractions(
        const std::vector<HumanRobotInteraction>& interactions,
        const std::string& provider,
        const armem::Time& referencedTime)
    {
        if (interactions.empty())
        {
            ARMARX_WARNING << "No interactions to commit";
            return false;
        }
 
        try
        {
            // Group interactions by person (entity)
            std::map<std::string, std::vector<HumanRobotInteraction>> interactionsByPerson;
 
            for (const HumanRobotInteraction& interaction : interactions)
            {
                std::string entityName;
                if (interaction.personID.has_value())
                {
                    entityName = interaction.personID.value().toMemoryEntityID();
                }
                else
                {
                    // If no person ID, use a default entity name
                    entityName = "unknown-person";
                }
 
                interactionsByPerson[entityName].push_back(interaction);
            }
 
            // Commit each entity separately
            for (const auto& [entityName, entityInteractions] : interactionsByPerson)
            {
                // Convert interactions to DTO
                std::vector<armarx::aron::data::DictPtr> interactionDtos;
                for (const HumanRobotInteraction& interaction : entityInteractions)
                {
                    armarx::human::arondto::HumanRobotInteraction interactionAron;
                    toAron(interactionAron, interaction);
                    interactionDtos.push_back(interactionAron.toAron());
                }
 
                // Create entity ID
                armarx::armem::MemoryID entityId =
                    armarx::human::HumanRobotInteractionCoreSegmentID
                        .withProviderSegmentName(provider)
                        .withEntityName(entityName);
 
                ARMARX_INFO << "Entity ID created: " << entityId.str();
                ARMARX_INFO << "  Memory: " << entityId.memoryName;
                ARMARX_INFO << "  CoreSegment: " << entityId.coreSegmentName;
                ARMARX_INFO << "  Provider: " << entityId.providerSegmentName;
                ARMARX_INFO << "  Entity: " << entityId.entityName;
 
                // Create entity update
                armarx::armem::EntityUpdate update = {
                    .entityID = entityId,
                    .instancesData = interactionDtos,
                    .referencedTime = referencedTime,
                    .confidence = 1.0F,
                    .sentTime = armarx::armem::Time::Now(),
                    .arrivedTime = armarx::armem::Time::Now(),
                };
 
                // Commit the update
                armem::Commit commit;
                commit.add(update);
 
                ARMARX_INFO << "Committing " << entityInteractions.size()
                            << " interactions for entity " << entityName;
                ARMARX_INFO << "Commit contains " << commit.updates.size() << " updates";
 
                memoryWriter().commit(commit);
 
                ARMARX_INFO << "Commit completed successfully";
            }
 
            return true;
        }
        catch (...)
        {
            ARMARX_WARNING << "Failed to commit human-robot interactions: "
                           << GetHandledExceptionString();
            return false;
        }
    }
 
 
} // namespace armarx::armem::human::client