XMLSceneImporter.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::XMLSceneImporter
* @author     (kozlov at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "XMLSceneImporter.h"
 
#include <ArmarXCore/util/json/JSONObject.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
#include <fstream>
#include <filesystem>
 
namespace fs = std::filesystem;
 
namespace memoryx
{
    void XMLSceneImporter::onInitComponent()
    {
        usingProxy("WorkingMemory");
        usingProxy("LongtermMemory");
    }
 
 
    void XMLSceneImporter::onConnectComponent()
    {
        ARMARX_INFO << "Starting XMLSceneImporter";
 
        memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
        objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
 
        longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
 
        const std::string sceneFile = getProperty<std::string>("SceneFile").getValue();
        const std::string snapshotName = getProperty<std::string>("SnapshotName").getValue();
        targetUnit = getProperty<LengthUnit>("TargetLengthUnit").getValue();
 
        importXMLSnapshot(sceneFile);
 
        if (longtermMemoryPrx->saveWorkingMemorySnapshot(snapshotName, memoryPrx))
        {
            ARMARX_INFO << "Snapshot " << snapshotName << " saved. Import complete!";
        }
    }
 
    void XMLSceneImporter::importXMLSnapshot(const std::string& fileName)
    {
        fs::path fpath(fileName);
 
        if (!fs::exists(fpath))
        {
            ARMARX_ERROR << "File " << fileName << " does not exist! Aborting.";
            return;
        }
 
        std::ifstream infile(fpath.c_str());
        std::string XMLString((std::istreambuf_iterator<char>(infile)), std::istreambuf_iterator<char>());
 
        // parse the specified scene file
        rapidxml::xml_document<> XMLDoc;
 
        try
        {
            XMLDoc.parse<0>(const_cast<char*>(XMLString.c_str()));
        }
        catch (rapidxml::parse_error& e)
        {
            ARMARX_ERROR << "Error parsing XML file: " << e.what();
            return;
        }
 
        // check if it contains a toplevel <scene> tag
        rapidxml::xml_node<>* rootNode = XMLDoc.first_node("scene");
 
        if (!rootNode)
        {
            ARMARX_ERROR << "Malformed XML-file: <scene> Tag is missing";
            return;
        }
 
        // the next child tag must be <nodes>
        rapidxml::xml_node<>* sceneNodes = rootNode->first_node("nodes");
 
        if (!sceneNodes)
        {
            ARMARX_ERROR << "Malformed XML-file: <scene> must be followed by a <nodes> tag.";
            return;
        }
 
        // iterate over all <node> tags and import them into the objectInstance memory
        for (rapidxml::xml_node<>* entityNode = sceneNodes->first_node("node"); entityNode; entityNode = entityNode->next_sibling("node"))
        {
            const ObjectInstanceBasePtr newObjectInstance = createObjectInstanceFromXML(entityNode);
 
            if (newObjectInstance)
            {
                objectInstancesMemoryPrx->addEntity(newObjectInstance);
            }
        }
 
        ARMARX_INFO << "File successfully imported: " << fileName;
    }
 
 
    ObjectInstanceBasePtr XMLSceneImporter::createObjectInstanceFromXML(rapidxml::xml_node<>* xmlNode)
    {
        if (!xmlNode)
        {
            return ObjectInstancePtr();
        }
 
        const std::string name(xmlNode->first_attribute("name")->value());
 
        ObjectInstancePtr objectInstance = new ObjectInstance(name);
        objectInstance->setExistenceCertainty(1.0f);
 
        // assume object class = object name for now
        objectInstance->setClass(name, 1.0f);
 
        // load position
        armarx::FramedPositionBasePtr entityPosition = positionFromXML(xmlNode);
 
        if (!entityPosition)
        {
            ARMARX_ERROR << "<position> tag is missing in node " << name;
            return ObjectInstancePtr();
        }
 
        objectInstance->setPosition(entityPosition);
 
        // load rotation
        armarx::FramedOrientationBasePtr entityOrientation = orientationFromXML(xmlNode);
 
        if (entityOrientation)
        {
            objectInstance->setOrientation(entityOrientation);
        }
 
        return objectInstance;
    }
 
    armarx::FramedPositionBasePtr XMLSceneImporter::positionFromXML(rapidxml::xml_node<>* xmlNode)
    {
        rapidxml::xml_node<>* positionNode = xmlNode->first_node("position");
 
        if (!positionNode)
        {
            return armarx::FramedPositionPtr();
        }
 
        Eigen::Vector3f position;
        std::istringstream(positionNode->first_attribute("x")->value()) >> position[0];
        std::istringstream(positionNode->first_attribute("y")->value()) >> position[1];
        std::istringstream(positionNode->first_attribute("z")->value()) >> position[2];
 
        float scaleFactor = scaleFactorFromPositionXML(positionNode);
        position *= scaleFactor;
 
        return new armarx::FramedPosition(position, armarx::GlobalFrame, "");
    }
 
    armarx::FramedOrientationBasePtr XMLSceneImporter::orientationFromXML(rapidxml::xml_node<>* xmlNode)
    {
        rapidxml::xml_node<>* quaternion = xmlNode->first_node("rotation");
 
        if (!quaternion)
        {
            return armarx::FramedOrientationPtr();
        }
 
        Eigen::Quaternionf orientation;
        std::istringstream(quaternion->first_attribute("qx")->value()) >> orientation.x();
        std::istringstream(quaternion->first_attribute("qy")->value()) >> orientation.y();
        std::istringstream(quaternion->first_attribute("qz")->value()) >> orientation.z();
        std::istringstream(quaternion->first_attribute("qw")->value()) >> orientation.w();
 
        return new armarx::FramedOrientation(orientation.toRotationMatrix(), armarx::GlobalFrame, "");
    }
 
    float XMLSceneImporter::scaleFactorFromPositionXML(rapidxml::xml_node<>* positionNode)
    {
        std::string unitstring = "METER";  // assume meter if no <unit> tag is available
 
        rapidxml::xml_attribute<char>* unitAttribute = positionNode->first_attribute("unit");
 
        if (unitAttribute)
        {
            unitstring = std::string(unitAttribute->value());
        }
 
        float sourceFactor;
 
        if (unitstring == "METER")
        {
            sourceFactor = 1.f;
        }
 
        if (unitstring == "CENTIMETER")
        {
            sourceFactor = 100.f;
        }
        else if (unitstring == "MILLIMETER")
        {
            sourceFactor = 1000.f;
        }
        else
        {
            std::stringstream strstr;
            strstr << __FILE__ << " : " << __LINE__ << " (" << __FUNCTION__ << "): unknown unitstring '" << unitstring << "'";
            throw std::invalid_argument {strstr.str()};
        }
 
        float targetFactor;
 
        switch (targetUnit)
        {
            case eMETER:
                targetFactor = 1.f;
                break;
 
            case eCENTIMETER:
                targetFactor = 100.f;
                break;
 
            case eMILLIMETER:
                targetFactor = 1000.f;
                break;
            default:
                std::stringstream strstr;
                strstr << __FILE__ << " : " << __LINE__ << " (" << __FUNCTION__ << "): unknown targetUnit '" << targetUnit << "'";
                throw std::invalid_argument {strstr.str()};
        }
 
        return targetFactor / sourceFactor;
    }
 
}