ArmarXObjectsToMemory.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    MemoryX::ArmarXObjects::ArmarXObjectsImporter
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "ArmarXObjectsToMemory.h"
 
#include <unordered_set>
 
#include <SimoxUtility/json.h>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/util/CPPUtility/Pointer.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
 
namespace fs = std::filesystem;
 
namespace memoryx
{
 
    ArmarXObjectsToMemory::ArmarXObjectsToMemory()
    {
    }
 
    void
    ArmarXObjectsToMemory::importObjectsToPriorKnowledge(
        const std::vector<armarx::ObjectInfo>& objectInfos,
        const GridFileManagerPtr& gridFileManager,
        const PersistentObjectClassSegmentBasePrx& objectClassesSegment,
        const std::string& mongoDbName) const
    {
        int importCount = 0;
 
        for (const armarx::ObjectInfo& objectInfo : objectInfos)
        {
            const fs::path xmlFile = objectInfo.simoxXML().absolutePath;
 
            if (fs::is_regular_file(xmlFile))
            {
                const std::string className = objectInfo.idStr();
 
                ARMARX_INFO << "Importing " << objectInfo.id() << " with Simox XML " << xmlFile;
 
                memoryx::ObjectClassPtr newClass = new memoryx::ObjectClass();
                newClass->setName(className);
 
                ARMARX_INFO << "\tStoring Simox XML file: " << xmlFile;
                if (!dryRun)
                {
                    memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
                        newClass->addWrapper(
                            new memoryx::EntityWrappers::SimoxObjectWrapper(gridFileManager));
                    simoxWrapper->setAndStoreManipulationFile(xmlFile.string(), mongoDbName);
                }
 
                memoryx::EntityBasePtr oldClass =
                    objectClassesSegment->getEntityByName(newClass->getName());
                if (oldClass)
                {
                    ARMARX_INFO << "\tUpdating existing entity " << oldClass->getName()
                                << "(ID: " << oldClass->getId() << ")";
                    if (!dryRun)
                    {
                        objectClassesSegment->updateEntity(oldClass->getId(), newClass);
                    }
                }
                else
                {
                    ARMARX_INFO << "\tCreating new entity " << newClass->getName();
                    if (!dryRun)
                    {
                        objectClassesSegment->addEntity(newClass);
                    }
                }
 
                importCount++;
            }
        }
 
        ARMARX_IMPORTANT << "Imported " << importCount << " object classes.";
    }
 
    void
    ArmarXObjectsToMemory::importScenesAsSnapshots(
        const std::filesystem::path& scenesDirectory,
        const LongtermMemoryInterfacePrx& longtermMemory,
        const WorkingMemoryInterfacePrx& workingMemory,
        const PersistentObjectClassSegmentBasePrx& objectClassesSegment,
        const std::unordered_set<std::string>& scenes) const
    {
        ARMARX_TRACE;
 
        ARMARX_CHECK_NOT_NULL(longtermMemory);
        ARMARX_CHECK_NOT_NULL(workingMemory);
        ARMARX_CHECK_NOT_NULL(objectClassesSegment);
 
        memoryx::ObjectInstanceMemorySegmentBasePrx objectInstancesSegment =
            workingMemory->getObjectInstancesSegment();
        ARMARX_CHECK_NOT_NULL(objectInstancesSegment);
 
        int fileCount = 0;
        fs::recursive_directory_iterator end_iter;
        for (fs::recursive_directory_iterator dir_iter(scenesDirectory); dir_iter != end_iter;
             ++dir_iter)
        {
            if (fs::is_regular_file(dir_iter->status()) &&
                (dir_iter->path().extension() == ".json"))
            {
                const std::filesystem::path sceneJsonFile = dir_iter->path();
                const std::string snapshotName = sceneJsonFile.stem();
 
                // Check enable-list (if set).
                if (not scenes.empty() and scenes.count(snapshotName) == 0)
                {
                    ARMARX_INFO << "Skipping snapshot " << snapshotName
                                << " as it has not been selected";
                }
                else
                {
                    ARMARX_INFO << "Found snapshot: " << snapshotName;
 
                    importSceneAsSnapshot(snapshotName,
                                          sceneJsonFile,
                                          longtermMemory,
                                          workingMemory,
                                          objectClassesSegment,
                                          objectInstancesSegment);
 
                    fileCount++;
                }
            }
        }
        ARMARX_IMPORTANT << "Found " << fileCount << " valid *.json files in the scenes directory '"
                         << scenesDirectory << "'";
    }
 
