LoadObjectsIntoMemoryWidgetController.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    ARCHES::gui-plugins::LoadObjectsIntoMemoryWidgetController
 * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * \date       2018
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "LoadObjectsIntoMemoryWidgetController.h"
 
#include <chrono>
#include <filesystem>
#include <string>
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Visualization/TriMeshUtils.h>
#include <VirtualRobot/Visualization/VisualizationNode.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
 
using namespace armarx;
 
LoadObjectsIntoMemoryWidgetController::LoadObjectsIntoMemoryWidgetController()
{
    widget.setupUi(getWidget());
    connect(widget.pushButtonAddObjects, SIGNAL(clicked()), this, SLOT(addObjects()));
}
 
void
LoadObjectsIntoMemoryWidgetController::onConnectComponent()
{
    priorPrx =
        getProxy<memoryx::PriorKnowledgeInterfacePrx>(dialog->getProxyName("PriorKnowledge"));
    classesSegmentPrx = priorPrx->getObjectClassesSegment();
 
    databasePrx = priorPrx->getCommonStorage();
    fileManager.reset(new memoryx::GridFileManager(databasePrx));
    QMetaObject::invokeMethod(this, "updateDBs", Qt::QueuedConnection);
}
 
QPointer<QDialog>
LoadObjectsIntoMemoryWidgetController::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new SimpleConfigDialog(parent);
        dialog->addProxyFinder<memoryx::PriorKnowledgeInterfacePrx>(
            {"PriorKnowledge", "PriorKnowledge", "PriorKnowledge*"});
    }
    return qobject_cast<SimpleConfigDialog*>(dialog);
}
 
void
LoadObjectsIntoMemoryWidgetController::updateDBs()
{
    const NameList dbNames = databasePrx->getDBNames();
    widget.comboBoxDBs->clear();
    for (const auto& db : dbNames)
    {
        widget.comboBoxDBs->addItem(QString::fromStdString(db));
    }
    ARMARX_INFO << "setting list of DBs to: " << dbNames;
}
 
