SimoxSceneImporter.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::SimoxSceneImporter
* @author     (vahrenkamp at kit dot edu)
* @date       2013
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Scene.h>
#include <VirtualRobot/VirtualRobotException.h>
#define BOOST_NO_SCOPED_ENUMS
#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <filesystem>
#include <fstream>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/VisualizationNode.h>
#include <VirtualRobot/XML/BaseIO.h>
 
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include "SimoxSceneImporter.h"
#include <Inventor/lists/SbStringList.h>
#include <MemoryX/core/GridFileManager.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;
 
using namespace VirtualRobot;
 
namespace memoryx
{
    SimoxSceneImporter::SimoxSceneImporter() :
        TEMPDIR("simoxsceneimport__tmp"), LONGTERM_SNAPSHOT_PREFIX("Snapshot_")
    {
    }
 
    void
    SimoxSceneImporter::onInitComponent()
    {
        usingProxy("WorkingMemory");
        usingProxy("LongtermMemory");
        usingProxy("PriorKnowledge");
        usingProxy("CommonStorage");
    }
 
    void
    SimoxSceneImporter::onConnectComponent()
    {
        ARMARX_INFO << "Starting SimoxSceneImporter";
 
        filesDBName = getProperty<std::string>("FilesDbName").getValue();
        sceneFile = getProperty<std::string>("SceneFile").getValue();
        snapshotName = getProperty<std::string>("SnapshotName").getValue();
 
        dbSerializer = new MongoSerializer(getIceManager()->getCommunicator(), true);
 
        dataBasePrx = getProxy<CommonStorageInterfacePrx>("CommonStorage");
 
        longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
 
        memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
        objectInstancesMemoryPrx =
            memoryPrx
                ->getObjectInstancesSegment(); // Fails if WorkingMemory.UsePriorKnowledge = false
 
        priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
        classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
 
 
        ScenePtr scene = loadSceneFromXML(sceneFile);
 
        if (!scene)
        {
            ARMARX_ERROR << "Error loading file " << sceneFile << " Aborting.";
            return;
        }
 
        memoryPrx->clear();
 
        if (!importScene(scene))
        {
            ARMARX_ERROR << "Error importing " << sceneFile << " Aborting.";
            return;
        }
 
        std::string prefixedSnapshotName = snapshotName;
 
        // check name
        if (!simox::alg::starts_with(prefixedSnapshotName,
                                     SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX))
        {
            prefixedSnapshotName =
                std::string(SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX) + snapshotName;
            ARMARX_WARNING << "Snapshot name must start with <"
                           << SimoxSceneImporter::LONGTERM_SNAPSHOT_PREFIX << ">. Changing to "
                           << prefixedSnapshotName;
        }
 
        if (longtermMemoryPrx->saveWorkingMemorySnapshot(prefixedSnapshotName, memoryPrx))
        {
            ARMARX_INFO << "Snapshot " << prefixedSnapshotName << " saved. Import complete!";
        }
    }
 
    VirtualRobot::ScenePtr
    SimoxSceneImporter::loadSceneFromXML(const std::string& fileName)
    {
        fs::path fpath(fileName);
 
        if (!fs::exists(fpath))
        {
            ARMARX_ERROR << "File " << fileName << " does not exist! Aborting.";
            return ScenePtr();
        }
 
        ScenePtr scene;
 
        try
        {
            scene = SceneIO::loadScene(fileName);
        }
        catch (VirtualRobotException& e)
        {
            ARMARX_ERROR << "Could not load " << fileName << "\n\t" << e.what();
            return ScenePtr();
        }
 
        return scene;
    }
 
