ComposedSkillLibrarySegment.cpp

#include "ComposedSkillLibrarySegment.h"

#include <optional>

#include <SimoxUtility/algorithm/string.h>

#include <ArmarXCore/core/time/ice_conversions.h>

#include "RobotAPI/libraries/armem/client/Query.h"
#include "RobotAPI/libraries/armem/client/forward_declarations.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
#include "RobotAPI/libraries/armem/core/wm/memory_definitions.h"
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/datatype/DatatypeConverterVisitor.h>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
#include <RobotAPI/libraries/skills/core/aron/FluxioSkill.aron.generated.h>

namespace armarx::skills::segment
{
    ComposedSkillLibraryCoreSegment::ComposedSkillLibraryCoreSegment(
        armem::server::MemoryToIceAdapter& iceMemory) :
        Base(iceMemory, CoreSegmentName, skills::manager::arondto::FluxioSkill::ToAronType())
    {
    }

    void
    ComposedSkillLibraryCoreSegment::defineProperties(PropertyDefinitionsPtr defs,
                                                      const std::string& prefix)
    {
        this->setDefaultMaxHistorySize(3);
        Base::defineProperties(defs, prefix);
    }

    void
    ComposedSkillLibraryCoreSegment::init()
    {
        Base::init();
    }

    void
    ComposedSkillLibraryCoreSegment::addSkill(const skills::manager::arondto::FluxioSkill& skill)
    {
        // add skills
        auto provId = id().withProviderSegmentName(skill.skillProviderId.id);

        armem::Commit commit;
        auto& entityUpdate = commit.add();
        entityUpdate.confidence = 1.0;
        entityUpdate.referencedTime = armem::Time::Now();
        entityUpdate.sentTime = armem::Time::Now();
        entityUpdate.arrivedTime = armem::Time::Now();
        entityUpdate.instancesData = {skill.toAron()};
        entityUpdate.entityID = provId.withEntityName(skill.id);

        // Commit data to memory and notify
        iceMemory.commit(commit);
    }

    std::optional<std::vector<skills::manager::arondto::FluxioSkill>>
    ComposedSkillLibraryCoreSegment::getSkills() const
    {
        std::vector<skills::manager::arondto::FluxioSkill> ret;
        armarx::armem::client::query::Builder qb;
        qb.allLatestInCoreSegment(id());
        armem::client::QueryResult qresult = iceMemory.query(qb.buildQueryInput());
        if (!qresult.success)
        {
            ARMARX_WARNING << "Query failed: " << qresult.errorMessage;
            return std::nullopt;
        }

        const armem::wm::Memory& data = qresult.memory;

        data.forEachInstance(
            [&ret](const armem::wm::EntityInstance& instance)
            {
                auto skill = instance.dataAs<skills::manager::arondto::FluxioSkill>();
                ret.push_back(skill);
            });

        return ret;
    }
} // namespace armarx::skills::segment