    bool
    ArmarXObjectsToMemory::importSceneAsSnapshot(
        const std::string& snapshotName,
        const std::filesystem::path& jsonFile,
        const LongtermMemoryInterfacePrx& longtermMemory,
        const WorkingMemoryInterfacePrx& workingMemory,
        const PersistentObjectClassSegmentBasePrx& objectClassesSegment,
        const memoryx::ObjectInstanceMemorySegmentBasePrx& objectInstancesSegment) const
    {
        ARMARX_TRACE;
 
        if (!dryRun)
        {
            ARMARX_TRACE;
            workingMemory->clear();
        }
 
        const nlohmann::json j = nlohmann::read_json(jsonFile);
        armarx::objects::Scene scene;
        scene = j;
 
        std::map<std::string, std::unordered_set<std::string>> instancesPerClass;
        try
        {
            for (const auto& object : scene.objects)
            {
                importObjectToWorkingMemory(
                    object, objectClassesSegment, objectInstancesSegment, instancesPerClass);
            }
 
            ARMARX_INFO << "Saving snapshot from working memory to longterm memory: "
                        << "'" << snapshotName << "'";
            if (!dryRun)
            {
                ARMARX_TRACE;
                longtermMemory->saveWorkingMemorySnapshot(snapshotName, workingMemory);
            }
 
            return true;
        }
        catch (...)
        {
            ARMARX_WARNING << armarx::GetHandledExceptionString();
            ARMARX_WARNING << "Skipping scene " << snapshotName;
            return false;
        }
    }
 
    bool
    ArmarXObjectsToMemory::importObjectToWorkingMemory(
        const armarx::objects::SceneObject& object,
        const PersistentObjectClassSegmentBasePrx& objectClassesSegment,
        const ObjectInstanceMemorySegmentBasePrx& objectInstancesSegment,
        std::map<std::string, std::unordered_set<std::string>>& instancesPerClass) const
    {
        ARMARX_TRACE;
 
        const std::string& className = object.className;
        const Eigen::Vector3f& position = object.position;
        const Eigen::Quaternionf& quat = object.orientation;
 
        // Get the object class
        const auto obj = objectClassesSegment->getObjectClassByName(className);
        if (armarx::isNullptr(obj))
        {
            ARMARX_WARNING << "Object class " << object.className << " for object "
                           << object.getObjectID() << " not found in class segment.";
            return false;
        }
 
        memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(obj);
        ARMARX_CHECK_NOT_NULL(objectClass);
 
        // Add object instance
        {
            auto& instancesOfThisClass = instancesPerClass[className];
 
            auto getObjectName = [&]() -> std::string
            {
                // Instance name specified
                if (not object.instanceName.empty())
                {
                    if (instancesOfThisClass.count(object.instanceName) > 0)
                    {
                        ARMARX_WARNING << "Instance name " << object.instanceName
                                       << " already in use.";
                        return "";
                    }
                    return object.instanceName;
                }
 
                // No instance name specified => use template
                for (int i = 0;; i++)
                {
                    std::string objectName = className + "_" + std::to_string(i);
                    if (instancesOfThisClass.count(objectName) == 0)
                    {
                        return objectName;
                    }
                }
 
                return "";
            };
 
            const std::string objectName = getObjectName();
            if (objectName.empty())
            {
                throw armarx::LocalException("No object name could be set!");
            }
 
            instancesOfThisClass.insert(objectName);
 
            memoryx::ObjectInstancePtr newObject = new memoryx::ObjectInstance(objectName);
 
            NameList attributeNames = objectClass->getAttributeNames();
            for (auto it = attributeNames.begin(); it != attributeNames.end(); ++it)
            {
                newObject->putAttribute(objectClass->getAttribute(*it));
            }
 
            newObject->setClass(objectClass->getName(), 1.0f);
            newObject->setExistenceCertainty(1.0f);
 
            armarx::FramedPositionPtr framedPosition =
                new armarx::FramedPosition(position, armarx::GlobalFrame, "");
            newObject->setPosition(framedPosition);
 
            armarx::FramedOrientationPtr newOrient =
                new armarx::FramedOrientation(quat.toRotationMatrix(), armarx::GlobalFrame, "");
            newObject->setOrientation(newOrient);
 
            std::string objectID = objectInstancesSegment->addEntity(newObject);
            newObject->setId(objectID);
 
            ARMARX_INFO << "Added object of class '" << className << "' with ID '" << objectID
                        << "'.";
 
            return true;
        }
    }
 
} // namespace memoryx