    bool
    SimoxSceneImporter::importScene(VirtualRobot::ScenePtr scene)
    {
        if (!scene || !objectInstancesMemoryPrx)
        {
            return false;
        }
 
        // create temp directory
        std::string prefixedSnapshotName =
            std::string(memoryx::PRIOR_COLLECTION_PREFIX) + snapshotName;
        fs::path tmpImportDirectory =
            fs::path(SimoxSceneImporter::TEMPDIR) / fs::path(prefixedSnapshotName);
 
        fs::remove_all(tmpImportDirectory);
        fs::create_directories(tmpImportDirectory);
 
        // copy all scene files into the temp directory
        VirtualRobot::ScenePtr tmpScene = createTempScene(tmpImportDirectory.string(), scene);
 
        if (!tmpScene)
        {
            return false;
        }
 
        // warn about robots in the scene
        // the tmpScene should not have any robots any more
        size_t robotCount = scene->getRobots().size();
 
        if (robotCount > 0)
        {
            ARMARX_WARNING << "Skipping " << robotCount
                           << " robot defintions in scene file. Robots are not processed!";
        }
 
        // process Obstacle and ManipulationObject instances
        std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr> objectInstances;
        // Obstacles
        int objectInstanceCount =
            createObjectInstances<ObstaclePtr>(tmpScene->getObstacles(), objectInstances);
        // ManipulationObjects
        objectInstanceCount += createObjectInstances<ManipulationObjectPtr>(
            tmpScene->getManipulationObjects(), objectInstances);
 
        importObjectsIntoPriorKnowledge(objectInstances);
 
        for (auto instancePair : objectInstances)
        {
            objectInstancesMemoryPrx->addEntity(instancePair.second);
        }
 
        ARMARX_INFO << "Added " << objectInstanceCount << " instances to working memory";
 
        return true;
    }
 
    ScenePtr
    SimoxSceneImporter::createTempScene(const std::string& tmpPath, VirtualRobot::ScenePtr scene)
    {
        if (!scene)
        {
            return ScenePtr();
        }
 
        // store scene string for saving scene file at the end
        std::stringstream ss;
        ss << "<Scene name='" << scene->getName() << "'>\n\n";
 
        fs::path sceneFileComplete = sceneFile;
        std::string scenePath = sceneFileComplete.parent_path().c_str();
        fs::path sceneFileNew = tmpPath / sceneFileComplete.filename();
 
        // Obstacles
        std::vector<ObstaclePtr> obstacles = scene->getObstacles();
        // ManipulationObjects
        std::vector<ManipulationObjectPtr> manipulationObjects = scene->getManipulationObjects();
        obstacles.insert(obstacles.end(), manipulationObjects.begin(), manipulationObjects.end());
 
        // loop over all scene objects
        for (ObstaclePtr obstacle : obstacles)
        {
            std::vector<std::string> absoluteVisualizationFilenames;
 
            // get absolute paths to the required visualization files
            getAbsoluteVisualizationFilenames(obstacle, absoluteVisualizationFilenames);
 
            // create old/new file mapping
            std::map<std::string, std::string> newFilenames;
            copyFilesToTempDir(absoluteVisualizationFilenames, tmpPath, scenePath, newFilenames);
 
            // update visu and col model filenames to new locations
            setNewVisualizationFilenames(obstacle, newFilenames);
 
            // set the name of the original name if it none was present
            std::string origFilename = obstacle->getFilename();
 
            if (origFilename.empty())
            {
                // create new filename
                origFilename += obstacle->getName();
                origFilename += ".xml";
            }
 
            // copy the objects XML file to the temp directory
            fs::path localFile = tmpPath / fs::path(origFilename).filename();
            std::string localTmpFile = localFile.string();
            ARMARX_INFO << "Saving modified ManipulationObject to " << localTmpFile
                        << " (original file: " << scenePath << "/" << origFilename << ")";
 
            fs::path absPath = fs::absolute(tmpPath);
            saveObstacleAsManipulationObject(obstacle, localTmpFile, absPath.string());
 
            // add XML to the stringstream
            ss << "\t<ManipulationObject name='" << obstacle->getName() << "'>\n";
            ss << "\t\t<File>" << localFile.filename().string() << "</File>\n";
            ss << obstacle->getPhysics().toXML(2);
            ss << "\t</ManipulationObject>\n\n";
        }
 
        ss << "</Scene>\n";
 
        // save scene file (not needed within memory since only the manipulation objects are used in workingMemory), just for testing
        ARMARX_LOG << "Saving scenefile from " << sceneFile << " to " << sceneFileNew.c_str();
        std::string sceneString = ss.str();
        BaseIO::writeXMLFile(sceneFileNew.c_str(), sceneString, true);
 
        return SceneIO::loadScene(sceneFileNew.string());
    }
 
