KnownGraspProviderSegment.cpp

Go to the documentation of this file.

#include "KnownGraspProviderSegment.h"
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/XML/ObjectIO.h>
 
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
 
namespace armarx::armem::grasping::segment
{
    KnownGraspProviderSegment::KnownGraspProviderSegment(armem::server::MemoryToIceAdapter& iceMemory) :
        Base(iceMemory, PROVIDER_SEGMENT_NAME, CORE_SEGMENT_NAME)
    {
 
    }
 
    void KnownGraspProviderSegment::init()
    {
        Base::init();
 
        loadMemory();
    }
 
    std::optional<arondto::KnownGraspInfo> KnownGraspProviderSegment::knownGraspInfoFromObjectInfo(const ObjectInfo& info)
    {
        std::string objectClassName = info.className();
        auto fileLocInfo = info.file(".xml", "_Grasps");
        std::filesystem::path graspFilePath = fileLocInfo.absolutePath;
 
        if (std::filesystem::is_regular_file(graspFilePath))
        {
            ARMARX_INFO << "loading " << graspFilePath;
            try {
                auto manipulationObject = VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
            
                if(manipulationObject == nullptr)
                {
                    ARMARX_WARNING << "Invalid file content: " << graspFilePath;
                    return std::nullopt;
                }
 
                arondto::KnownGraspInfo ret;
                ret.correspondingObject.memoryName = "Object";
                ret.correspondingObject.coreSegmentName = "Class";
                ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
                ret.correspondingObject.entityName = info.idStr();
                ret.xml.package = fileLocInfo.package;
                ret.xml.path = fileLocInfo.relativePath;
 
                for (const VirtualRobot::GraspSetPtr& graspSet : manipulationObject->getAllGraspSets())
                {
                    ARMARX_CHECK_NOT_NULL(graspSet);
 
                    arondto::KnownGraspSet retGraspSet;
 
                    retGraspSet.name = graspSet->getName();
                    retGraspSet.robot = graspSet->getRobotType();
                    retGraspSet.endeffector = graspSet->getEndEffector();
 
                    for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
                    {
                        ARMARX_CHECK_NOT_NULL(grasp);
 
                        arondto::KnownGrasp retGrasp;
 
                        retGrasp.name = grasp->getName();
                        retGrasp.quality = grasp->getQuality();
                        retGrasp.creator = grasp->getCreationMethod();
                        retGrasp.pose = grasp->getTransformation();
 
                        ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
 
                        retGraspSet.grasps.push_back(retGrasp);
                    }
                    ret.graspSets[retGraspSet.name] = retGraspSet;
                }
                return ret;
 
            } catch (...) {
                ARMARX_WARNING << graspFilePath << " is not a manipulation object!";
                return std::nullopt;
            }
        }
        return std::nullopt;
    }
 
    void KnownGraspProviderSegment::loadMemory()
    {
        // load data from prior knowledge
        ObjectFinder objectFinder;
        const auto now = armem::Time::Now();
 
        const bool checkPaths = false;
        std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
 
        const MemoryID providerID = segmentPtr->id().withProviderSegmentName(objectFinder.getPackageName());
        ARMARX_INFO << "Checking up to " << infos.size() << " object classes from '" << objectFinder.getPackageName() << "' ...";
 
        Commit commit;
        for (ObjectInfo& info : infos)
        {
            info.setLogError(false);
            if (auto knownGraspCandidate = knownGraspInfoFromObjectInfo(info); knownGraspCandidate)
            {
                EntityUpdate& update = commit.add();
                update.entityID = providerID.withEntityName(info.id().str());
                update.entityID.timestamp = update.arrivedTime = update.referencedTime = update.sentTime = now;
 
                update.instancesData =
                {
                    knownGraspCandidate->toAron()
                };
            }
        }
        ARMARX_INFO << "Loaded " << commit.updates.size() << " grasp candidates from object classes from '" << objectFinder.getPackageName() << "'.";
        auto result = iceMemory.commit(commit);
        if (!result.allSuccess())
        {
            ARMARX_WARNING << "Got errors for commit: " << result.allErrorMessages();
        }
    }
}