HumanActivityWriter.cpp

Go to the documentation of this file.

#include "HumanActivityWriter.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
{
 
    ActivityWriter::ActivityWriter() : armem::client::util::SimpleWriterBase()
    {
    }
 
    ActivityWriter::~ActivityWriter() = default;
 
    void
    ActivityWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        armem::client::util::SimpleWriterBase::registerPropertyDefinitions(def);
    }
 
    void
    ActivityWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        ARMARX_VERBOSE << "Connecting ...";
        armem::client::util::SimpleWriterBase::connect(memoryNameSystem);
        ARMARX_VERBOSE << "Connected.";
 
        reader.connect(memoryNameSystem);
    }
 
    bool
    ActivityWriter::commitHumanActivity(const HumanActivity& activity,
                                        const std::string& provider,
                                        const armem::Time& referencedTime)
    {
        return this->commitHumanActivities({activity}, provider, referencedTime);
    }
 
    bool
    ActivityWriter::commitHumanActivities(const std::vector<HumanActivity>& activities,
                                          const std::string& provider,
                                          const armem::Time& referencedTime)
    {
        if (activities.empty())
        {
            ARMARX_WARNING << "No activities to commit";
            return false;
        }
 
        try
        {
            // Group activities by person (entity)
            std::map<std::string, std::vector<HumanActivity>> activitiesByPerson;
 
            for (const HumanActivity& activity : activities)
            {
                std::string entityName;
                if (activity.personID.has_value())
                {
                    entityName = activity.personID.value().toMemoryEntityID();
                }
                else
                {
                    // If no person ID, use a default entity name
                    entityName = "unknown-person";
                }
 
                activitiesByPerson[entityName].push_back(activity);
            }
 
            // Commit each entity separately
            for (const auto& [entityName, entityActivities] : activitiesByPerson)
            {
                // Convert activities to DTO
                std::vector<armarx::aron::data::DictPtr> activityDtos;
                for (const HumanActivity& activity : entityActivities)
                {
                    armarx::human::arondto::HumanActivity activityAron;
                    toAron(activityAron, activity);
                    activityDtos.push_back(activityAron.toAron());
                }
 
                // Create entity ID
                armarx::armem::MemoryID entityId =
                    armarx::human::HumanActivityCoreSegmentID.withProviderSegmentName(provider)
                        .withEntityName(entityName);
 
                // Create entity update
                armarx::armem::EntityUpdate update = {
                    .entityID = entityId,
                    .instancesData = activityDtos,
                    .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_DEBUG << "Committing " << entityActivities.size()
                             << " activities for entity " << entityName;
 
                memoryWriter().commit(commit);
            }
 
            return true;
        }
        catch (...)
        {
            ARMARX_WARNING << "Failed to commit human activities: " << GetHandledExceptionString();
            return false;
        }
    }
 
 
} // namespace armarx::armem::human::client