    void
    SimoxSceneImporter::getAbsoluteVisualizationFilenames(
        ObstaclePtr obstacle,
        std::vector<std::string>& absoluteFilenames)
    {
        if (obstacle->getVisualization())
        {
            SoNode* visuNode =
                CoinVisualizationFactory::getCoinVisualization(obstacle->getVisualization());
 
            EntityWrappers::SimoxObjectWrapper::GetAllFilenames(
                visuNode, absoluteFilenames, obstacle->getVisualization()->getFilename());
            std::string visuFile = obstacle->getVisualization()->getFilename();
 
            if (!visuFile.empty())
            {
                absoluteFilenames.push_back(visuFile);
            }
        }
 
        if (obstacle->getCollisionModel() && obstacle->getCollisionModel()->getVisualization())
        {
            SoNode* colNode = CoinVisualizationFactory::getCoinVisualization(
                obstacle->getCollisionModel()->getVisualization());
            EntityWrappers::SimoxObjectWrapper::GetAllFilenames(
                colNode,
                absoluteFilenames,
                obstacle->getCollisionModel()->getVisualization()->getFilename());
            std::string visuFile = obstacle->getCollisionModel()->getVisualization()->getFilename();
 
            if (!visuFile.empty())
            {
                absoluteFilenames.push_back(visuFile);
            }
        }
    }
 
    void
    SimoxSceneImporter::copyFilesToTempDir(const std::vector<std::string>& files,
                                           const std::string& tmpPath,
                                           const std::string& origPath,
                                           std::map<std::string, std::string>& newFilenameMapping)
    {
        fs::path tmpDir = tmpPath;
 
        for (size_t i = 0; i < files.size(); i++)
        {
            fs::path origFile = files[i];
            std::string relativePath = fs::absolute(files[i]).string();
            VirtualRobot::BaseIO::makeRelativePath(fs::absolute(origPath).string(), relativePath);
            fs::path tmpFile = tmpDir / fs::path(relativePath);
 
            ARMARX_INFO << "\norigFile:" << origFile.c_str() << "\n -> relPathS:" << relativePath
                        << "\n -> tmpFile:" << tmpFile.c_str();
 
            if (!fs::exists(origFile))
            {
                ARMARX_ERROR << "File does not exist:" << origFile.c_str();
                continue;
            }
 
            if (!fs::is_regular_file(origFile))
            {
                ARMARX_ERROR << "Not a regular file: " << origFile.c_str();
                continue;
            }
 
            if (fs::is_directory(origFile))
            {
                ARMARX_ERROR << "File is a directory:" << origFile.c_str();
                continue;
            }
 
            ARMARX_LOG << "Copying file from " << origFile.c_str() << " to " << tmpFile.c_str();
            fs::create_directories(tmpFile.parent_path());
            //            int r = system(std::string("cp  \"" + origFile.string() + "\"  \"" + tmpFile.string() + "\"").c_str());
            //            if (r != 0)
            //            {
            //                ARMARX_INFO << "Coyping failed";
            //            }
            fs::copy_file(origFile, tmpFile, fs::copy_options::overwrite_existing);
            newFilenameMapping[origFile.string()] = fs::absolute(tmpFile).string(); //relativePath;
        }
    }
 
    void
    SimoxSceneImporter::setNewVisualizationFilenames(
        ObstaclePtr obstacle,
        std::map<std::string, std::string>& newFilenames)
    {
        if (obstacle->getVisualization())
        {
            std::string visuFile = obstacle->getVisualization()->getFilename();
            std::string newVisuFile = newFilenames[visuFile];
 
            if (!newVisuFile.empty())
            {
                obstacle->getVisualization()->setFilename(
                    newVisuFile, obstacle->getVisualization()->usedBoundingBoxVisu());
            }
        }
 
        if (obstacle->getCollisionModel() && obstacle->getCollisionModel()->getVisualization())
        {
            std::string visuFile = obstacle->getCollisionModel()->getVisualization()->getFilename();
            std::string newVisuFile = newFilenames[visuFile];
 
            if (!newVisuFile.empty())
            {
                obstacle->getCollisionModel()->getVisualization()->setFilename(
                    newVisuFile,
                    obstacle->getCollisionModel()->getVisualization()->usedBoundingBoxVisu());
                ARMARX_INFO << "   COLLISION> "
                            << obstacle->getCollisionModel()->getVisualization()->getFilename();
            }
        }
    }
 
