KnownGraspProviderSegment.cpp

Go to the documentation of this file.

#include "KnownGraspProviderSegment.h"
 
#include <cstring>
#include <filesystem>
#include <map>
#include <optional>
#include <string>
#include <vector>
 
#include <sys/inotify.h>
#include <sys/types.h>
#include <unistd.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Grasping/Grasp.h> // IWYU pragma: keep
#include <VirtualRobot/Grasping/GraspSet.h> // IWYU pragma: keep
#include <VirtualRobot/ManipulationObject.h> // IWYU pragma: keep
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/ObjectIO.h>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include "RobotAPI/libraries/ArmarXObjects/ObjectFinder.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
#include "RobotAPI/libraries/armem/core/Commit.h"
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
 
#include <linux/limits.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();
 
        installFileWatcher();
    }
 
    std::optional<arondto::KnownGraspInfo>
    KnownGraspProviderSegment::knownGraspInfoFromObjectInfo(const ObjectInfo& info)
    {
        return knownGraspInfoFromFile(GraspFileInfo::FromObjectInfo(info));
    }
 
    std::optional<arondto::KnownGraspInfo>
    KnownGraspProviderSegment::knownGraspInfoFromFile(const GraspFileInfo& graspFileInfo)
    {
 
        const std::filesystem::path graspFilePath = graspFileInfo.fileLocInfo.absolutePath;
        if (not std::filesystem::is_regular_file(graspFilePath))
        {
            return std::nullopt; // file does not exist
        }
 
        ARMARX_VERBOSE << "Loading `" << graspFilePath << "`.";
        try
        {
            const std::string objectClassName = graspFileInfo.objectId.className();
 
            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 = graspFileInfo.objectId.str();
            ret.xml.package = graspFileInfo.fileLocInfo.package;
            ret.xml.path = graspFileInfo.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();
 
                VirtualRobot::GraspSetPtr preGraspSet = [&graspSet]()
                {
                    std::vector<VirtualRobot::GraspPtr> preGrasps;
                    for (const auto& grasp : graspSet->getGrasps())
                    {
                        // check if grasp is a prepose by checking if grasp name ends with "_Prepose"
                        if (simox::alg::ends_with(grasp->getName(), PREPOSE_SUFFIX))
                        {
                            preGrasps.emplace_back(grasp);
                        }
 
                        // everything else is assumed to be a grasp
                    }
 
                    // create a new grasp set with the preposes
                    VirtualRobot::GraspSetPtr preGraspSet = graspSet->clone();
                    preGraspSet->removeAllGrasps(); // just keep the EEF info etc
                    for (const auto& preGrasp : preGrasps)
                    {
                        preGraspSet->addGrasp(preGrasp);
                    }
 
                    return preGraspSet;
                }();
 
 
                ARMARX_DEBUG << VAROUT(preGraspSet->getSize());
 
                // remove all preposes from the grasp set
                for (const auto& preGrasp : preGraspSet->getGrasps())
                {
                    graspSet->removeGrasp(preGrasp);
                }
 
 
                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();
                    retGrasp.prepose.reset();
 
                    // check if grasp has a prepose by checking if grasp name ends with "_Prepose"
                    {
                        const std::string prePoseName = retGrasp.name + PREPOSE_SUFFIX;
 
                        ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
 
                        if (preGraspSet->hasGrasp(prePoseName))
                        {
                            retGrasp.prepose =
                                preGraspSet->getGrasp(prePoseName)->getTransformation();
 
                            // remove the prepose from the set as it found its match
                            preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
 
 
                            ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '"
                                         << retGrasp.name << "' in set '" << retGraspSet.name
                                         << "' for obj '" << objectClassName << "' with pose \n"
                                         << retGrasp.prepose.value();
                        }
                    }
 
                    // check if grasp has a prepose for a grasp with a specific name, e.g., "XY_Grasp" (GRASP_OPTIONAL_SUFFIX)
                    if ( // not retGrasp.prepose.has_value() and
                        simox::alg::ends_with(retGrasp.name, GRASP_OPTIONAL_SUFFIX))
                    {
                        const std::string prePoseName =
                            retGrasp.name.substr(
                                0, retGrasp.name.size() - std::strlen(GRASP_OPTIONAL_SUFFIX)) +
                            PREPOSE_SUFFIX;
 
                        ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
 
                        if (preGraspSet->hasGrasp(prePoseName))
                        {
                            retGrasp.prepose =
                                preGraspSet->getGrasp(prePoseName)->getTransformation();
 
                            // remove the prepose from the set as it found its match
                            preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
 
                            ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '"
                                         << retGrasp.name << "' in set '" << retGraspSet.name
                                         << "' for obj '" << objectClassName << "' with pose \n"
                                         << retGrasp.prepose.value();
                        }
                    }
 
                    ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
                                 << retGraspSet.name << "' for obj '" << objectClassName
                                 << "' with pose \n"
                                 << retGrasp.pose;
 
                    retGraspSet.grasps.push_back(retGrasp);
                }
 
                // Now, check if there are any preposes left in the set. This should not have happened.
                if (preGraspSet->getSize() > 0)
                {
                    ARMARX_WARNING << "Found " << preGraspSet->getSize()
                                   << " preposes in the grasp set '" << retGraspSet.name
                                   << "' for obj '" << objectClassName
                                   << "' that do not have a corresponding grasp!";
                    for (const auto& preGrasp : preGraspSet->getGrasps())
                    {
                        ARMARX_WARNING << "Prepose '" << preGrasp->getName();
                    }
                }
 
                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;
        }
    }
 
    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_DEBUG << VAROUT(knownGraspCandidate->graspSets.size());
                for (const auto& gs : knownGraspCandidate->graspSets)
                {
                    ARMARX_DEBUG << VAROUT(gs.second.grasps.size());
                    for (const auto& grasp : gs.second.grasps)
                    {
                        ARMARX_DEBUG << 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();
        }
    }
 
    void
    KnownGraspProviderSegment::installFileWatcher()
    {
        // load data from prior knowledge
        const ObjectFinder objectFinder;
        const std::vector<ObjectInfo> infos = objectFinder.findAllObjects(false);
 
        const bool autoReloadSceneSnapshotsOnFileChange = true; // FIXME param
 
        if (autoReloadSceneSnapshotsOnFileChange)
        {
            int inotifyFd;
 
            // init inotify
            {
 
                inotifyFd = inotify_init();
                if (inotifyFd == -1)
                {
                    ARMARX_WARNING << "inotify_init failed";
                }
            }
 
            std::map<int, GraspFileInfo> wds;
 
            // set up inotify watchers
            for (const auto& info : infos)
            {
                const auto graspFileInfo = GraspFileInfo::FromObjectInfo(info);
 
                if (std::filesystem::exists(graspFileInfo.fileLocInfo.absolutePath))
                {
                    auto wd = inotify_add_watch(
                        inotifyFd, graspFileInfo.fileLocInfo.absolutePath.c_str(), IN_MODIFY);
                    if (wd == -1)
                    {
                        ARMARX_WARNING << "inotify_add_watch for grasp file: "
                                       << graspFileInfo.fileLocInfo.absolutePath << "` failed.";
                    }
 
                    ARMARX_VERBOSE << "inotify_add_watch for grasp file: "
                                << graspFileInfo.fileLocInfo.absolutePath << "` added.";
                    wds.emplace(wd, graspFileInfo);
                }
            }
 
            ARMARX_INFO << "Set up " << wds.size() << " inotify events to watch for file changes.";
 
            fileWatcherTask = new SimpleRunningTask<>(
                [this, inotifyFd, wds]()
                {
                    ObjectFinder objectFinder;
 
                    constexpr std::size_t BUF_LEN = (10 * (sizeof(inotify_event) + NAME_MAX + 1));
                    char buf[BUF_LEN];
 
                    for (;;)
                    {
                        ssize_t numRead;
 
                        numRead = read(inotifyFd, buf, BUF_LEN);
                        if (numRead == 0)
                        {
                            ARMARX_WARNING << "read() from inotify fd returned 0!";
                        }
 
                        if (numRead == -1)
                        {
                            ARMARX_VERBOSE << "read";
                        }
 
                        ARMARX_DEBUG << VAROUT(numRead);
 
                        for (char* p = buf; p < buf + numRead;)
                        {
                            auto* event = reinterpret_cast<inotify_event*>(p);
 
                            const auto& graspFileInfo = wds.at(event->wd);
                            ARMARX_VERBOSE << "File changed: "
                                        << VAROUT(graspFileInfo.fileLocInfo.absolutePath);
 
                            ARMARX_INFO << "Reloading file for object `" << graspFileInfo.objectId << "`.";
 
                            p += sizeof(struct inotify_event) + event->len;
 
                            const bool lockMemory = true;
 
                            // read updated grasp file and store it in the memory
                            {
                                const MemoryID providerID =
                                    segmentPtr->id().withProviderSegmentName(
                                        objectFinder.getPackageName());
 
                                // create commit from new file content
                                Commit commit;
                                {
                                    if (auto knownGraspCandidate =
                                            knownGraspInfoFromFile(graspFileInfo);
                                        knownGraspCandidate)
                                    {
                                        EntityUpdate& update = commit.add();
                                        update.entityID =
                                            providerID.withEntityName(graspFileInfo.objectId.str());
                                        update.entityID.timestamp = update.arrivedTime =
                                            update.referencedTime = update.sentTime =
                                                armarx::Clock::Now();
 
                                        update.instancesData = {knownGraspCandidate->toAron()};
 
                                        ARMARX_DEBUG
                                            << VAROUT(knownGraspCandidate->graspSets.size());
                                        for (const auto& gs : knownGraspCandidate->graspSets)
                                        {
                                            ARMARX_DEBUG << VAROUT(gs.second.grasps.size());
                                            for (const auto& grasp : gs.second.grasps)
                                            {
                                                ARMARX_DEBUG << VAROUT(grasp.name);
                                            }
                                        }
                                    }
                                }
 
                                // now store commit in memory
                                if (not commit.updates.empty())
                                {
                                    ARMARX_VERBOSE << "Storing update in memory";
 
                                    const auto result = iceMemory.commit(commit);
                                    if (not result.allSuccess())
                                    {
                                        ARMARX_WARNING << "Got errors for commit: "
                                                       << result.allErrorMessages();
                                    }
                                }
                            }
                        }
                    }
                });
 
            fileWatcherTask->start();
        }
    }
} // namespace armarx::armem::grasping::segment