void
LoadObjectsIntoMemoryWidgetController::addObjects()
{
    namespace fs = std::filesystem;
 
    const std::string dir = widget.lineEditDir->text().toStdString();
    const std::string collection = widget.lineEditCollection->text().toStdString();
    const std::string db = widget.comboBoxDBs->currentText().toStdString();
    const std::string tmpdir =
        "/tmp/LoadObjectsIntoMemoryWidget/" +
        std::to_string(std::chrono::high_resolution_clock::now().time_since_epoch().count()) + "/";
 
    fs::create_directories(tmpdir);
 
    ARMARX_CHECK_EXPRESSION(!dir.empty());
    ARMARX_CHECK_EXPRESSION(fs::exists(dir));
    ARMARX_CHECK_EQUAL(collection.empty(), db.empty());
 
    //set write collection
    if (!collection.empty())
    {
        const std::string fullCollectionName = db + "." + collection;
        ARMARX_INFO << "setting write collection to: " << fullCollectionName;
        memoryx::CollectionInterfacePrx coll = databasePrx->requestCollection(fullCollectionName);
        ARMARX_CHECK_NOT_NULL(coll);
        classesSegmentPrx->setWriteCollection(coll);
        ARMARX_INFO << "setting write collection done";
    }
 
    for (fs::directory_iterator dit{dir}; dit != fs::directory_iterator{}; ++dit)
    {
        const fs::path p = *dit;
        if (!fs::is_regular_file(p))
        {
            ARMARX_INFO << "skipping non file " << p;
            continue;
        }
        const std::string fname = p.filename().string();
        const std::string fpath = p.string();
        const auto endswith = [&](const std::string& suff)
        {
            const auto pos = fname.rfind(suff);
            return pos != std::string::npos && pos == (fname.size() - suff.size());
        };
        std::string className = fname;
 
 
        //trim name
        {
            if (widget.radioButtonWRL->isChecked())
            {
                if (endswith(".wrl"))
                {
                    className = className.substr(0, className.size() - 4);
                }
                else if (endswith(".iv"))
                {
                    className = className.substr(0, className.size() - 3);
                }
                else
                {
                    ARMARX_INFO << "skipping non wrl/iv file " << p;
                    continue;
                }
            }
            else
            {
                if (endswith(".xml"))
                {
                    className = className.substr(0, className.size() - 4);
                }
                else
                {
                    ARMARX_INFO << "skipping non xml file " << p;
                    continue;
                }
            }
            ARMARX_INFO << "processing file " << p;
        }
 
        memoryx::ObjectClassPtr objectClass;
        const bool isNewClass = !classesSegmentPrx->hasEntityByName(className);
        {
            if (!isNewClass)
            {
                if (!widget.checkBoxOverride->isChecked())
                {
                    ARMARX_INFO << "object class " << className << " already exists in memory";
                    continue;
                }
                objectClass = memoryx::ObjectClassPtr::dynamicCast(
                    classesSegmentPrx->getEntityByName(className));
                ARMARX_CHECK_NOT_NULL(objectClass);
            }
            else
            {
                objectClass = new memoryx::ObjectClass();
            }
        }
 
        //wrappers
        objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
 
        //set attributes
        {
            objectClass->setName(className);
            objectClass->clearParentClasses();
            {
                const std::string parentClass = widget.lineEditParentClass->text().toStdString();
                if (!parentClass.empty())
                {
                    objectClass->addParentClass(parentClass);
                }
            }
            objectClass->setInstanceable(true);
        }
 
        //update simox data
        {
            //            VirtualRobot::ManipulationObject
 
            memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
                objectClass->getWrapper<memoryx::EntityWrappers::SimoxObjectWrapper>();
            if (widget.radioButtonWRL->isChecked())
            {
                ARMARX_INFO << "setting wrl as model";
                simoxWrapper->setAndStoreModelIVFiles(fpath, fpath, getFilesDBName());
            }
            else
            {
                ARMARX_INFO << "reading simox xml";
                simoxWrapper->setAndStoreManipulationFile(fpath, getFilesDBName());
            }
            if (widget.checkBoxScale->isChecked() || widget.checkBoxBBDensity->isChecked())
            {
                bool change = false;
                Eigen::Vector3f bbmin;
                Eigen::Vector3f bbmax;
                auto mo = simoxWrapper->getManipulationObject();
                auto col = mo->getCollisionModel();
                ARMARX_CHECK_EXPRESSION(col->getTriMeshModel()->getSize(bbmin, bbmax));
                Eigen::Vector3f bbsz = bbmax - bbmin;
 
                if (widget.checkBoxBBDensity->isChecked())
                {
                    const float vol = bbsz(0) * bbsz(1) * bbsz(2);
                    const float mass = vol * widget.doubleSpinBoxBBDensity->value();
                    if (mo->getMass() == mass)
                    {
                        change = true;
                        mo->setMass(mass);
                    }
                }
                if (widget.checkBoxScale->isChecked())
                {
                    const double sidesz = bbsz(widget.spinBoxScaleDim->value());
                    bool doScale = false;
                    float factor = 1;
                    if (sidesz > widget.doubleSpinBoxScaleMax->value())
                    {
                        change = true;
                        doScale = true;
                        factor = widget.doubleSpinBoxScaleMax->value() / sidesz;
                    }
                    else if (sidesz < widget.doubleSpinBoxScaleMin->value())
                    {
                        change = true;
                        doScale = true;
                        factor = widget.doubleSpinBoxScaleMin->value() / sidesz;
                    }
                    if (doScale)
                    {
                        ARMARX_INFO << "scaling mesh with factor " << factor;
                        const Eigen::Vector3f scalev{factor, factor, factor};
                        auto triNew = col->getTriMeshModel()->clone(scalev);
                        auto moNew = VirtualRobot::ManipulationObject::createFromMesh(triNew);
                        moNew->setMass(mo->getMass());
                        moNew->setInertiaMatrix(mo->getInertiaMatrix());
                        moNew->setCoMLocal(mo->getCoMLocal());
                        const std::string meshName = tmpdir + className + ".wrl";
                        moNew->getVisualization()->setFilename(meshName, false);
                        moNew->getCollisionModel()->getVisualization()->setFilename(meshName,
                                                                                    false);
                        mo = moNew;
                    }
                }
                if (change)
                {
                    mo->setName(className);
                    mo->saveModelFiles(tmpdir, false);
                    const std::string tmpxml = tmpdir + className + ".xml";
                    std::ofstream{tmpxml} << mo->toXML(tmpdir);
 
                    ARMARX_INFO << "reading generated xml";
                    simoxWrapper->setAndStoreManipulationFile(tmpxml, getFilesDBName());
                }
            }
        }
 
        //add or update
        if (!widget.checkBoxDryRun->isChecked())
        {
            if (isNewClass)
            {
                ARMARX_INFO << "adding   " << className;
                const std::string id = classesSegmentPrx->addEntity(objectClass);
                ARMARX_IMPORTANT << "added   class " << objectClass << " with id " << id;
            }
            else
            {
                ARMARX_INFO << "updating " << className;
                const std::string id = objectClass->getId();
                classesSegmentPrx->updateEntity(id, objectClass);
                ARMARX_IMPORTANT << "updated class " << objectClass << " with id " << id;
            }
        }
    }
}