Segment.cpp

Go to the documentation of this file.

// BaseClass
#include "Segment.h"
 
// ArmarX
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
 
#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 
#include "motionprimitives.h"
 
// STD / STL
#include <fstream>
#include <iostream>
#include <sstream>
 
namespace armarx::armem::server::motions::mps::segment
{
    MPSegment::MPSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        Base(memoryToIceAdapter, "Trajectory", "MovementPrimitive")
    {
    }
 
    void
    MPSegment::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
    MPSegment::init()
    {
        Base::init();
 
        if (properties.loadFromMotionsPackage)
        {
            loadByMotionFinder(properties.motionsPackage);
        }
    }
 
    void
    MPSegment::onConnect()
    {
    }
 
    int
    MPSegment::loadByMotionFinder(const std::string& packageName)
    {
        priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/");
        int loadedMotions = 0;
 
        {
            auto allMotions = motionFinder.findAll("trajectories");
            for (const auto& motionFinderInfo : allMotions)
            {
                auto pathToInfoJson =
                    motionFinderInfo.getFullPath() /
                    motionFinderInfo
                        .getID(); // / (motionFinderInfo.getID() + ".csv"); // todo: needs to be adapted, account for task and joint space
                for (const auto& entry : std::filesystem::directory_iterator(pathToInfoJson))
                {
                    if (std::string(entry.path().filename()).rfind("taskspace", 0) == 0)
                    {
                        //ARMARX_IMPORTANT << entry.path().filename();
                        loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), true);
                        loadedMotions += allMotions.size();
                    }
                    /*else if (std::string(entry.path().filename()).rfind("joint-trajectory", 0) == 0)
                    {
                      loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), false);
                      loadedMotions += allMotions.size();
                    }*/
                }
            }
 
            loadedMotions += allMotions.size();
        }
 
        return loadedMotions;
    }
 
    void
    MPSegment::loadSingleMotionFinder(const std::string& pathToInfoJson,
                                      const std::string& entityName,
                                      bool taskspace)
    {
        if (auto op = util::createFromFile(pathToInfoJson, taskspace); op.has_value())
        {
            std::stringstream ss;
            ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: ";
 
            armem::wm::EntityInstance instance;
            instance.metadata().referencedTime = armem::Time::Now(); //op->createdDate;
            instance.metadata().sentTime = armem::Time::Now();
            instance.metadata().arrivedTime = armem::Time::Now();
            instance.metadata().confidence = 1.0;
 
            if (taskspace)
            {
                std::filesystem::path path(pathToInfoJson);
                for (const auto& entry : std::filesystem::directory_iterator(path.parent_path()))
                {
                    std::string newname =
                        "joint-trajectory" + std::string(path.filename()).erase(0, 20);
                    if (std::string(entry.path().filename()).rfind(newname, 0) == 0)
                    {
                        if (auto op2 = util::createFromFile(entry.path(), false);
                            op.has_value()) // here now mps::createFromFile(pathToInfoJson)
                        {
                            op->jointSpace = op2->jointSpace;
                            instance.data() = op->toAron();
                            if (this->segmentPtr->hasEntity(entityName))
                            {
                                auto& entity = this->segmentPtr->getEntity(entityName);
                                auto& snapshot = entity.addSnapshot(armem::Time::Now());
                                snapshot.addInstance(instance);
                            }
                            else
                            {
                                auto& entity = this->segmentPtr->addEntity(entityName);
                                auto& snapshot = entity.addSnapshot(armem::Time::Now());
                                snapshot.addInstance(instance);
                            }
                            ARMARX_IMPORTANT << "Full content trajectory: " << op->name;
                        }
                    }
                }
            }
        }
        else
        {
            ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
        }
    }
} // namespace armarx::armem::server::motions::mps::segment