    bool
    SimoxSceneImporter::saveObstacleAsManipulationObject(ObstaclePtr object,
                                                         const std::string& xmlFile,
                                                         const std::string& tmpPath)
    {
        THROW_VR_EXCEPTION_IF(!object, "NULL object");
 
        std::string xmlString = object->toXML(tmpPath);
 
        // This fix is needed until Simox uses virtual toXML() method
        ManipulationObject* manip = dynamic_cast<ManipulationObject*>(object.get());
 
        if (manip)
        {
            xmlString = manip->toXML(tmpPath);
        }
 
        // HACK: right now we ware only able to process manipulation objects -> convert it the hard way
        xmlString = simox::alg::replace_all(xmlString, "<Obstacle", "<ManipulationObject");
        xmlString = simox::alg::replace_all(xmlString, "</Obstacle", "</ManipulationObject");
 
        return BaseIO::writeXMLFile(xmlFile, xmlString, true);
    }
 
    template <typename T>
    int
    SimoxSceneImporter::createObjectInstances(
        std::vector<T> obstacles,
        std::map<ObstaclePtr, ObjectInstanceBasePtr>& objectInstances)
    {
        int objectCount = 0;
 
        for (size_t i = 0; i < obstacles.size(); i++)
        {
            T obstacle = obstacles[i];
            ObjectInstanceBasePtr obstacleInstance = createInstance(obstacle);
 
            if (obstacle && obstacleInstance)
            {
                objectInstances[obstacle] = obstacleInstance;
                objectCount++;
            }
        }
 
        return objectCount;
    }
 
    ObjectInstanceBasePtr
    SimoxSceneImporter::createInstance(SceneObjectPtr sceneObject)
    {
        if (!sceneObject)
        {
            return ObjectInstancePtr();
        }
 
        std::string objectName = sceneObject->getName();
        ObjectInstancePtr objectInstance = new ObjectInstance(objectName);
 
        objectInstance->setExistenceCertainty(1.0f);
 
        // assume object class = object name for now
        objectInstance->setClass(objectName, 1.0f);
 
        // position
        Eigen::Vector3f position = sceneObject->getGlobalPose().block(0, 3, 3, 1);
        armarx::FramedPositionBasePtr objectPosition =
            new armarx::FramedPosition(position, armarx::GlobalFrame, "");
        objectInstance->setPosition(objectPosition);
 
        //orientation
        armarx::FramedOrientationBasePtr objectOrientation =
            new armarx::FramedOrientation(sceneObject->getGlobalPose(), armarx::GlobalFrame, "");
        objectInstance->setOrientation(objectOrientation);
 
        return objectInstance;
    }
 
    void
    SimoxSceneImporter::importObjectsIntoPriorKnowledge(
        std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr>& objectInstances)
    {
        GridFileManagerPtr fileManager(new GridFileManager(dataBasePrx));
 
        std::map<VirtualRobot::ObstaclePtr, ObjectInstanceBasePtr>::iterator it;
 
        for (it = objectInstances.begin(); it != objectInstances.end(); it++)
        {
            Obstacle* obstacle = it->first.get();
 
            ManipulationObject* manip = dynamic_cast<ManipulationObject*>(obstacle);
 
            if (manip)
            {
                obstacle = manip;
            }
 
            std::string simoxFilename = obstacle->getFilename();
 
            fs::path visuFName = obstacle->getVisualization()->getFilename();
            fs::path collisionFName =
                obstacle->getCollisionModel()->getVisualization()->getFilename();
 
            if (collisionFName.empty())
            {
                collisionFName = visuFName;
            }
 
            {
                ObjectClassPtr newClass = new ObjectClass();
                newClass->setName(obstacle->getName());
 
                EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
                    newClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
                ARMARX_INFO << "Store file: " << visuFName.string();
                ARMARX_INFO << "simoxFilename: " << simoxFilename;
                ARMARX_INFO << "visuFName.string(): " << visuFName.string();
                ARMARX_INFO << "collisionFName.string(): " << collisionFName.string();
                simoxWrapper->setAndStoreManipulationFile(simoxFilename, filesDBName);
 
                EntityBasePtr ent = classesSegmentPrx->getEntityByName(newClass->getName());
 
                if (ent)
                {
                    ARMARX_IMPORTANT << "Updating existing entity " << ent->getName()
                                     << "(Id: " << ent->getId() << ")";
                    classesSegmentPrx->updateEntity(ent->getId(), ent);
                }
                else
                {
                    classesSegmentPrx->addEntity(newClass);
                }
            }
        }
    }
} // namespace memoryx