KnownGraspProviderSegment.cpp

#include "KnownGraspProviderSegment.h"

#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/XML/ObjectIO.h>

#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>

#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.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 = simox::alg::split(graspSet->getRobotType(), " ").front();

                    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);
                    }

                    ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) ==
                                 0)
                        << "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name
                        << "` was defined twice!";

                    ret.graspSets[retGraspSet.robot + "/" + retGraspSet.name] = retGraspSet;
                }
                return ret;
            }
            catch (...)
            {
                ARMARX_WARNING << graspFilePath << " is not a manipulation object!"
                               << GetHandledExceptionString();
                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_VERBOSE << VAROUT(knownGraspCandidate->graspSets.size());
                for (const auto& gs : knownGraspCandidate->graspSets)
                {
                    ARMARX_VERBOSE << VAROUT(gs.second.grasps.size());
                    for (const auto& grasp : gs.second.grasps)
                    {
                        ARMARX_VERBOSE << VAROUT(grasp.name);
                    }
                }
            }
        }
        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();
        }
    }
} // namespace armarx::armem::grasping::segment