MotionSegment.cpp

Go to the documentation of this file.

// BaseClass
#include "MotionSegment.h"
 
// ArmarX
#include "mdb_conversions.h"
 
#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
 
// STD / STL
#include <iostream>
#include <fstream>
#include <sstream>
 
 
namespace armarx::armem::server::motions::mdb::segment
{
    MDBMotionSegment::MDBMotionSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        Base(memoryToIceAdapter, "MotionDatabase", "Motion", armarx::motion::mdb::arondto::MDBReference::ToAronType())
    {
    }
 
    void MDBMotionSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        Base::defineProperties(defs, prefix);
 
        defs->optional(properties.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from.");
        defs->optional(properties.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup.");
    }
 
    void MDBMotionSegment::init()
    {
        Base::init();
 
        if (properties.loadFromMotionsPackage)
        {
            loadByMotionFinder(properties.motionsPackage);
        }
    }
 
    void MDBMotionSegment::onConnect()
    {
 
    }
 
    int MDBMotionSegment::loadByMotionFinder(const std::string& packageName)
    {
        priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/");
        int loadedMotions = 0;
 
        ARMARX_INFO << "Load Motions from package" << std::endl;
        {
            // Load MDB Motions
            auto allMotions = motionFinder.findAll("mdb");
            for (const auto& motionFinderInfo : allMotions)
            {
                auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID() / (motionFinderInfo.getID() + ".json");
                if (auto op = conversion::createFromFile(pathToInfoJson); op.has_value())
                {
                    std::stringstream ss;
                    ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: ";
                    for (const auto& [key, attachedFiles] : op->attachedFiles)
                    {
                        for (const auto& attachedFile : attachedFiles)
                        {
                            ss << "\n" << attachedFile.fileName << " (" << key << ")";
                        }
                    }
                    ARMARX_INFO << ss.str();  // ToDo @Fabian PK: Remove
                    auto& entity = this->segmentPtr->addEntity(std::to_string(op->id));
                    auto& snapshot = entity.addSnapshot(op->createdDate);
 
                    armem::wm::EntityInstance& instance = snapshot.addInstance();
                    instance.metadata().referencedTime = op->createdDate;
                    instance.metadata().sentTime = Time::Now();
                    instance.metadata().arrivedTime = Time::Now();
                    instance.metadata().confidence = 1.0;
                    instance.data() = op->toAron();
                }
                else
                {
                    ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
                }
            }
 
            loadedMotions += allMotions.size();
        }
 
        return loadedMotions;
    }
}